Merge pull request #701 from asa/asa/ConstantVelocityFactor

Constant Velocity Constraint between NavStates

gtsam/navigation/ConstantVelocityFactor.h

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    ConstantVelocityFactor.h
+ * @brief   Maintain a constant velocity motion model between two NavStates
+ * @author  Asa Hammond
+ */
+
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor for applying a constant velocity model to a moving body represented as a NavState.
+ * The only measurement is dt, the time delta between the states.
+ */
+class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
+    double dt_;
+
+   public:
+    using Base = NoiseModelFactor2<NavState, NavState>;
+
+   public:
+    ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
+        : NoiseModelFactor2<NavState, NavState>(model, i, j), dt_(dt) {}
+    ~ConstantVelocityFactor() override{};
+
+    /**
+     * @brief Caclulate error: (x2 - x1.update(dt)))
+     * where X1 and X1 are NavStates and dt is
+     * the time difference in seconds between the states.
+     * @param x1 NavState for key a
+     * @param x2 NavState for key b
+     * @param H1 optional jacobian in x1
+     * @param H2 optional jacobian in x2
+     * @return * Vector
+     */
+    gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
+                                boost::optional<gtsam::Matrix &> H1 = boost::none,
+                                boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
+        // only used to use update() below
+        static const Vector3 b_accel{0.0, 0.0, 0.0};
+        static const Vector3 b_omega{0.0, 0.0, 0.0};
+
+        Matrix99 predicted_H_x1;
+        NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
+
+        Matrix99 error_H_predicted;
+        Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
+
+        if (H1) {
+            *H1 = error_H_predicted * predicted_H_x1;
+        }
+        return error;
+    }
+};
+
+}  // namespace gtsam

gtsam/navigation/NavState.cpp

@@ -136,12 +136,12 @@ Vector9 NavState::localCoordinates(const NavState& g, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
   Matrix3 D_dR_R, D_dt_R, D_dv_R;
   const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
-  const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
+  const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
-  const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
+  const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
 
   Vector9 xi;
   Matrix3 D_xi_R;
-  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
+  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
   if (H1) {
     *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
     D_dt_R, -I_3x3, Z_3x3, //

gtsam/navigation/tests/testConstantVelocityFactor.cpp

@@ -0,0 +1,81 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testConstantVelocityFactor.cpp
+ * @brief   Unit test for ConstantVelocityFactor
+ * @author  Asa Hammond
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/ConstantVelocityFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <list>
+
+/* ************************************************************************* */
+TEST(ConstantVelocityFactor, VelocityFactor) {
+    using namespace gtsam;
+
+    const double tol{1e-5};
+
+    const Key x1 = Key{1};
+    const Key x2 = Key{2};
+
+    const double dt{1.0};
+
+    // moving upward with groundtruth velocity"
+    const auto origin = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 0.0}};
+
+    const auto state0 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 1.0}};
+
+    const auto state1 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 1.0}}, Velocity3{0.0, 0.0, 1.0}};
+
+    const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}};
+
+    const auto state3 = NavState{Pose3{Rot3::Yaw(M_PI_2), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}};
+
+    const double mu{1000};
+    const auto noise_model = noiseModel::Constrained::All(9, mu);
+
+    const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model);
+
+    // positions are the same, secondary state has velocity 1.0 in z,
+    const auto state0_err_origin = factor.evaluateError(origin, state0);
+    EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(), state0_err_origin, tol));
+
+    // same velocities, different position
+    // second state agrees with initial state + velocity * dt
+    const auto state1_err_state0 = factor.evaluateError(state0, state1);
+    EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state1_err_state0, tol));
+
+    // same velocities, same position, different rotations
+    // second state agrees with initial state + velocity * dt
+    // as we assume that omega is 0.0 this is the same as the above case
+    //  TODO: this should respect omega and actually fail in this case
+    const auto state3_err_state2 = factor.evaluateError(state0, state1);
+    EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state3_err_state2, tol));
+
+    // both bodies have the same velocity,
+    // but state2.pose() does not agree with state0.update()
+    // error comes from this position difference
+    const auto state2_err_state0 = factor.evaluateError(state0, state2);
+    EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0).finished(), state2_err_state0, tol));
+}
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */