diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h new file mode 100644 index 000000000..ed68ac077 --- /dev/null +++ b/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 +#include + +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_; + + public: + using Base = NoiseModelFactor2; + + public: + ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) + : 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 { + // 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 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, // diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp new file mode 100644 index 000000000..540c27eab --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include + +/* ************************************************************************* */ +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); +} +/* ************************************************************************* */