Add VisualISAM2Example. Still need to finish some details of the example

python/gtsam/examples/VisualISAM2Example.py

@@ -0,0 +1,77 @@
+import gtsam
+from gtsam.examples.SFMdata import *
+
+def visual_ISAM2_example():
+    # Define the camera calibration parameters
+    K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Define the camera observation noise model
+    measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
+
+    # Create the set of ground-truth landmarks
+    points = createPoints() # from SFMdata
+
+    # Create the set of ground-truth poses
+    poses = createPoses() # from SFMdata
+
+    # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
+    # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter
+    # structure is available that allows the user to set various properties, such as the relinearization threshold
+    # and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result
+    # will approach the batch result.
+    parameters = gtsam.ISAM2Params()
+    # parameters.relinearizeThreshold = 0.01; # TODO
+    # parameters.relinearizeSkip = 1;         # TODO
+    isam = gtsam.ISAM2(parameters)
+
+    # Create a Factor Graph and Values to hold the new data
+    graph = gtsam.NonlinearFactorGraph()
+    initialEstimate = gtsam.Values()
+
+    #  Loop over the different poses, adding the observations to iSAM incrementally
+    for i, pose in enumerate(poses):
+
+        # Add factors for each landmark observation
+        for j, point in enumerate(points):
+            camera = gtsam.PinholeCameraCal3_S2(pose, K)
+            measurement = camera.project(point)
+            graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(gtsam.Symbol('x', i)), int(gtsam.Symbol('l', j)), K))
+
+        # Add an initial guess for the current pose
+        # Intentionally initialize the variables off from the ground truth
+        initialEstimate.insert(int(gtsam.Symbol('x', i)), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
+
+        # If this is the first iteration, add a prior on the first pose to set the coordinate frame
+        # and a prior on the first landmark to set the scale
+        # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
+        # adding it to iSAM.
+        if( i == 0):
+            # Add a prior on pose x0
+            poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+            graph.push_back(gtsam.PriorFactorPose3(int(gtsam.Symbol('x', 0)), poses[0], poseNoise))
+
+            # Add a prior on landmark l0
+            pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+            graph.push_back(gtsam.PriorFactorPoint3(int(gtsam.Symbol('l', 0)), points[0], pointNoise)) # add directly to graph
+            
+            # Add initial guesses to all observed landmarks
+            # Intentionally initialize the variables off from the ground truth
+            for j, point in enumerate(points):
+                initialEstimate.insert(int(gtsam.Symbol('l', j)), point + gtsam.Point3(-0.25, 0.20, 0.15));
+        else:
+			# Update iSAM with the new factors
+			isam.update(graph, initialEstimate)
+			# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
+			# If accuracy is desired at the expense of time, update(*) can be called additional times
+			# to perform multiple optimizer iterations every step.
+			isam.update()
+			currentEstimate = isam.calculate_estimate();
+			# print "****************************************************"
+			# print "Frame", i, ":"
+			# currentEstimate.print("Current estimate: "); # TODO: Print to screen or plot using matplotlib
+
+			# Clear the factor graph and values for the next iteration
+			graph.resize(0);
+			initialEstimate.clear();
+
+# TODO: Add a __main__ section here

python/gtsam/examples/__init__.py

@@ -1 +1,2 @@
 from SFMdata import *
+from VisualISAM2Example import *