Fixed circle example

gtsam/navigation/Scenario.h

@@ -49,22 +49,30 @@ class Scenario {
   }
 
   /// Pose of body in nav frame at time t
-  Pose3 poseAtTime(double t) const { return Pose3::Expmap(twist_ * t); }
+  Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
 
   /// Velocity in nav frame at time t
-  Vector3 velocityAtTime(double t) {
+  Vector3 velocity(double t) {
     const Rot3 nRb = rotAtTime(t);
     return nRb * linearVelocityInBody();
   }
 
   // acceleration in nav frame
-  Vector3 accelerationAtTime(double t) const {
+  Vector3 acceleration(double t) const {
-    const Rot3 nRb = rotAtTime(t);
     const Vector3 centripetalAcceleration =
         angularVelocityInBody().cross(linearVelocityInBody());
+    const Rot3 nRb = rotAtTime(t);
     return nRb * centripetalAcceleration - gravity();
   }
 
+  // acceleration in body frame frame
+  Vector3 accelerationInBody(double t) const {
+    const Vector3 centripetalAcceleration =
+        angularVelocityInBody().cross(linearVelocityInBody());
+    const Rot3 nRb = rotAtTime(t);
+    return centripetalAcceleration - nRb.transpose() * gravity();
+  }
+
  private:
   Vector6 twist_;
   double imuSampleTime_;

gtsam/navigation/tests/testScenario.cpp

@@ -29,7 +29,7 @@ TEST(Scenario, Forward) {
   const double v = 2;  // m/s
   Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
 
-  const Pose3 T15 = forward.poseAtTime(15);
+  const Pose3 T15 = forward.pose(15);
   EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
   EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
 }
@@ -42,7 +42,7 @@ TEST(Scenario, Circle) {
 
   // R = v/omega, so test if circle is of right size
   const double R = v / omega;
-  const Pose3 T15 = circle.poseAtTime(15);
+  const Pose3 T15 = circle.pose(15);
   EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
   EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
 }

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -48,13 +48,19 @@ class ScenarioRunner {
     const double deltaT = scenario_.imuSampleTime();
     const size_t nrSteps = T / deltaT;
     double t = 0;
+    Vector3 v0 = scenario_.velocity(0);
+    Vector3 v = Vector3::Zero();
+    Vector3 p = Vector3::Zero();
     for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
       std::cout << t << ", " << deltaT << ": ";
-      const Vector3 measuredAcc = scenario_.accelerationAtTime(t);
+      p += deltaT * v;
+      v += deltaT * scenario_.acceleration(t);
+      const Vector3 measuredAcc = scenario_.accelerationInBody(t);
       result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-      //      std::cout << result.deltaRij() << std::endl;
+      std::cout << " P:" << result.deltaPij().transpose();
-      std::cout << " a:" << measuredAcc.transpose();
+      std::cout << " p:" << p.transpose();
-      std::cout << " P:" << result.deltaVij().transpose() << std::endl;
+      std::cout << " p0:" << (p + v0 * t).transpose();
+      std::cout << std::endl;
     }
 
     return result;
@@ -65,7 +71,7 @@ class ScenarioRunner {
     // TODO(frank): allow non-standard
     const imuBias::ConstantBias zeroBias;
     const Pose3 pose_i = Pose3::identity();
-    const Vector3 vel_i = scenario_.velocityAtTime(0);
+    const Vector3 vel_i = scenario_.velocity(0);
     const Vector3 omegaCoriolis = Vector3::Zero();
     const bool use2ndOrderCoriolis = true;
     const PoseVelocityBias prediction =
@@ -95,7 +101,7 @@ using namespace gtsam;
 
 static const double degree = M_PI / 180.0;
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
   const double v = 2;  // m/s
   Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
@@ -103,19 +109,19 @@ TEST(ScenarioRunner, Forward) {
   ScenarioRunner runner(forward);
   const double T = 1;  // seconds
   ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
-  EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9));
+  EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9));
 }
 
 /* ************************************************************************* */
 TEST(ScenarioRunner, Circle) {
   // Forward velocity 2m/s, angular velocity 6 degree/sec
   const double v = 2, omega = 6 * degree;
-  Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.1);
+  Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.01);
 
   ScenarioRunner runner(circle);
   const double T = 15;  // seconds
   ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
-  EXPECT(assert_equal(circle.poseAtTime(T), runner.mean(integrated), 1e-9));
+  EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1));
 }
 
 /* ************************************************************************* */