testConstantVelocityFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/geometry/Pose3.h>
20 #include <gtsam/inference/Symbol.h>
22 
24 #include <list>
25 
26 /* ************************************************************************* */
27 TEST(ConstantVelocityFactor, VelocityFactor) {
28  using namespace gtsam;
29 
30  const double tol{1e-5};
31 
32  const Key x1 = Key{1};
33  const Key x2 = Key{2};
34 
35  const double dt{1.0};
36 
37  // moving upward with groundtruth velocity"
38  const auto origin = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 0.0}};
39 
40  const auto state0 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 1.0}};
41 
42  const auto state1 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 1.0}}, Velocity3{0.0, 0.0, 1.0}};
43 
44  const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}};
45 
46  const auto state3 = NavState{Pose3{Rot3::Yaw(M_PI_2), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}};
47 
48  const double mu{1000};
49  const auto noise_model = noiseModel::Constrained::All(9, mu);
50 
51  const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model);
52 
53  // positions are the same, secondary state has velocity 1.0 in z,
54  const auto state0_err_origin = factor.evaluateError(origin, state0);
55  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));
56 
57  // same velocities, different position
58  // second state agrees with initial state + velocity * dt
59  const auto state1_err_state0 = factor.evaluateError(state0, state1);
60  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));
61 
62  // same velocities, same position, different rotations
63  // second state agrees with initial state + velocity * dt
64  // as we assume that omega is 0.0 this is the same as the above case
65  // TODO: this should respect omega and actually fail in this case
66  const auto state3_err_state2 = factor.evaluateError(state0, state1);
67  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));
68 
69  // both bodies have the same velocity,
70  // but state2.pose() does not agree with state0.update()
71  // error comes from this position difference
72  const auto state2_err_state0 = factor.evaluateError(state0, state2);
73  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));
74 }
75 
76 /* ************************************************************************* */
77 int main() {
78  TestResult tr;
79  return TestRegistry::runAllTests(tr);
80 }
81 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
static int runAllTests(TestResult &result)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
double mu
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
static const NavState state1(x1, v1)
TEST(ConstantVelocityFactor, VelocityFactor)
const double dt
Maintain a constant velocity motion model between two NavStates.
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
static const NavState state2(x2, v2)
#define EXPECT(condition)
Definition: Test.h:150
Array< double, 1, 3 > e(1./3., 0.5, 2.)
traits
Definition: chartTesting.h:28
Pose3 x1
Definition: testPose3.cpp:663
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
static shared_ptr All(size_t dim)
Definition: NoiseModel.h:466


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:59