Fixed both C++ and python examples

cython/gtsam/examples/ImuFactorExample2.py

@@ -31,24 +31,6 @@ def vector3(x, y, z):
     return np.array([x, y, z], dtype=np.float)
 
 
-def create_poses(angular_velocity=np.pi,
-                 delta_t=0.01, radius=30.0):
-    """Create the set of ground-truth poses."""
-    poses = []
-    theta = 0.0
-    up = gtsam.Point3(0, 0, 1)
-    target = gtsam.Point3(0, 0, 0)
-    for i in range(80):
-        position = gtsam.Point3(radius * math.cos(theta),
-                                radius * math.sin(theta), 0.0)
-        camera = gtsam.SimpleCamera.Lookat(
-            position, target, up, gtsam.Cal3_S2())
-        poses.append(camera.pose())
-        theta += delta_t * angular_velocity
-
-    return poses
-
-
 def ISAM2_plot(values, fignum=0):
     """Plot poses."""
     fig = plt.figure(fignum)
@@ -77,8 +59,9 @@ def ISAM2_plot(values, fignum=0):
 # IMU preintegration parameters
 # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
 g = 9.81
-I = np.eye(3)
+n_gravity = vector3(0, 0, -g)
 PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
+I = np.eye(3)
 PARAMS.setAccelerometerCovariance(I * 0.1)
 PARAMS.setGyroscopeCovariance(I * 0.1)
 PARAMS.setIntegrationCovariance(I * 0.1)
@@ -86,16 +69,29 @@ PARAMS.setUse2ndOrderCoriolis(False)
 PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
 
 BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
+DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
+                    gtsam.Point3(0.05, -0.10, 0.20))
 
 
 def IMU_example():
+    """Run iSAM 2 example with IMU factor."""
+
+    # Start with a camera on x-axis looking at origin
+    radius = 30
+    up = gtsam.Point3(0, 0, 1)
+    target = gtsam.Point3(0, 0, 0)
+    position = gtsam.Point3(radius, 0, 0)
+    camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
+    pose_0 = camera.pose()
 
     # Create the set of ground-truth landmarks and poses
     angular_velocity = math.radians(180)  # rad/sec
     delta_t = 1.0/18  # makes for 10 degrees per step
-    radius = 30
+
-    acceleration = radius * angular_velocity**2
+    angular_velocity_vector = vector3(0, -angular_velocity, 0)
-    poses = create_poses(angular_velocity, delta_t, radius)
+    linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
+    scenario = gtsam.ConstantTwistScenario(
+        angular_velocity_vector, linear_velocity_vector, pose_0)
 
     # Create a factor graph
     newgraph = gtsam.NonlinearFactorGraph()
@@ -111,7 +107,7 @@ def IMU_example():
     # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
     noise = gtsam.noiseModel_Diagonal.Sigmas(
         np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
-    newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise))
+    newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors
     biasKey = gtsam.symbol(ord('b'), 0)
@@ -123,22 +119,23 @@ def IMU_example():
     velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
 
     # Calculate with correct initial velocity
-    velocity = vector3(0, angular_velocity * radius, 0)
+    n_velocity = vector3(0, angular_velocity * radius, 0)
-    velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise)
+    velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
     newgraph.push_back(velprior)
-    initialEstimate.insert(V(0), velocity)
+    initialEstimate.insert(V(0), n_velocity)
 
     accum = gtsam.PreintegratedImuMeasurements(PARAMS)
 
     # Simulate poses and imu measurements, adding them to the factor graph
-    for i, pose_i in enumerate(poses):
+    for i in range(80):
-        delta = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
+        t = i * delta_t  # simulation time
-                            gtsam.Point3(0.05, -0.10, 0.20))
         if i == 0:  # First time add two poses
-            initialEstimate.insert(X(0), poses[0].compose(delta))
+            pose_1 = scenario.pose(delta_t)
-            initialEstimate.insert(X(1), poses[1].compose(delta))
+            initialEstimate.insert(X(0), pose_0.compose(DELTA))
+            initialEstimate.insert(X(1), pose_1.compose(DELTA))
         elif i >= 2:  # Add more poses as necessary
-            initialEstimate.insert(X(i), pose_i.compose(delta))
+            pose_i = scenario.pose(t)
+            initialEstimate.insert(X(i), pose_i.compose(DELTA))
 
         if i > 0:
             # Add Bias variables periodically
@@ -150,11 +147,10 @@ def IMU_example():
                 initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
 
             # Predict acceleration and gyro measurements in (actual) body frame
-            nRb = pose_i.rotation().matrix()
+            nRb = scenario.rotation(t).matrix()
             bRn = np.transpose(nRb)
-            measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + \
+            measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
-                vector3(0, 0, acceleration)  # in body
+            measuredOmega = scenario.omega_b(t)
-            measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity))
             accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
 
             # Add Imu Factor
@@ -162,8 +158,8 @@ def IMU_example():
                 X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
             newgraph.add(imufac)
 
-            # insert new velocity
+            # insert new velocity, which is wrong
-            initialEstimate.insert(V(i), velocity)
+            initialEstimate.insert(V(i), n_velocity)
             accum.resetIntegration()
 
         # Incremental solution

examples/ImuFactorExample2.cpp

@@ -3,42 +3,51 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/Scenario.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 
 #include <vector>
 
-#define GTSAM4
-
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
+// Shorthand for velocity and pose variables
-vector<Pose3> createPoses() {
+using symbol_shorthand::V;
-  // Create the set of ground-truth poses
+using symbol_shorthand::X;
-  vector<Pose3> poses;
+
-  double radius = 30.0, theta = 0.0;
+const double kGravity = 9.81;
-  Point3 up(0, 0, 1), target(0, 0, 0);
-  for (size_t i = 0; i < 80; ++i, theta += 2 * M_PI / 8) {
-    Point3 position(radius * cos(theta), radius * sin(theta), 0.0);
-    SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
-    poses.push_back(camera.pose());
-  }
-  return poses;
-}
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-  // Shorthand for velocity and pose variables
+  auto params = PreintegrationParams::MakeSharedU(kGravity);
-  using symbol_shorthand::V;
+  params->setAccelerometerCovariance(I_3x3 * 0.1);
-  using symbol_shorthand::X;
+  params->setGyroscopeCovariance(I_3x3 * 0.1);
+  params->setIntegrationCovariance(I_3x3 * 0.1);
+  params->setUse2ndOrderCoriolis(false);
+  params->setOmegaCoriolis(Vector3(0, 0, 0));
 
-  // Create the set of ground-truth landmarks and poses
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
-  vector<Pose3> poses = createPoses();
+
+  // Start with a camera on x-axis looking at origin
+  double radius = 30;
+  const Point3 up(0, 0, 1), target(0, 0, 0);
+  const Point3 position(radius, 0, 0);
+  const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
+  const auto pose_0 = camera.pose();
+
+  // Now, create a constant-twist scenario that makes the camera orbit the
+  // origin
+  double angular_velocity = M_PI,  // rad/sec
+      delta_t = 1.0 / 18;          // makes for 10 degrees per step
+  Vector3 angular_velocity_vector(0, -angular_velocity, 0);
+  Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
+  auto scenario = ConstantTwistScenario(angular_velocity_vector,
+                                        linear_velocity_vector, pose_0);
 
   // Create a factor graph
-  NonlinearFactorGraph newgraph, totalgraph;
+  NonlinearFactorGraph newgraph;
 
   // Create (incremental) ISAM2 solver
   ISAM2 isam;
@@ -51,8 +60,7 @@ int main(int argc, char* argv[]) {
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  newgraph.push_back(PriorFactor<Pose3>(X(0), poses[0], noise));
+  newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
-  totalgraph.push_back(PriorFactor<Pose3>(X(0), poses[0], noise));
 
   // Add imu priors
   Key biasKey = Symbol('b', 0);
@@ -60,65 +68,29 @@ int main(int argc, char* argv[]) {
   PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
                                                biasnoise);
   newgraph.push_back(biasprior);
-  totalgraph.push_back(biasprior);
   initialEstimate.insert(biasKey, imuBias::ConstantBias());
-  totalEstimate.insert(biasKey, imuBias::ConstantBias());
   auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
 
-  Vector gravity(3), zero(3), velocity(3);
+  Vector n_velocity(3);
-  gravity << 0, 0, -9.8;
+  n_velocity << 0, angular_velocity * radius, 0;
-  zero << 0, 0, 0;
+  PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
-  velocity << 0, 0, 0;
-#ifdef GTSAM4
-  PriorFactor<Vector> velprior(V(0), zero, velnoise);
-#else
-  PriorFactor<LieVector> velprior(V(0), zero, velnoise);
-#endif
   newgraph.push_back(velprior);
-  totalgraph.push_back(velprior);
 
-#ifdef GTSAM4
+  initialEstimate.insert(V(0), n_velocity);
-  initialEstimate.insert(V(0), zero);
-  totalEstimate.insert(V(0), zero);
-#else
-  initialEstimate.insert(V(0), LieVector(zero));
-  totalEstimate.insert(V(0), LieVector(zero));
-#endif
 
-  Matrix3 I;
-  I << 1, 0, 0, 0, 1, 0, 0, 0, 1;
-  Matrix3 accCov = I * 0.1;
-  Matrix3 gyroCov = I * 0.1;
-  Matrix3 intCov = I * 0.1;
-  bool secOrder = false;
-#ifdef GTSAM4
   // IMU preintegrator
-  PreintegratedImuMeasurements accum(PreintegrationParams::MakeSharedD());
+  PreintegratedImuMeasurements accum(params);
-  accum.params()->setAccelerometerCovariance(accCov);
-  accum.params()->setGyroscopeCovariance(gyroCov);
-  accum.params()->setIntegrationCovariance(intCov);
-  accum.params()->setUse2ndOrderCoriolis(secOrder);
-  accum.params()->setOmegaCoriolis(Vector3(0, 0, 0));
-#else
-  ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov,
-                                             gyroCov, intCov, secOrder);
-#endif
 
   // Simulate poses and imu measurements, adding them to the factor graph
-  for (size_t i = 0; i < poses.size(); ++i) {
+  for (size_t i = 0; i < 36; ++i) {
-#ifdef GTSAM4
+    double t = i * delta_t;
-    Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
-#else
-    Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
-#endif
     if (i == 0) {  // First time add two poses
-      initialEstimate.insert(X(0), poses[0].compose(delta));
+      auto pose_1 = scenario.pose(delta_t);
-      initialEstimate.insert(X(1), poses[1].compose(delta));
+      initialEstimate.insert(X(0), pose_0.compose(delta));
-      totalEstimate.insert(X(0), poses[0].compose(delta));
+      initialEstimate.insert(X(1), pose_1.compose(delta));
-      totalEstimate.insert(X(1), poses[1].compose(delta));
     } else if (i >= 2) {  // Add more poses as necessary
-      initialEstimate.insert(X(i), poses[i].compose(delta));
+      auto pose_i = scenario.pose(t);
-      totalEstimate.insert(X(i), poses[i].compose(delta));
+      initialEstimate.insert(X(i), pose_i.compose(delta));
     }
 
     if (i > 0) {
@@ -135,44 +107,30 @@ int main(int argc, char* argv[]) {
         auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
             b1, b2, imuBias::ConstantBias(), cov);
         newgraph.add(f);
-        totalgraph.add(f);
         initialEstimate.insert(biasKey, imuBias::ConstantBias());
-        totalEstimate.insert(biasKey, imuBias::ConstantBias());
       }
-      // Add Imu Factor
+      // Predict acceleration and gyro measurements in (actual) body frame
-      accum.integrateMeasurement(gravity, zero, 0.5);
+      auto measuredAcc = scenario.acceleration_b(t) -
-#ifdef GTSAM4
+                         scenario.rotation(t).transpose() * params->n_gravity;
-      ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
+      auto measuredOmega = scenario.omega_b(t);
-#else
+      accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
-      ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity,
-                       zero);
-#endif
-      newgraph.add(imufac);
-      totalgraph.add(imufac);
 
-      // insert new velocity
+      // Add Imu Factor
-#ifdef GTSAM4
+      ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
-      initialEstimate.insert(V(i), velocity);
+      newgraph.add(imufac);
-      totalEstimate.insert(V(i), velocity);
+
-#else
+      // insert new velocity, which is wrong
-      initialEstimate.insert(V(i), LieVector(velocity));
+      initialEstimate.insert(V(i), n_velocity);
-      totalEstimate.insert(V(i), LieVector(velocity));
-#endif
       accum.resetIntegration();
     }
 
-    // Batch solution
-    ISAM2 isam_full;
-    isam_full.update(totalgraph, totalEstimate);
-    result = isam_full.calculateEstimate();
-
     // Incremental solution
     isam.update(newgraph, initialEstimate);
     result = isam.calculateEstimate();
     newgraph = NonlinearFactorGraph();
     initialEstimate.clear();
   }
-
+  GTSAM_PRINT(result);
   return 0;
 }
 /* ************************************************************************* */