From d3534b2d2b2b73b73192074a9a843a7bea81a06a Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 11:37:04 -0800 Subject: [PATCH] Fixed circle example --- gtsam/navigation/Scenario.h | 16 +++++++++---- gtsam/navigation/tests/testScenario.cpp | 4 ++-- gtsam/navigation/tests/testScenarioRunner.cpp | 24 ++++++++++++------- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index ea9cba6a7..8872d536d 100644 --- a/gtsam/navigation/Scenario.h +++ b/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 { - const Rot3 nRb = rotAtTime(t); + Vector3 acceleration(double t) const { 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_; diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index a4176be12..25c3dfdc8 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/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)); } diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index a00dfe7fa..30d7fbd14 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/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 << " a:" << measuredAcc.transpose(); - std::cout << " P:" << result.deltaVij().transpose() << std::endl; + std::cout << " P:" << result.deltaPij().transpose(); + std::cout << " p:" << p.transpose(); + 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)); } /* ************************************************************************* */