Constant Velocity Constraint between NavStates

gtsam/navigation/ConstantVelocityFactor.h

@@ -0,0 +1,53 @@
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+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{};
+
+    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 {
+        // so we can reuse this calculation below
+        auto evaluateErrorInner = [dt = dt_](const NavState &x1, const NavState &x2) -> gtsam::Vector {
+            const Velocity3 &v1 = x1.v();
+            const Velocity3 &v2 = x2.v();
+            const Point3 &p1 = x1.t();
+            const Point3 &p2 = x2.t();
+
+            // trapezoidal integration constant accelleration
+            // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0);
+
+            // euler start
+            // const Point3 hx = p1 + Point3(v1 * dt);
+
+            // euler end
+            const Point3 hx = p1 + Point3(v2 * dt);
+
+            return p2 - hx;
+        };
+
+        if (H1) {
+            (*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
+        }
+        if (H2) {
+            (*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
+        }
+
+        return evaluateErrorInner(x1, x2);
+    }
+};
+
+}  // namespace gtsam

gtsam/navigation/tests/testConstantVelocityFactor.cpp

@@ -0,0 +1,74 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  Alex Cunningham
+ * @author  Asa Hammond
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/ConstantVelocityFactor.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/bind.hpp>
+#include <list>
+
+/* ************************************************************************* */
+TEST(ConstantVelocityFactor, VelocityFactor) {
+    using namespace gtsam;
+
+    const auto tol = double{1e-5};
+
+    const auto x1 = Key{1};
+    const auto x2 = Key{2};
+
+    const auto dt = double{1.0};
+    const auto mu = double{1000};
+
+    // 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 noise_model = noiseModel::Constrained::All(3, mu);
+
+    const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model);
+
+    // positions are the same, secondary state has velocity 1.0 ,
+    EXPECT(assert_equal(Unit3{0.0, 0.0, -1.0}.unitVector(), factor.evaluateError(origin, state0), tol));
+
+    // same velocities, different position
+    // second state agrees with initial state + velocity * dt
+    EXPECT(assert_equal(Unit3{0.0, 0.0, 0.0}.unitVector(), factor.evaluateError(state0, state1), tol));
+
+    // both bodies have the same velocity,
+    // but state2.pose() != state0.pose() + state0.velocity * dt
+    // error comes from this pose difference
+    EXPECT(assert_equal(Unit3{0.0, 0.0, 1.0}.unitVector(), factor.evaluateError(state0, state2), tol));
+}
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */