Compare prediction with actual navState in two scenarios

python/gtsam_examples/ImuFactorExample.py

@@ -20,21 +20,23 @@ X = lambda i: int(gtsam.Symbol('x', i))
 
 class ImuFactorExample(PreintegrationExample):
 
+    def __init__(self):
+        self.velocity = np.array([2, 0, 0])
+        forward_twist = (np.zeros(3), self.velocity)
+        loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
+        super(ImuFactorExample, self).__init__(loop_twist)
+    
     def run(self):
         graph = gtsam.NonlinearFactorGraph()
-        for i in [0, 12]:
+
-            priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
-            graph.push_back(gtsam.PriorFactorPose3(X(i), gtsam.Pose3(), priorNoise))
-            velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
-            graph.push_back(gtsam.PriorFactorVector3(V(i), np.array([2, 0, 0]), velNoise))
-        
         i = 0  # state index
         
         # initialize data structure for pre-integrated IMU measurements 
         pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
         
         # simulate the loop
-        T = self.timeForOneLoop
+        T = 3
+        state = self.scenario.navState(0)
         for k, t in enumerate(np.arange(0, T, self.dt)):
             # get measurements and add them to PIM
             measuredOmega = self.runner.measuredAngularVelocity(t)
@@ -50,25 +52,37 @@ class ImuFactorExample(PreintegrationExample):
             if (k + 1) % 100 == 0:
                 factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim)
                 graph.push_back(factor)
+                H1 = gtsam.OptionalJacobian9()
+                H2 = gtsam.OptionalJacobian96()
+                print(pim)
+                predicted = pim.predict(state, self.actualBias, H1, H2)
                 pim.resetIntegration()
+                state = self.scenario.navState(t + self.dt)
+                print("predicted.{}\nstate.{}".format(predicted, state))
                 i += 1
 
-        graph.print()
+        # add priors on beginning and end
         num_poses = i + 1
+        priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
+        velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+        for i, pose in [(0, self.scenario.pose(0)), (num_poses - 1, self.scenario.pose(T))]:
+            graph.push_back(gtsam.PriorFactorPose3(X(i), pose, priorNoise))
+            graph.push_back(gtsam.PriorFactorVector3(V(i), self.velocity, velNoise))
+        
+#         graph.print("\Graph:\n")
 
         initial = gtsam.Values()
-        initial.insert(BIAS_KEY, gtsam.ConstantBias())
+        initial.insert(BIAS_KEY, self.actualBias)
         for i in range(num_poses):
-            initial.insert(X(i), gtsam.Pose3())
+            initial.insert(X(i), self.scenario.pose(float(i)))
-            initial.insert(V(i), np.zeros(3, np.float))
+            initial.insert(V(i), self.velocity)
         
         # optimize using Levenberg-Marquardt optimization
         params = gtsam.LevenbergMarquardtParams()
         params.setVerbosityLM("SUMMARY")
         optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
         result = optimizer.optimize()
-        # result.print("\Result:\n")
+#         result.print("\Result:\n")
-        print(self.actualBias)
 
         # Plot cameras
         i = 0

python/gtsam_examples/PreintegrationExample.py

@@ -27,23 +27,25 @@ class PreintegrationExample(object):
         params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float)
         return params
 
-    def __init__(self):
+    def __init__(self, twist=None):
+        """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
+        
         # setup interactive plotting
         plt.ion()
 
-        # Setup loop scenario
+            # Setup loop as default scenario
-        # Forward velocity 2m/s
+        if twist is not None:
-        # Pitch up with angular velocity 6 degree/sec (negative in FLU)
+            (W, V) = twist
-        v = 2
+        else:
-        w = math.radians(30)
+            # default = loop with forward velocity 2m/s, while pitching up
-        W = np.array([0, -w, 0])
+            # with angular velocity 30 degree/sec (negative in FLU)
-        V = np.array([v, 0, 0])
+            W = np.array([0, -math.radians(30), 0])
+            V = np.array([2, 0, 0])
+
         self.scenario = gtsam.ConstantTwistScenario(W, V)
         self.dt = 1e-2
 
-        # Calculate time to do 1 loop
+        self.maxDim = 5
-        self.radius = v / w
-        self.timeForOneLoop = 2.0 * math.pi / w
         self.labels = list('xyz')
         self.colors = list('rgb')
 
@@ -93,16 +95,18 @@ class PreintegrationExample(object):
         # plot ground truth pose, as well as prediction from integrated IMU measurements
         actualPose = self.scenario.pose(t)
         plotPose3(POSES_FIG, actualPose, 0.3)
+        t = actualPose.translation()
+        self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
         ax = plt.gca()
-        ax.set_xlim3d(-self.radius, self.radius)
+        ax.set_xlim3d(-self.maxDim, self.maxDim)
-        ax.set_ylim3d(-self.radius, self.radius)
+        ax.set_ylim3d(-self.maxDim, self.maxDim)
-        ax.set_zlim3d(0, self.radius * 2)
+        ax.set_zlim3d(-self.maxDim, self.maxDim)
 
         plt.pause(0.01)
 
     def run(self):
-        # simulate the loop up to the top
+        # simulate the loop
-        T = self.timeForOneLoop
+        T = 12
         for i, t in enumerate(np.arange(0, T, self.dt)):
             measuredOmega = self.runner.measuredAngularVelocity(t)
             measuredAcc = self.runner.measuredSpecificForce(t)

python/handwritten/navigation/ImuFactor.cpp

@@ -24,7 +24,13 @@
 using namespace boost::python;
 using namespace gtsam;
 
+typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9;
+typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96;
+
 void exportImuFactor() {
+  class_<OptionalJacobian9>("OptionalJacobian9", init<>());
+  class_<OptionalJacobian96>("OptionalJacobian96", init<>());
+
   class_<NavState>("NavState", init<>())
       // TODO(frank): overload with jacobians
       //      .def("attitude", &NavState::attitude)
@@ -61,6 +67,7 @@ void exportImuFactor() {
       init<const boost::shared_ptr<PreintegrationParams>&,
            const imuBias::ConstantBias&>())
       .def(repr(self))
+      .def("predict", &PreintegratedImuMeasurements::predict)
       .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration)
       .def("integrateMeasurement",
            &PreintegratedImuMeasurements::integrateMeasurement)