From 8c324ccbf58efd92a7dc774827993edd23a3c6b4 Mon Sep 17 00:00:00 2001
From: Asa Hammond <asahammond@gmail.com>
Date: Tue, 23 Feb 2021 17:15:27 -0800
Subject: [PATCH] Initial implimentation of a Constant Velocity Constraint
 between NavStates

---
 gtsam/navigation/ConstantVelocityFactor.h     | 59 +++++++++++++++
 .../tests/testConstantVelocityFactor.cpp      | 74 +++++++++++++++++++
 2 files changed, 133 insertions(+)
 create mode 100644 gtsam/navigation/ConstantVelocityFactor.h
 create mode 100644 gtsam/navigation/tests/testConstantVelocityFactor.cpp

diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h
new file mode 100644
index 000000000..8105609d1
--- /dev/null
+++ b/gtsam/navigation/ConstantVelocityFactor.h
@@ -0,0 +1,59 @@
+#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>
+
+using gtsam::Key;
+using gtsam::NavState;
+using gtsam::NoiseModelFactor2;
+using gtsam::Point3;
+using gtsam::SharedNoiseModel;
+using gtsam::Velocity3;
+
+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 {
+        if (H1) {
+            (*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(
+                boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
+        }
+        if (H2) {
+            (*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(
+                boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
+        }
+
+        return evaluateError_(x1, x2, dt_);
+    }
+
+   private:
+    static gtsam::Vector evaluateError_(const NavState &x1, const NavState &x2, double dt) {
+        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;
+    }
+};
+
diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp
new file mode 100644
index 000000000..9e3c9f929
--- /dev/null
+++ b/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);
+}
+/* ************************************************************************* */