Simplifying bias tests

gtsam/navigation/tests/imuFactorTesting.h

@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    imuFactorTesting.h
+ * @brief   Common testing infrastructure
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/inference/Symbol.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Convenience for named keys
+using symbol_shorthand::X;
+using symbol_shorthand::V;
+using symbol_shorthand::B;
+
+typedef imuBias::ConstantBias Bias;
+static const Vector3 Z_3x1 = Vector3::Zero();
+static const Bias kZeroBiasHat, kZeroBias;
+
+static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
+static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
+
+namespace testing {
+
+struct ImuMeasurement {
+  ImuMeasurement(const Vector3& acc, const Vector3& gyro, double dt)
+      : acc(acc), gyro(gyro), dt(dt) {}
+  const Vector3 acc, gyro;
+  const double dt;
+};
+
+template <typename PIM>
+void integrateMeasurements(const vector<ImuMeasurement>& measurements,
+                           PIM* pim) {
+  for (const auto& m : measurements)
+    pim->integrateMeasurement(m.acc, m.gyro, m.dt);
+}
+
+struct SomeMeasurements : vector<ImuMeasurement> {
+  SomeMeasurements() {
+    reserve(102);
+    const double dt = 0.01, pi100 = M_PI / 100;
+    emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt);
+    emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt);
+    for (int i = 1; i < 100; i++) {
+      emplace_back(Vector3(0.05, 0.09, 0.01),
+                   Vector3(pi100, pi100 * 3, 2 * pi100), dt);
+    }
+  }
+};
+
+}  // namespace testing

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -32,17 +32,7 @@
 #include <boost/bind.hpp>
 #include <list>
 
-using namespace std;
+#include "imuFactorTesting.h"
-using namespace gtsam;
-
-// Convenience for named keys
-using symbol_shorthand::X;
-using symbol_shorthand::V;
-using symbol_shorthand::B;
-
-typedef imuBias::ConstantBias Bias;
-static const Vector3 Z_3x1 = Vector3::Zero();
-static const Bias kZeroBiasHat, kZeroBias;
 
 namespace {
 
@@ -50,34 +40,22 @@ namespace {
 // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
 /* ************************************************************************* */
 PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
+    const imuBias::ConstantBias& bias) {
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
-  PreintegratedCombinedMeasurements result(p, bias);
+  PreintegratedCombinedMeasurements pim(p, bias);
-
+  integrateMeasurements(testing::SomeMeasurements(), &pim);
-  list<Vector3>::const_iterator itAcc = measuredAccs.begin();
+  return pim;
-  list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
-  list<double>::const_iterator itDeltaT = deltaTs.begin();
-  for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
-    result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
-  }
-  return result;
 }
 
 Vector3 evaluatePreintegratedMeasurementsPosition(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
+    const imuBias::ConstantBias& bias) {
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
+  return evaluatePreintegratedMeasurements(bias).deltaPij();
-  return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-      deltaTs).deltaPij();
 }
 
 Vector3 evaluatePreintegratedMeasurementsVelocity(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
+    const imuBias::ConstantBias& bias) {
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
+  return evaluatePreintegratedMeasurements(bias).deltaVij();
-  return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-      deltaTs).deltaVij();
 }
-
 }
 
 /* ************************************************************************* */
@@ -168,37 +146,14 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
 
 /* ************************************************************************* */
 TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
-  // Linearization point
-  imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
-
-  Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
-
-  // Measurements
-  list<Vector3> measuredAccs, measuredOmegas;
-  list<double> deltaTs;
-  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
-  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
-  deltaTs.push_back(0.01);
-  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
-  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
-  deltaTs.push_back(0.01);
-  for (int i = 1; i < 100; i++) {
-    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
-    measuredOmegas.push_back(
-        Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
-    deltaTs.push_back(0.01);
-  }
-
   // Actual preintegrated values
   PreintegratedCombinedMeasurements pim =
-      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
+      evaluatePreintegratedMeasurements(kZeroBiasHat);
-          deltaTs);
 
   // Check derivative of rotation
-  boost::function<Vector3(const Vector3&, const Vector3&)> theta = [=](
+  boost::function<Vector3(const Vector3&, const Vector3&)> theta =
-      const Vector3& a, const Vector3& w) {
+      [=](const Vector3& a, const Vector3& w) {
-    return evaluatePreintegratedMeasurements(
+        return evaluatePreintegratedMeasurements(Bias(a, w)).theta();
-               Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta();
       };
   EXPECT(
       assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
@@ -206,17 +161,15 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
                       pim.delRdelBiasOmega(), 1e-7));
 
   // Compute numerical derivatives
-  Matrix expectedDelPdelBias = numericalDerivative11<Vector,
+  Matrix expectedDelPdelBias =
-      imuBias::ConstantBias>(
+      numericalDerivative11<Vector, imuBias::ConstantBias>(
-      boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
+          evaluatePreintegratedMeasurementsPosition, kZeroBiasHat);
-          measuredOmegas, deltaTs), bias);
   Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
   Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
 
-  Matrix expectedDelVdelBias = numericalDerivative11<Vector,
+  Matrix expectedDelVdelBias =
-      imuBias::ConstantBias>(
+      numericalDerivative11<Vector, imuBias::ConstantBias>(
-      boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
+          &evaluatePreintegratedMeasurementsVelocity, kZeroBiasHat);
-          measuredOmegas, deltaTs), bias);
   Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
 

gtsam/navigation/tests/testImuFactor.cpp

@@ -12,17 +12,18 @@
 /**
  * @file    testImuFactor.cpp
  * @brief   Unit test for ImuFactor
- * @author  Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert
+ * @author  Luca Carlone
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ * @author  Stephen Williams
  */
 
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/linear/Sampler.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 
@@ -31,22 +32,10 @@
 #include <list>
 #include <fstream>
 
-using namespace std;
+#include "imuFactorTesting.h"
-using namespace gtsam;
-
-// Convenience for named keys
-using symbol_shorthand::X;
-using symbol_shorthand::V;
-using symbol_shorthand::B;
-
-typedef imuBias::ConstantBias Bias;
-static const Vector3 Z_3x1 = Vector3::Zero();
-static const Bias kZeroBiasHat, kZeroBias;
 
 static const double kGravity = 10;
 static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
-static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
-static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
 
 /* ************************************************************************* */
 namespace {
@@ -78,31 +67,18 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
 // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
 /* ************************************************************************* */
 PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
-    const Bias& bias, const list<Vector3>& measuredAccs,
+    const Bias& bias) {
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
+  PreintegratedImuMeasurements pim(defaultParams(), bias);
-  PreintegratedImuMeasurements result(defaultParams(), bias);
+  integrateMeasurements(testing::SomeMeasurements(), &pim);
-
+  return pim;
-  list<Vector3>::const_iterator itAcc = measuredAccs.begin();
-  list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
-  list<double>::const_iterator itDeltaT = deltaTs.begin();
-  for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
-    result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
-  }
-  return result;
 }
 
-Vector3 evaluatePreintegratedMeasurementsPosition(
+Vector3 evaluatePreintegratedMeasurementsPosition(const Bias& bias) {
-    const Bias& bias, const list<Vector3>& measuredAccs,
+  return evaluatePreintegratedMeasurements(bias).deltaPij();
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
-  return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-      deltaTs).deltaPij();
 }
 
-Vector3 evaluatePreintegratedMeasurementsVelocity(
+Vector3 evaluatePreintegratedMeasurementsVelocity(const Bias& bias) {
-    const Bias& bias, const list<Vector3>& measuredAccs,
+  return evaluatePreintegratedMeasurements(bias).deltaVij();
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
-  return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-      deltaTs).deltaVij();
 }
 
 Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
@@ -478,32 +454,14 @@ TEST(ImuFactor, fistOrderExponential) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
-  // Measurements
-  list<Vector3> measuredAccs, measuredOmegas;
-  list<double> deltaTs;
-  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
-  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
-  deltaTs.push_back(0.01);
-  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
-  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
-  deltaTs.push_back(0.01);
-  for (int i = 1; i < 100; i++) {
-    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
-    measuredOmegas.push_back(
-        Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
-    deltaTs.push_back(0.01);
-  }
-
   // Actual pre-integrated values
   PreintegratedImuMeasurements preintegrated =
-      evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
+      evaluatePreintegratedMeasurements(kZeroBias);
-                                        deltaTs);
 
   // Check derivative of rotation
-  boost::function<Vector3(const Vector3&, const Vector3&)> theta = [=](
+  boost::function<Vector3(const Vector3&, const Vector3&)> theta =
-      const Vector3& a, const Vector3& w) {
+      [=](const Vector3& a, const Vector3& w) {
-    return evaluatePreintegratedMeasurements(
+        return evaluatePreintegratedMeasurements(Bias(a, w)).theta();
-               Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta();
       };
   EXPECT(
       assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
@@ -512,9 +470,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
 
   // Check derivative of translation
   Matrix expectedDelPdelBias = numericalDerivative11<Vector, Bias>(
-      boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
+      &evaluatePreintegratedMeasurementsPosition, kZeroBias);
-                  measuredOmegas, deltaTs),
-      kZeroBias);
   Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
   Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
   EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
@@ -523,9 +479,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
 
   // Check derivative of velocity
   Matrix expectedDelVdelBias = numericalDerivative11<Vector, Bias>(
-      boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
+      &evaluatePreintegratedMeasurementsVelocity, kZeroBias);
-                  measuredOmegas, deltaTs),
-      kZeroBias);
   Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
   EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));