1/**
2 * @file testVelocityConstraint
3 * @author Alex Cunningham
4 */
5
6#include <CppUnitLite/TestHarness.h>
7
8#include <gtsam_unstable/dynamics/VelocityConstraint.h>
9
10using namespace gtsam;
11
12const double tol=1e-5;
13
14const Key x1 = 1, x2 = 2;
15const double dt = 1.0;
16
17PoseRTV origin,
18 pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)),
19 pose1a(Point3(0.5, 0.0, 0.0)),
20 pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0));
21
22/* ************************************************************************* */
23TEST( testVelocityConstraint, trapezoidal ) {
24 // hard constraints don't need a noise model
25 VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt);
26
27 // verify error function
28 EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol));
29 EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
30 EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol));
31 EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
32}
33
34/* ************************************************************************* */
35TEST( testEulerVelocityConstraint, euler_start ) {
36 // hard constraints create their own noise model
37 VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt);
38
39 // verify error function
40 EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), tol));
41 EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
42 EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
43 EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
44}
45
46/* ************************************************************************* */
47TEST( testEulerVelocityConstraint, euler_end ) {
48 // hard constraints create their own noise model
49 VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt);
50
51 // verify error function
52 EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), tol));
53 EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
54 EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
55 EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
56}
57
58/* ************************************************************************* */
59int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
60/* ************************************************************************* */
61