Merge branch 'fix/imuFactor_BodyPSensor' into feature/cleanup_ImuFactor

Conflicts:
	gtsam/navigation/CombinedImuFactor.cpp
	gtsam/navigation/ImuFactor.cpp
	gtsam/navigation/PreintegrationBase.cpp
	gtsam/navigation/PreintegrationBase.h
	gtsam/navigation/tests/testImuFactor.cpp

gtsam/navigation/CombinedImuFactor.cpp

@@ -59,8 +59,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
 
   Vector3 correctedAcc, correctedOmega;
-  correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
+  boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor);
-      &correctedAcc, &correctedOmega);
 
   const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
   Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr

gtsam/navigation/ImuFactor.cpp

@@ -54,8 +54,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
     OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
 
   Vector3 correctedAcc, correctedOmega;
-  correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
+  boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor);
-      &correctedAcc, &correctedOmega);
 
   // rotation increment computed from the current rotation rate measurement
   const Vector3 integratedOmega = correctedOmega * deltaT;

gtsam/navigation/PreintegrationBase.cpp

@@ -102,23 +102,30 @@ void PreintegrationBase::updatePreintegratedJacobians(
   update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
 }
 
-void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
+std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
     const Vector3& measuredAcc, const Vector3& measuredOmega,
-    Vector3* correctedAcc, Vector3* correctedOmega) {
+    boost::optional<const Pose3&> body_P_sensor) const {
-  *correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  // Correct for bias in the sensor frame
-  *correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+  Vector3 s_correctedAcc, s_correctedOmega;
+  s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  s_correctedOmega = biasHat_.correctGyroscope(measuredOmega);
 
-  // Then compensate for sensor-body displacement: we express the quantities
+  // Compensate for sensor-body displacement if needed: we express the quantities
   // (originally in the IMU frame) into the body frame
-  if (p().body_P_sensor) {
+  // Equations below assume the "body" frame is the CG
-    Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix();
+  if (body_P_sensor) {
-    *correctedOmega = body_R_sensor * (*correctedOmega); // rotation rate vector in the body frame
+    Matrix3 bRs = body_P_sensor->rotation().matrix();
-    Matrix3 body_omega_body__cross = skewSymmetric(*correctedOmega);
+    Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame
-    *correctedAcc = body_R_sensor * (*correctedAcc)
+    Matrix3 body_omega_body__cross = skewSymmetric(b_correctedOmega);
-        - body_omega_body__cross * body_omega_body__cross
+    Vector3 b_arm = body_P_sensor->translation().vector();
-            * p().body_P_sensor->translation().vector();
+    Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm
-    // linear acceleration vector in the body frame
+    // Subtract out the the centripetal acceleration from the measured one
-  }
+    // to get linear acceleration vector in the body frame:
+    Vector3 b_correctedAcc = bRs * s_correctedAcc
+        - b_correctedOmega.cross(b_velocity_bs);
+    return std::make_pair(b_correctedAcc, b_correctedOmega);
+  } else
+    return std::make_pair(correctedAcc, s_correctedOmega);
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/PreintegrationBase.h

@@ -177,11 +177,11 @@ public:
   void updatePreintegratedJacobians(const Vector3& correctedAcc,
       const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT);
 
-  void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
+  std::pair<Vector3, Vector3>
-      const Vector3& measuredOmega, Vector3* correctedAcc,
+  correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
-      Vector3* correctedOmega);
+      const Vector3& measuredOmega) const;
 
-  /// Given the estimate of the bias, return a NavState tangent vector
+    /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
   Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
       OptionalJacobian<9, 6> H = boost::none) const;

gtsam/navigation/tests/testImuFactor.cpp

@@ -958,6 +958,141 @@ TEST(ImuFactor, PredictArbitrary) {
   EXPECT(assert_equal(Vector(expectedV), v2, 1e-7));
 }
 
+/* ************************************************************************* */
+TEST(ImuFactor, bodyPSensorNoBias) {
+  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
+
+  // Measurements
+  Vector3 n_gravity(0, 0, -9.81); // z-up nav frame
+  Vector3 omegaCoriolis(0, 0, 0);
+  // Sensor frame is z-down
+  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
+  Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10);
+  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
+  // table exerts an equal and opposite force w.r.t gravity
+  Vector3 s_accMeas(0, 0, -9.81);
+  double dt = 0.001;
+
+  // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
+  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
+
+  ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true);
+
+  for (int i = 0; i < 1000; ++i)
+    pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor);
+
+  // Create factor
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis);
+
+  // Predict
+  Pose3 x1;
+  Vector3 v1(0, 0, 0);
+  PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity,
+      omegaCoriolis);
+
+  Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
+  EXPECT(assert_equal(expectedPose, poseVelocity.pose));
+
+  Vector3 expectedVelocity(0, 0, 0);
+  EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
+}
+
+/* ************************************************************************* */
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+
+TEST(ImuFactor, bodyPSensorWithBias) {
+  using noiseModel::Diagonal;
+  typedef imuBias::ConstantBias Bias;
+
+  int numFactors = 80;
+  Vector6 noiseBetweenBiasSigma;
+  noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
+      3.0e-6, 3.0e-6);
+  SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
+
+  // Measurements
+  Vector3 n_gravity(0, 0, -9.81);
+  Vector3 omegaCoriolis(0, 0, 0);
+
+  // Sensor frame is z-down
+  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
+  Vector3 measuredOmega(0, 0.01, 0);
+  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
+  // table exerts an equal and opposite force w.r.t gravity
+  Vector3 measuredAcc(0, 0, -9.81);
+
+  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
+
+  Matrix3 accCov = 1e-7 * I_3x3;
+  Matrix3 gyroCov = 1e-8 * I_3x3;
+  Matrix3 integrationCov = 1e-9 * I_3x3;
+  double deltaT = 0.005;
+
+  //   Specify noise values on priors
+  Vector6 priorNoisePoseSigmas(
+      (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished());
+  Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished());
+  Vector6 priorNoiseBiasSigmas(
+      (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished());
+  SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
+  SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas);
+  SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
+  Vector3 zeroVel(0, 0, 0);
+
+  // Create a factor graph with priors on initial pose, vlocity and bias
+  NonlinearFactorGraph graph;
+  Values values;
+
+  PriorFactor<Pose3> priorPose(X(0), Pose3(), priorNoisePose);
+  graph.add(priorPose);
+  values.insert(X(0), Pose3());
+
+  PriorFactor<Vector3> priorVel(V(0), zeroVel, priorNoiseVel);
+  graph.add(priorVel);
+  values.insert(V(0), zeroVel);
+
+  // The key to this test is that we specify the bias, in the sensor frame, as known a priori
+  // We also create factors below that encode our assumption that this bias is constant over time
+  // In theory, after optimization, we should recover that same bias estimate
+  Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
+  PriorFactor<Bias> priorBiasFactor(B(0), priorBias, priorNoiseBias);
+  graph.add(priorBiasFactor);
+  values.insert(B(0), priorBias);
+
+  // Now add IMU factors and bias noise models
+  Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
+  for (int i = 1; i < numFactors; i++) {
+    ImuFactor::PreintegratedMeasurements pim =
+        ImuFactor::PreintegratedMeasurements(zeroBias, accCov, gyroCov,
+            integrationCov, true);
+    for (int j = 0; j < 200; ++j)
+      pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
+          body_P_sensor);
+
+    // Create factors
+    graph.add(
+        ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity,
+            omegaCoriolis));
+    graph.add(BetweenFactor<Bias>(B(i - 1), B(i), zeroBias, biasNoiseModel));
+
+    values.insert(X(i), Pose3());
+    values.insert(V(i), zeroVel);
+    values.insert(B(i), priorBias);
+  }
+
+  // Finally, optimize, and get bias at last time step
+  Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
+  Bias biasActual = results.at<Bias>(B(numFactors - 1));
+
+  // And compare it with expected value (our prior)
+  Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
+  EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;