Skeleton compiles/links

gtsam/navigation/ScenarioRunner.cpp

@@ -21,13 +21,26 @@
 
 namespace gtsam {
 
+////////////////////////////////////////////////////////////////////////////////////////////
+
+void PreintegratedMeasurements2::integrateMeasurement(
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {}
+
+NavState PreintegratedMeasurements2::predict(
+    const NavState& state_i, const imuBias::ConstantBias& bias_i,
+    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
+  return NavState();
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+
 static double intNoiseVar = 0.0000001;
 static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
 
-ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
+PreintegratedMeasurements2 ScenarioRunner::integrate(
     double T, const imuBias::ConstantBias& estimatedBias,
     bool corrupted) const {
-  ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias);
+  PreintegratedMeasurements2 pim(p_, estimatedBias);
 
   const double dt = imuSampleTime();
   const size_t nrSteps = T / dt;
@@ -43,7 +56,7 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
 }
 
 NavState ScenarioRunner::predict(
-    const ImuFactor::PreintegratedMeasurements& pim,
+    const PreintegratedMeasurements2& pim,
     const imuBias::ConstantBias& estimatedBias) const {
   const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
   return pim.predict(state_i, estimatedBias);

gtsam/navigation/ScenarioRunner.h

@@ -22,14 +22,41 @@
 
 namespace gtsam {
 
+/**
+ * Class that integrates on the manifold
+ */
+class PreintegratedMeasurements2 {
+ public:
+  typedef ImuFactor::PreintegratedMeasurements::Params Params;
+
+  Matrix9 preintMeasCov() const { return Matrix9::Zero(); }
+
+  PreintegratedMeasurements2(
+      const boost::shared_ptr<Params>& p,
+      const gtsam::imuBias::ConstantBias& estimatedBias) {}
+
+  /**
+   * Add a single IMU measurement to the preintegration.
+   * @param measuredAcc Measured acceleration (in body frame)
+   * @param measuredOmega Measured angular velocity (in body frame)
+   * @param dt Time interval between this and the last IMU measurement
+   */
+  void integrateMeasurement(const Vector3& measuredAcc,
+                            const Vector3& measuredOmega, double dt);
+
+  /// Predict state at time j
+  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
+                   OptionalJacobian<9, 9> H1 = boost::none,
+                   OptionalJacobian<9, 6> H2 = boost::none) const;
+};
+
 /*
  *  Simple class to test navigation scenarios.
  *  Takes a trajectory scenario as input, and can generate IMU measurements
  */
 class ScenarioRunner {
  public:
-  typedef boost::shared_ptr<ImuFactor::PreintegratedMeasurements::Params>
+  typedef boost::shared_ptr<PreintegratedMeasurements2::Params> SharedParams;
-      SharedParams;
   ScenarioRunner(const Scenario* scenario, const SharedParams& p,
                  double imuSampleTime = 1.0 / 100.0,
                  const imuBias::ConstantBias& bias = imuBias::ConstantBias())
@@ -67,19 +94,18 @@ class ScenarioRunner {
   const double& imuSampleTime() const { return imuSampleTime_; }
 
   /// Integrate measurements for T seconds into a PIM
-  ImuFactor::PreintegratedMeasurements integrate(
+  PreintegratedMeasurements2 integrate(
       double T,
       const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
       bool corrupted = false) const;
 
   /// Predict predict given a PIM
-  NavState predict(const ImuFactor::PreintegratedMeasurements& pim,
+  NavState predict(const PreintegratedMeasurements2& pim,
                    const imuBias::ConstantBias& estimatedBias =
                        imuBias::ConstantBias()) const;
 
   /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
-  Matrix6 poseCovariance(
+  Matrix6 poseCovariance(const PreintegratedMeasurements2& pim) const {
-      const ImuFactor::PreintegratedMeasurements& pim) const {
     Matrix9 cov = pim.preintMeasCov();
     Matrix6 poseCov;
     poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3),  //

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -31,7 +31,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
 static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
 
 // Create default parameters with Z-down and above noise parameters
-static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
+static boost::shared_ptr<PreintegratedMeasurements2::Params> defaultParams() {
   auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
   p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
@@ -50,7 +50,7 @@ TEST(ScenarioRunner, Forward) {
   ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
-  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@@ -63,7 +63,7 @@ TEST(ScenarioRunner, ForwardWithBias) {
   ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
-  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
+  auto pim = runner.integrate(T, kNonZeroBias);
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
 }
@@ -77,7 +77,7 @@ TEST(ScenarioRunner, Circle) {
   ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
-  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@@ -94,7 +94,7 @@ TEST(ScenarioRunner, Loop) {
   ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
-  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@@ -125,7 +125,7 @@ TEST(ScenarioRunner, Accelerating) {
   using namespace accelerating;
   ScenarioRunner runner(&scenario, defaultParams(), T / 10);
 
-  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@@ -139,7 +139,7 @@ TEST(ScenarioRunner, Accelerating) {
 //  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
 //                        kNonZeroBias);
 //
-//  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T,
+//  auto pim = runner.integrate(T,
 //  kNonZeroBias);
 //  Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
 //  kNonZeroBias);
@@ -156,7 +156,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
   const double T = 3;  // seconds
   ScenarioRunner runner(&scenario, defaultParams(), T / 10);
 
-  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);