Example Bundle Adjustment Graph

This Bundle Adjustment example entirely focuses on graph construction.

Example Code:

• ExampleBundleAdjustmentGraph.java

Concepts:

• Bundle Adjustment
• Multiple Views
• Scene Reconstruction

Related:

• Bundle Adjustment (Optimization)

Example Code

/**
 * Example which shows you how to construct the scene graph and observations that are feed into bundle adjustment.
 *
 * Here we will optimize a synchronized stereo camera system. The baseline is known and the location of camera[0] in
 * each time step is estimated. By synchronized we mean that the two cameras capture images at the exact same time.
 * We will intentionally give it an incorrect set of parameters then see if bundle adjustment will fix it given
 * perfect observations.
 *
 * @author Peter Abeles
 */
public class ExampleBundleAdjustmentGraph {
	public static void main( String[] args ) {
		var rand = new Random(234);
		int numPoints = 100;
		int numMotions = 10;
		int numCameras = 2;
		var intrinsic0 = new CameraPinholeBrown(500, 510, 0, 400, 405, 800, 700);
		var intrinsic1 = new CameraPinholeBrown(300, 299, 0, 500, 400, 1000, 800);
		var view0_to_view1 = eulerXyz(-0.15, 0.05, 0, 0, 0, 0.05, null);

		// Initialize data structures by telling it the number of features, cameras, views, motions
		// Homogenous coordinates will be used since they can handle points at infinity
		var structure = new SceneStructureMetric(/*homogenous*/ true);
		var observations = new SceneObservations();

		// Index of the motion where the stereo baseline is stored
		int baselineIndex = 0;

		// Must call initialize() before all other functions.
		structure.initialize(
				numCameras, /*views*/ numMotions*numCameras, /* motions */numMotions + 1,
				numPoints, /* known rigid objects */0);
		observations.initialize(numMotions*2);

		structure.setCamera(0, true, intrinsic0);
		structure.setCamera(1, true, intrinsic1);

		// Set up the motion from camera[0] to camera[1] that define the stereo pair
		structure.motions.grow(); // motion is the one data structure that isn't predeclared at init()
		structure.motions.get(baselineIndex).known = false;
		structure.motions.get(baselineIndex).parent_to_view.setTo(view0_to_view1);
		structure.motions.get(baselineIndex).parent_to_view.T.y += 0.05; // give it an imperfect estimate

		// A synthetic scene is going to be created to enable us to focus on the main problem
		List<Point3D_F64> cloud = UtilPoint3D_F64.random(new Point3D_F64(0, 0, 3), -1, 1, 100, rand);

		// Add points to bundle adjustment parameters. Here we will add noise to make it more interesting.
		for (int pointIdx = 0; pointIdx < cloud.size(); pointIdx++) {
			// NOTE: All points must be in the global coordinate system
			Point3D_F64 p = cloud.get(pointIdx);
			structure.points.get(pointIdx).set(p.x, p.y, p.z, 1.0);

			// ADDING NOISE IS FOR DEMONSTRATION PURPOSES. DO NOT DO THIS WITH REAL DATA
			for (int i = 0; i < 3; i++) {
				structure.points.get(pointIdx).coordinate[1] += rand.nextGaussian()*0.05;
			}
		}

		System.out.println("Simulating scene:");
		var w2p0 = new WorldToCameraToPixel();
		var w2p1 = new WorldToCameraToPixel();
		var pixel = new Point2D_F64();

		for (int motionIdx = 0; motionIdx < numMotions; motionIdx++) {
			// Two views for every motion. Index of view[0] at this time step
			int viewIdx0 = motionIdx*2;

			// Specify where the views are located
			Se3_F64 worldToView0 = eulerXyz(-1.2 + motionIdx*0.4, 0, 0, rand.nextGaussian()*0.1, 0, 0, null);
			Se3_F64 worldToView1 = worldToView0.concat(view0_to_view1, null);

			// Set up projection from a point in world coordinates to a point in a camera view
			w2p0.configure(intrinsic0, worldToView0);
			w2p1.configure(intrinsic1, worldToView1);

			// Get observations for this view
			// views for the two cameras will be interleaved together
			SceneObservations.View pview0 = observations.getView(viewIdx0);
			SceneObservations.View pview1 = observations.getView(viewIdx0 + 1);

			// camera[0] is easy to configure since it's always relative to the global frame.
			// We will fix view[0] to stop the global coordinate system from randomly floating around.
			// If this was a real problem, typically view[0] is defined as the global coordinate system's origin.
			structure.setView(/* view */ viewIdx0, /* camera */0, /* known */motionIdx == 0, worldToView0, -1);

			// camera[1] is more difficult since multiple views share the same motion, but link to camera[0] view
			SceneStructureMetric.View sview1 = structure.views.get(viewIdx0 + 1);
			sview1.parent = structure.views.get(viewIdx0);
			sview1.camera = 1;
			sview1.parent_to_view = baselineIndex;

			for (int pointIdx = 0; pointIdx < cloud.size(); pointIdx++) {
				Point3D_F64 p = cloud.get(pointIdx);

				// Don't add the observation if it's behind the camera or outside the image
				if (w2p0.transform(p, pixel) && intrinsic0.isInside(pixel.x, pixel.y)) {
					// Save the pixel observations
					pview0.add(/* feature */ pointIdx, (float)pixel.x, (float)pixel.y);
					// Add a connection between this point and the view in the scene graph
					structure.connectPointToView(pointIdx, viewIdx0);
				}

				if (w2p1.transform(p, pixel) && intrinsic1.isInside(pixel.x, pixel.y)) {
					pview1.add(/* feature */ pointIdx, (float)pixel.x, (float)pixel.y);
					structure.connectPointToView(pointIdx, viewIdx0 + 1);
				}
			}
			System.out.printf(" view[%2d] observations=%d\n", viewIdx0, pview0.size());
			System.out.printf(" view[%2d] observations=%d\n", viewIdx0 + 1, pview1.size());
		}

		// Let's optimize everything now and see if it fixes the noise we injected
		BundleAdjustment<SceneStructureMetric> bundleAdjustment = FactoryMultiView.bundleSparseMetric(null);

		// Tell it to print results every iteration. More interesting that way
		bundleAdjustment.setVerbose(System.out, null);
		bundleAdjustment.setParameters(structure, observations);
		bundleAdjustment.configure(1e-12, 1e-12, /* max iterations */ 30);

		// Perform the optimization. This will take a moment. Note you can pass in the same structure for output
		if (!bundleAdjustment.optimize(/* output */structure))
			throw new RuntimeException("Optimization failed");

		// Print and see if it fixed the incorrect baseline that was passed in
		structure.motions.get(baselineIndex).parent_to_view.print();
	}
}