Merge branch 'feature/ImuFactorPush2' into manifold

Conflicts:
	gtsam/navigation/ScenarioRunner.cpp
	gtsam/navigation/ScenarioRunner.h

gtsam/navigation/PreintegrationBase.h

@@ -103,6 +103,7 @@ public:
 protected:
 
   /// Parameters. Declared mutable only for deprecated predict method.
+  /// TODO(frank): make const once deprecated method is removed
 #ifdef ALLOW_DEPRECATED_IN_GTSAM4
   mutable
 #endif

gtsam/navigation/Scenario.h

@@ -33,7 +33,7 @@ class Scenario {
 
   // Derived quantities:
 
-  virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
+  Rot3 rotation(double t) const { return pose(t).rotation(); }
 
   Vector3 velocity_b(double t) const {
     const Rot3 nRb = rotation(t);
@@ -47,20 +47,19 @@ class Scenario {
 };
 
 /// Scenario with constant twist 3D trajectory.
-class ExpmapScenario : public Scenario {
+class ConstantTwistScenario : public Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
-  ExpmapScenario(const Vector3& w, const Vector3& v)
+  ConstantTwistScenario(const Vector3& w, const Vector3& v)
       : twist_((Vector6() << w, v).finished()),
         a_b_(twist_.head<3>().cross(twist_.tail<3>())) {}
 
-  Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
+  Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); }
-  Rot3 rotation(double t) const { return Rot3::Expmap(twist_.head<3>() * t); }
+  Vector3 omega_b(double t) const override { return twist_.head<3>(); }
-  Vector3 omega_b(double t) const { return twist_.head<3>(); }
+  Vector3 velocity_n(double t) const override {
-  Vector3 velocity_n(double t) const {
     return rotation(t).matrix() * twist_.tail<3>();
   }
-  Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
+  Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
 
  private:
   const Vector6 twist_;
@@ -77,12 +76,12 @@ class AcceleratingScenario : public Scenario {
                        const Vector3& omega_b = Vector3::Zero())
       : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
 
-  Pose3 pose(double t) const {
+  Pose3 pose(double t) const override {
     return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
   }
-  Vector3 omega_b(double t) const { return omega_b_; }
+  Vector3 omega_b(double t) const override { return omega_b_; }
-  Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
+  Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
-  Vector3 acceleration_n(double t) const { return a_n_; }
+  Vector3 acceleration_n(double t) const override { return a_n_; }
 
  private:
   const Rot3 nRb_;

gtsam/navigation/ScenarioRunner.cpp

@@ -16,6 +16,8 @@
  */
 
 #include <gtsam/navigation/ScenarioRunner.h>
+#include <gtsam/base/timing.h>
+
 #include <cmath>
 
 using namespace std;
@@ -35,9 +37,10 @@ AggregateImuReadings ScenarioRunner::integrate(double T,
   const size_t nrSteps = T / dt;
   double t = 0;
   for (size_t k = 0; k < nrSteps; k++, t += dt) {
-    Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t);
+    Vector3 measuredOmega =
+        corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
     Vector3 measuredAcc =
-        corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t);
+        corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
     pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
   }
 
@@ -52,6 +55,8 @@ NavState ScenarioRunner::predict(const AggregateImuReadings& pim,
 
 Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
                                            const Bias& estimatedBias) const {
+  gttic_(estimateCovariance);
+
   // Get predict prediction from ground truth measurements
   NavState prediction = predict(integrate(T));
 

gtsam/navigation/ScenarioRunner.h

@@ -57,21 +57,23 @@ class ScenarioRunner {
   const Vector3& gravity_n() const { return p_->n_gravity; }
 
   // A gyro simply measures angular velocity in body frame
-  Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
+  Vector3 actualAngularVelocity(double t) const {
+    return scenario_->omega_b(t);
+  }
 
   // An accelerometer measures acceleration in body, but not gravity
-  Vector3 actual_specific_force_b(double t) const {
+  Vector3 actualSpecificForce(double t) const {
     Rot3 bRn = scenario_->rotation(t).transpose();
     return scenario_->acceleration_b(t) - bRn * gravity_n();
   }
 
   // versions corrupted by bias and noise
-  Vector3 measured_omega_b(double t) const {
+  Vector3 measuredAngularVelocity(double t) const {
-    return actual_omega_b(t) + estimatedBias_.gyroscope() +
+    return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
            gyroSampler_.sample() / sqrt_dt_;
   }
-  Vector3 measured_specific_force_b(double t) const {
+  Vector3 measuredSpecificForce(double t) const {
-    return actual_specific_force_b(t) + estimatedBias_.accelerometer() +
+    return actualSpecificForce(t) + estimatedBias_.accelerometer() +
            accSampler_.sample() / sqrt_dt_;
   }
 

gtsam/navigation/tests/testScenario.cpp

@@ -48,7 +48,7 @@ TEST(Scenario, Spin) {
 TEST(Scenario, Forward) {
   const double v = 2;  // m/s
   const Vector3 W(0, 0, 0), V(v, 0, 0);
-  const ExpmapScenario scenario(W, V);
+  const ConstantTwistScenario scenario(W, V);
 
   const double T = 15;
   EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
@@ -65,7 +65,7 @@ TEST(Scenario, Circle) {
   // Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z
   const double v = 2, w = 6 * kDegree;
   const Vector3 W(0, 0, w), V(v, 0, 0);
-  const ExpmapScenario scenario(W, V);
+  const ConstantTwistScenario scenario(W, V);
 
   const double T = 15;
   EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
@@ -85,7 +85,7 @@ TEST(Scenario, Loop) {
   // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
   const double v = 2, w = 6 * kDegree;
   const Vector3 W(0, -w, 0), V(v, 0, 0);
-  const ExpmapScenario scenario(W, V);
+  const ConstantTwistScenario scenario(W, V);
 
   const double T = 30;
   EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/navigation/ScenarioRunner.h>
+#include <gtsam/base/timing.h>
 #include <CppUnitLite/TestHarness.h>
 #include <cmath>
 
@@ -78,7 +79,7 @@ TEST(ScenarioRunner, Spin) {
 /* ************************************************************************* */
 namespace forward {
 const double v = 2;  // m/s
-ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
+ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
 }
 /* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
@@ -110,7 +111,7 @@ TEST(ScenarioRunner, ForwardWithBias) {
 TEST(ScenarioRunner, Circle) {
   // Forward velocity 2m/s, angular velocity 6 kDegree/sec
   const double v = 2, w = 6 * kDegree;
-  ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
+  ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
 
   ScenarioRunner runner(&scenario, defaultParams(), kDt);
   const double T = 0.1;  // seconds
@@ -128,7 +129,7 @@ TEST(ScenarioRunner, Loop) {
   // Forward velocity 2m/s
   // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
   const double v = 2, w = 6 * kDegree;
-  ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
+  ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
 
   ScenarioRunner runner(&scenario, defaultParams(), kDt);
   const double T = 0.1;  // seconds
@@ -398,6 +399,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
 /* ************************************************************************* */
 int main() {
   TestResult tr;
-  return TestRegistry::runAllTests(tr);
+  auto result = TestRegistry::runAllTests(tr);
+  tictoc_print_();
+  return result;
 }
 /* ************************************************************************* */