1/**
2 * @file testRelativeElevationFactor.cpp
3 *
4 * @date Aug 17, 2012
5 * @author Alex Cunningham
6 */
7
8#include <CppUnitLite/TestHarness.h>
9
10#include <gtsam_unstable/slam/RelativeElevationFactor.h>
11
12#include <gtsam/base/numericalDerivative.h>
13
14using namespace std::placeholders;
15using namespace gtsam;
16
17SharedNoiseModel model1 = noiseModel::Unit::Create(dim: 1);
18
19const double tol = 1e-5;
20
21const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
22const Pose3 pose2(Rot3::Pitch(t: -M_PI_2), Point3(2.0, 3.0, 4.0));
23const Pose3 pose3(Rot3::RzRyRx(x: 0.1, y: 0.2, z: 0.3), Point3(2.0, 3.0, 4.0));
24const Point3 point1(3.0, 4.0, 2.0);
25const gtsam::Key poseKey = 1, pointKey = 2;
26
27/* ************************************************************************* */
28Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
29 return factor.evaluateError(x, x: p);
30}
31
32/* ************************************************************************* */
33TEST( testRelativeElevationFactor, level_zero_error ) {
34 // Zero error
35 double measured = 2.0;
36 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
37 Matrix actH1, actH2;
38 EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
39 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
40 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
41 args: std::placeholders::_2),
42 x1: pose1, x2: point1, delta: 1e-5);
43 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
44 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
45 args: std::placeholders::_2),
46 x1: pose1, x2: point1, delta: 1e-5);
47 EXPECT(assert_equal(expH1, actH1, tol));
48 EXPECT(assert_equal(expH2, actH2, tol));
49}
50
51/* ************************************************************************* */
52TEST( testRelativeElevationFactor, level_positive ) {
53 // Positive meas
54 double measured = 0.0;
55 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
56 Matrix actH1, actH2;
57 EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
58 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
59 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
60 args: std::placeholders::_2),
61 x1: pose1, x2: point1, delta: 1e-5);
62 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
63 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
64 args: std::placeholders::_2),
65 x1: pose1, x2: point1, delta: 1e-5);
66 EXPECT(assert_equal(expH1, actH1, tol));
67 EXPECT(assert_equal(expH2, actH2, tol));
68}
69
70/* ************************************************************************* */
71TEST( testRelativeElevationFactor, level_negative ) {
72 // Negative meas
73 double measured = -1.0;
74 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
75 Matrix actH1, actH2;
76 EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
77 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
78 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
79 args: std::placeholders::_2),
80 x1: pose1, x2: point1, delta: 1e-5);
81 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
82 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
83 args: std::placeholders::_2),
84 x1: pose1, x2: point1, delta: 1e-5);
85 EXPECT(assert_equal(expH1, actH1, tol));
86 EXPECT(assert_equal(expH2, actH2, tol));
87}
88
89/* ************************************************************************* */
90TEST( testRelativeElevationFactor, rotated_zero_error ) {
91 // Zero error
92 double measured = 2.0;
93 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
94 Matrix actH1, actH2;
95 EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
96 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
97 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
98 args: std::placeholders::_2),
99 x1: pose2, x2: point1, delta: 1e-5);
100 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
101 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
102 args: std::placeholders::_2),
103 x1: pose2, x2: point1, delta: 1e-5);
104 EXPECT(assert_equal(expH1, actH1, tol));
105 EXPECT(assert_equal(expH2, actH2, tol));
106}
107
108/* ************************************************************************* */
109TEST( testRelativeElevationFactor, rotated_positive ) {
110 // Positive meas
111 double measured = 0.0;
112 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
113 Matrix actH1, actH2;
114 EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
115 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
116 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
117 args: std::placeholders::_2),
118 x1: pose2, x2: point1, delta: 1e-5);
119 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
120 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
121 args: std::placeholders::_2),
122 x1: pose2, x2: point1, delta: 1e-5);
123 EXPECT(assert_equal(expH1, actH1, tol));
124 EXPECT(assert_equal(expH2, actH2, tol));
125}
126
127/* ************************************************************************* */
128TEST( testRelativeElevationFactor, rotated_negative1 ) {
129 // Negative meas
130 double measured = -1.0;
131 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
132 Matrix actH1, actH2;
133 EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
134 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
135 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
136 args: std::placeholders::_2),
137 x1: pose2, x2: point1, delta: 1e-5);
138 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
139 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
140 args: std::placeholders::_2),
141 x1: pose2, x2: point1, delta: 1e-5);
142 EXPECT(assert_equal(expH1, actH1, tol));
143 EXPECT(assert_equal(expH2, actH2, tol));
144}
145
146/* ************************************************************************* */
147TEST( testRelativeElevationFactor, rotated_negative2 ) {
148 // Negative meas
149 double measured = -1.0;
150 RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
151 Matrix actH1, actH2;
152 EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2)));
153 Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
154 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
155 args: std::placeholders::_2),
156 x1: pose3, x2: point1, delta: 1e-5);
157 Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
158 h: std::bind(f&: evalFactorError, args&: factor, args: std::placeholders::_1,
159 args: std::placeholders::_2),
160 x1: pose3, x2: point1, delta: 1e-5);
161 EXPECT(assert_equal(expH1, actH1, tol));
162 EXPECT(assert_equal(expH2, actH2, tol));
163}
164
165/* ************************************************************************* */
166int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
167/* ************************************************************************* */
168