Merge pull request #701 from asa/asa/ConstantVelocityFactor
Constant Velocity Constraint between NavStatesrelease/4.3a0
Frank Dellaert
GitHub
commit
a9f9d463bf
|
|
@ -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 <gtsam/navigation/NavState.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
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<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{};
|
||||
|
||||
/**
|
||||
* @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<gtsam::Matrix &> H1 = boost::none,
|
||||
boost::optional<gtsam::Matrix &> 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
|
||||
|
|
@ -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, //
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/ConstantVelocityFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <list>
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue