12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add docstrings, use update() to build predicted state and build the error calc

release/4.3a0
Asa Hammond 2021-02-25 18:09:45 -08:00
parent c0cd024b2a
commit 38ec991731
2 changed files with 57 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -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<NavState, NavState> {
double dt_;
@ -17,36 +21,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
: 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 {
// 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<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
*H1 = error_H_predicted * predicted_H_x1;
}
if (H2) {
(*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
}
return evaluateErrorInner(x1, x2);
return error;
}
};

39
gtsam/navigation/tests/testConstantVelocityFactor.cpp
View File

@ -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));
}
/* ************************************************************************* */