From 8c324ccbf58efd92a7dc774827993edd23a3c6b4 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Tue, 23 Feb 2021 17:15:27 -0800 Subject: [PATCH 01/13] 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 +#include +#include +#include +#include + +using gtsam::Key; +using gtsam::NavState; +using gtsam::NoiseModelFactor2; +using gtsam::Point3; +using gtsam::SharedNoiseModel; +using gtsam::Velocity3; + +class ConstantVelocityFactor : public NoiseModelFactor2 { + double dt_; + + public: + using Base = NoiseModelFactor2; + + public: + ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) + : NoiseModelFactor2(model, i, j), dt_(dt) {} + ~ConstantVelocityFactor() override{}; + + gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + if (H1) { + (*H1) = numericalDerivative21( + boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + } + if (H2) { + (*H2) = numericalDerivative22( + 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 +#include +#include +#include +#include +#include + +#include + +#include +#include + +/* ************************************************************************* */ +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); +} +/* ************************************************************************* */ From 2aa7144b7ea4c67cc55b6997b64477a429684eae Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Tue, 23 Feb 2021 17:32:48 -0800 Subject: [PATCH 02/13] add gtsam namespace --- gtsam/navigation/ConstantVelocityFactor.h | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 8105609d1..2188cc76e 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -4,12 +4,7 @@ #include #include -using gtsam::Key; -using gtsam::NavState; -using gtsam::NoiseModelFactor2; -using gtsam::Point3; -using gtsam::SharedNoiseModel; -using gtsam::Velocity3; +namespace gtsam { class ConstantVelocityFactor : public NoiseModelFactor2 { double dt_; @@ -57,3 +52,4 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { } }; +} // namespace gtsam From c0cd024b2a9ba5a2bebeede6e9e53022637641c2 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Tue, 23 Feb 2021 18:27:20 -0800 Subject: [PATCH 03/13] bind a lambda instead of a static function --- gtsam/navigation/ConstantVelocityFactor.h | 46 +++++++++++------------ 1 file changed, 22 insertions(+), 24 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 2188cc76e..08cd531b6 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -20,35 +20,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, boost::optional H1 = boost::none, boost::optional 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( - boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + (*H1) = numericalDerivative21(evaluateErrorInner, x1, x2, 1e-5); } if (H2) { - (*H2) = numericalDerivative22( - boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + (*H2) = numericalDerivative22(evaluateErrorInner, 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; + return evaluateErrorInner(x1, x2); } }; From 38ec991731136015c0e03cfe0fce1c2d615f7b28 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:09:45 -0800 Subject: [PATCH 04/13] Add docstrings, use update() to build predicted state and build the error calc --- gtsam/navigation/ConstantVelocityFactor.h | 45 ++++++++++--------- .../tests/testConstantVelocityFactor.cpp | 39 +++++++++++++--- 2 files changed, 57 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 08cd531b6..f676108ce 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -6,6 +6,10 @@ 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 { double dt_; @@ -17,36 +21,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { : NoiseModelFactor2(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 H1 = boost::none, boost::optional 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(); + // only used to use update() below + const Vector3 b_accel{0.0, 0.0, 0.0}; + const Vector3 b_omega{0.0, 0.0, 0.0}; - // trapezoidal integration constant accelleration - // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0); + Matrix predicted_H_x1; + NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); - // euler start - // const Point3 hx = p1 + Point3(v1 * dt); - - // euler end - const Point3 hx = p1 + Point3(v2 * dt); - - return p2 - hx; - }; + Matrix error_H_predicted; + Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2); if (H1) { - (*H1) = numericalDerivative21(evaluateErrorInner, x1, x2, 1e-5); + *H1 = error_H_predicted * predicted_H_x1; } - if (H2) { - (*H2) = numericalDerivative22(evaluateErrorInner, x1, x2, 1e-5); - } - - return evaluateErrorInner(x1, x2); + return error; } }; diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 9e3c9f929..cdf05b253 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -53,17 +53,46 @@ TEST(ConstantVelocityFactor, VelocityFactor) { 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)); + // TODO make these tests way less verbose! + // ideally I could find an initializer for Vector9 like: Vector9{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}' + // positions are the same, secondary state has velocity 1.0 in z, + const auto state0_err_origin = factor.evaluateError(origin, state0); + EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).y(), tol)); + EXPECT(assert_equal(1.0, NavState::dV(state0_err_origin).z(), 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)); + const auto state1_err_state0 = factor.evaluateError(state0, state1); + EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).z(), tol)); // both bodies have the same velocity, - // but state2.pose() != state0.pose() + state0.velocity * dt + // but state2.pose() does not agree with .update() // error comes from this pose difference - EXPECT(assert_equal(Unit3{0.0, 0.0, 1.0}.unitVector(), factor.evaluateError(state0, state2), tol)); + const auto state2_err_state0 = factor.evaluateError(state0, state2); + EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).y(), tol)); + EXPECT(assert_equal(1.0, NavState::dP(state2_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).z(), tol)); } /* ************************************************************************* */ From df76b3b51f4fcfbd4498c9e5423a4f2732a8fa4a Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:12:08 -0800 Subject: [PATCH 05/13] remove AAA style for Dellaert style on simpler types --- gtsam/navigation/tests/testConstantVelocityFactor.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index cdf05b253..ca731f404 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -32,13 +32,12 @@ TEST(ConstantVelocityFactor, VelocityFactor) { using namespace gtsam; - const auto tol = double{1e-5}; + const double tol{1e-5}; - const auto x1 = Key{1}; - const auto x2 = Key{2}; + const Key x1 = Key{1}; + const Key x2 = Key{2}; - const auto dt = double{1.0}; - const auto mu = double{1000}; + 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}}; @@ -49,6 +48,7 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto state2 = NavState{Pose3{Rot3::Yaw(0), 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(3, mu); const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); From 34a7ce0d0d263a01b4d2216c10e6909a76d055a6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:35:29 -0800 Subject: [PATCH 06/13] rename some vars for consistency --- gtsam/navigation/NavState.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 0181ea45f..f21a2f660 100644 --- a/gtsam/navigation/NavState.cpp +++ b/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 Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_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); 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, // From d7c565d88533c415d392f908d17f6010f3011a11 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:37:10 -0800 Subject: [PATCH 07/13] static const for placeholder vars --- gtsam/navigation/ConstantVelocityFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index f676108ce..f5a35947f 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -35,8 +35,8 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { // only used to use update() below - const Vector3 b_accel{0.0, 0.0, 0.0}; - const Vector3 b_omega{0.0, 0.0, 0.0}; + static const Vector3 b_accel{0.0, 0.0, 0.0}; + static const Vector3 b_omega{0.0, 0.0, 0.0}; Matrix predicted_H_x1; NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); From 78b92e0fbc675b4c61b48f93e28b60d7cacfe22c Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:38:45 -0800 Subject: [PATCH 08/13] fixup noisemodel so its correct dimension --- gtsam/navigation/tests/testConstantVelocityFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index ca731f404..5bc2599c3 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -49,7 +49,7 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto state2 = NavState{Pose3{Rot3::Yaw(0), 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(3, mu); + const auto noise_model = noiseModel::Constrained::All(9, mu); const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); From aacba70562e2d93ac35ded75207a143c6e2ef5c6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:50:42 -0800 Subject: [PATCH 09/13] cleanup some cruft, add license text --- gtsam/navigation/ConstantVelocityFactor.h | 20 ++++++++++++++++--- .../tests/testConstantVelocityFactor.cpp | 5 ----- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index f5a35947f..028026f7d 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -1,6 +1,20 @@ -#include -#include -#include +/* ---------------------------------------------------------------------------- + + * 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 NonlinearFactor.h + * @brief Non-linear factor base classes + * @author Asa Hammond + */ + #include #include diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 5bc2599c3..5c32fe738 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -12,20 +12,15 @@ /** * @file testConstantVelocityFactor.cpp * @brief Unit test for ConstantVelocityFactor - * @author Alex Cunningham * @author Asa Hammond */ #include -#include #include #include #include -#include #include - -#include #include /* ************************************************************************* */ From 68cdb417060f2e999bc7db330b94573febd4cb75 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:55:19 -0800 Subject: [PATCH 10/13] corrected license header --- gtsam/navigation/ConstantVelocityFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 028026f7d..6cb349869 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearFactor.h - * @brief Non-linear factor base classes + * @file ConstantVelocityFactor.h + * @brief Maintain a constant velocity motion model between two NavStates * @author Asa Hammond */ From f48a2f6273e964ec0dda99f57dcdbd7b179d74a6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Fri, 26 Feb 2021 07:11:21 -0800 Subject: [PATCH 11/13] test cleanup --- .../tests/testConstantVelocityFactor.cpp | 45 ++++++------------- 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 5c32fe738..540c27eab 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -43,51 +43,34 @@ TEST(ConstantVelocityFactor, VelocityFactor) { 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); - // TODO make these tests way less verbose! - // ideally I could find an initializer for Vector9 like: Vector9{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}' // positions are the same, secondary state has velocity 1.0 in z, const auto state0_err_origin = factor.evaluateError(origin, state0); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).y(), tol)); - EXPECT(assert_equal(1.0, NavState::dV(state0_err_origin).z(), tol)); + 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(0.0, NavState::dP(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).z(), tol)); + 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 .update() - // error comes from this pose difference + // 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(0.0, NavState::dP(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).y(), tol)); - EXPECT(assert_equal(1.0, NavState::dP(state2_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).z(), tol)); + 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)); } /* ************************************************************************* */ From b0b5b04ce8d2bad7fbb4f52dd1663374c07eeaa5 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 11 Mar 2021 09:22:40 -0800 Subject: [PATCH 12/13] Avoid derivative calcs if they aren't asked for --- gtsam/navigation/ConstantVelocityFactor.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 6cb349869..0e1401375 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -52,11 +52,20 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { static const Vector3 b_accel{0.0, 0.0, 0.0}; static const Vector3 b_omega{0.0, 0.0, 0.0}; + NavState predicted; Matrix predicted_H_x1; - NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); + + if (H1) + predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); + else + predicted = x1.update(b_accel, b_omega, dt_, boost::none, {}, {}); Matrix error_H_predicted; - Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2); + Vector9 error; + if (H1) + error = predicted.localCoordinates(x2, error_H_predicted, H2); + else + error = predicted.localCoordinates(x2, boost::none, H2); if (H1) { *H1 = error_H_predicted * predicted_H_x1; From 11b196b52843b1a2146a509e648b02a808ddb186 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 11 Mar 2021 11:41:16 -0800 Subject: [PATCH 13/13] Move to fixed size matrix for derivative calculations --- gtsam/navigation/ConstantVelocityFactor.h | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 0e1401375..ed68ac077 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -52,20 +52,11 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { static const Vector3 b_accel{0.0, 0.0, 0.0}; static const Vector3 b_omega{0.0, 0.0, 0.0}; - NavState predicted; - Matrix predicted_H_x1; + Matrix99 predicted_H_x1; + NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {}); - if (H1) - predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); - else - predicted = x1.update(b_accel, b_omega, dt_, boost::none, {}, {}); - - Matrix error_H_predicted; - Vector9 error; - if (H1) - error = predicted.localCoordinates(x2, error_H_predicted, H2); - else - error = predicted.localCoordinates(x2, boost::none, H2); + Matrix99 error_H_predicted; + Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2); if (H1) { *H1 = error_H_predicted * predicted_H_x1;