From 38ec991731136015c0e03cfe0fce1c2d615f7b28 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:09:45 -0800 Subject: [PATCH] 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)); } /* ************************************************************************* */