testConstantVelocityFactor.cpp

Go to the documentation of this file.

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