Add a strong end-to-end test

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -29,9 +29,12 @@
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/BetweenFactor.h>
 
 #include <cmath>
 #include <list>
+#include <memory>
+#include "gtsam/nonlinear/LevenbergMarquardtParams.h"
 
 using namespace std::placeholders;
 using namespace std;
@@ -39,7 +42,7 @@ using namespace gtsam;
 
 // Convenience for named keys
 using symbol_shorthand::B;
-using symbol_shorthand::X;
+using symbol_shorthand::R;
 
 Vector3 kZeroOmegaCoriolis(0, 0, 0);
 
@@ -136,7 +139,7 @@ TEST(AHRSFactor, Error) {
   pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {});
 
   // Check value
   Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
@@ -145,8 +148,8 @@ TEST(AHRSFactor, Error) {
 
   // Check Derivatives
   Values values;
-  values.insert(X(1), Ri);
+  values.insert(R(1), Ri);
-  values.insert(X(2), Rj);
+  values.insert(R(2), Rj);
   values.insert(B(1), bias);
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
 }
@@ -165,7 +168,7 @@ TEST(AHRSFactor, ErrorWithBiases) {
   pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
 
   // Check value
   Vector3 errorExpected(0, 0, 0);
@@ -174,8 +177,8 @@ TEST(AHRSFactor, ErrorWithBiases) {
 
   // Check Derivatives
   Values values;
-  values.insert(X(1), Ri);
+  values.insert(R(1), Ri);
-  values.insert(X(2), Rj);
+  values.insert(R(2), Rj);
   values.insert(B(1), bias);
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
 }
@@ -324,12 +327,12 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov()));
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis);
 
   // Check Derivatives
   Values values;
-  values.insert(X(1), Ri);
+  values.insert(R(1), Ri);
-  values.insert(X(2), Rj);
+  values.insert(R(2), Rj);
   values.insert(B(1), bias);
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
 }
@@ -350,7 +353,7 @@ TEST(AHRSFactor, predictTest) {
   expectedMeasCov = 200 * kMeasuredOmegaCovariance;
   EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
 
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
 
   // Predict
   Rot3 x;
@@ -368,7 +371,6 @@ TEST(AHRSFactor, predictTest) {
   EXPECT(assert_equal(expectedH, H, 1e-8));
 }
 //******************************************************************************
-
 TEST(AHRSFactor, graphTest) {
   // linearization point
   Rot3 Ri(Rot3::RzRyRx(0, 0, 0));
@@ -393,15 +395,87 @@ TEST(AHRSFactor, graphTest) {
   }
 
   // pim.print("Pre integrated measurements");
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
-  values.insert(X(1), Ri);
+  values.insert(R(1), Ri);
-  values.insert(X(2), Rj);
+  values.insert(R(2), Rj);
   values.insert(B(1), bias);
   graph.push_back(factor);
   LevenbergMarquardtOptimizer optimizer(graph, values);
   Values result = optimizer.optimize();
   Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0));
-  EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
+  EXPECT(assert_equal(expectedRot, result.at<Rot3>(R(2))));
+}
+
+/* ************************************************************************* */
+TEST(AHRSFactor, bodyPSensorWithBias) {
+  using noiseModel::Diagonal;
+
+  int numRotations = 10;
+  const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6);
+  SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
+
+  // Measurements in the sensor frame:
+  const double omega = 0.1;
+  const Vector3 realOmega(omega, 0, 0);
+  const Vector3 realBias(1, 2, 3);  // large !
+  const Vector3 measuredOmega = realOmega + realBias;
+
+  auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
+  p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0));
+  p->gyroscopeCovariance = 1e-8 * I_3x3;
+  double deltaT = 0.005;
+
+  // Specify noise values on priors
+  const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001);
+  const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1);
+  SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
+  SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
+
+  // Create a factor graph with priors on initial pose, velocity and bias
+  NonlinearFactorGraph graph;
+  Values values;
+
+  graph.addPrior(R(0), Rot3(), priorNoisePose);
+  values.insert(R(0), Rot3());
+
+  // 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
+  graph.addPrior(B(0), realBias, priorNoiseBias);
+  values.insert(B(0), realBias);
+
+  // Now add IMU factors and bias noise models
+  const Vector3 zeroBias(0, 0, 0);
+  for (int i = 1; i < numRotations; i++) {
+    PreintegratedAhrsMeasurements pim(p, realBias);
+    for (int j = 0; j < 200; ++j)
+      pim.integrateMeasurement(measuredOmega, deltaT);
+
+    // Create factors
+    graph.emplace_shared<AHRSFactor>(R(i - 1), R(i), B(i - 1), pim);
+    graph.emplace_shared<BetweenFactor<Vector3> >(B(i - 1), B(i), zeroBias,
+                                                  biasNoiseModel);
+
+    values.insert(R(i), Rot3());
+    values.insert(B(i), realBias);
+  }
+
+  // Finally, optimize, and get bias at last time step
+  LevenbergMarquardtParams params;
+  // params.setVerbosityLM("SUMMARY");
+  Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
+  const Vector3 biasActual = result.at<Vector3>(B(numRotations - 1));
+
+  // Bias should be a self-fulfilling prophesy:
+  EXPECT(assert_equal(realBias, biasActual, 1e-3));
+
+  // Check that the successive rotations are all `omega` apart:
+  for (int i = 0; i < numRotations; i++) {
+    Rot3 expectedRot = Rot3::Pitch(omega * i);
+    Rot3 actualRot = result.at<Rot3>(R(i));
+    EXPECT(assert_equal(expectedRot, actualRot, 1e-3));
+  }
 }
 
 //******************************************************************************

gtsam/navigation/tests/testImuFactor.cpp

@@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
   const Matrix3 Jr =
       Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
 
-  Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
+  Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
+  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9));
 }
 
 /* ************************************************************************* */
 TEST(ImuFactor, PartialDerivativeLogmap) {
   // Linearization point
-  Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
+  Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias
 
   // Measurements
-  Vector3 deltatheta(0, 0, 0);
+  Vector3 deltaTheta(0, 0, 0);
 
-  auto evaluateLogRotation = [=](const Vector3 deltatheta) {
+  auto evaluateLogRotation = [=](const Vector3 delta) {
     return Rot3::Logmap(
-        Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
+        Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
   };
 
   // Compute numerical derivatives
-  Matrix expectedDelFdeltheta =
+  Matrix expectedDelFdelTheta =
-      numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltatheta);
+      numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
 
-  Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
+  Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
+  EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta));
 }
 
 /* ************************************************************************* */
@@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) {
 
   // change w.r.t. linearization point
   double alpha = 0.0;
-  Vector3 deltabiasOmega;
+  Vector3 deltaBiasOmega;
-  deltabiasOmega << alpha, alpha, alpha;
+  deltaBiasOmega << alpha, alpha, alpha;
 
   const Matrix3 Jr = Rot3::ExpmapDerivative(
       (measuredOmega - biasOmega) * deltaT);
@@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) {
   Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
 
   const Matrix expectedRot = Rot3::Expmap(
-      (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
+      (measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix();
 
   const Matrix3 hatRot =
       Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
   const Matrix3 actualRot = hatRot
-      * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
+      * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
-  // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
 
   // This is a first order expansion so the equality is only an approximation
   EXPECT(assert_equal(expectedRot, actualRot));
@@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   using noiseModel::Diagonal;
   typedef Bias Bias;
 
-  int numFactors = 10;
+  int numPoses = 10;
   Vector6 noiseBetweenBiasSigma;
   noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
       3.0e-6, 3.0e-6);
@@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
   Vector3 zeroVel(0, 0, 0);
 
-  // Create a factor graph with priors on initial pose, vlocity and bias
+  // Create a factor graph with priors on initial pose, velocity and bias
   NonlinearFactorGraph graph;
   Values values;
 
@@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
 
   // 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++) {
+  for (int i = 1; i < numPoses; i++) {
-    PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
+    PreintegratedImuMeasurements pim(p, priorBias);
-        priorBias);
     for (int j = 0; j < 200; ++j)
       pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
@@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   }
 
   // Finally, optimize, and get bias at last time step
-  Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
+  Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
-  Bias biasActual = results.at<Bias>(B(numFactors - 1));
+  Bias biasActual = result.at<Bias>(B(numPoses - 1));
 
   // And compare it with expected value (our prior)
   Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
@@ -851,11 +849,11 @@ struct ImuFactorMergeTest {
                         actual_pim02.preintegrated(), tol));
     EXPECT(assert_equal(pim02_expected, actual_pim02, tol));
 
-    ImuFactor::shared_ptr factor01 =
+    auto factor01 =
         std::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
-    ImuFactor::shared_ptr factor12 =
+    auto factor12 =
         std::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
-    ImuFactor::shared_ptr factor02_expected = std::make_shared<ImuFactor>(
+    auto factor02_expected = std::make_shared<ImuFactor>(
         X(0), V(0), X(2), V(2), B(0), pim02_expected);
 
     ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);