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
12/**
13 * @file testProjectionFactor.cpp
14 * @brief Unit tests for ProjectionFactor Class
15 * @author Frank Dellaert
16 * @date Nov 2009
17 */
18
19#include <gtsam_unstable/slam/PoseBetweenFactor.h>
20#include <gtsam/inference/Symbol.h>
21#include <gtsam/geometry/Pose3.h>
22#include <gtsam/base/numericalDerivative.h>
23#include <gtsam/base/TestableAssertions.h>
24
25#include <CppUnitLite/TestHarness.h>
26
27using namespace std::placeholders;
28using namespace std;
29using namespace gtsam;
30
31typedef PoseBetweenFactor<Pose3> TestPoseBetweenFactor;
32
33/// traits
34namespace gtsam {
35template<>
36struct traits<TestPoseBetweenFactor> : public Testable<TestPoseBetweenFactor> {
37};
38}
39
40/* ************************************************************************* */
41TEST( PoseBetweenFactor, Constructor) {
42 Key poseKey1(1);
43 Key poseKey2(2);
44 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
45 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
46 TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
47}
48
49/* ************************************************************************* */
50TEST( PoseBetweenFactor, ConstructorWithTransform) {
51 Key poseKey1(1);
52 Key poseKey2(2);
53 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
54 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
55 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
56 TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
57}
58
59/* ************************************************************************* */
60TEST( PoseBetweenFactor, Equals ) {
61 // Create two identical factors and make sure they're equal
62 Key poseKey1(1);
63 Key poseKey2(2);
64 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
65 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
66 TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model);
67 TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model);
68
69 CHECK(assert_equal(factor1, factor2));
70
71 // Create a third, different factor and check for inequality
72 Pose3 measurement2(Rot3::RzRyRx(x: 0.20, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
73 TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement2, model);
74
75 CHECK(assert_inequal(factor1, factor3));
76}
77
78/* ************************************************************************* */
79TEST( PoseBetweenFactor, EqualsWithTransform ) {
80 // Create two identical factors and make sure they're equal
81 Key poseKey1(1);
82 Key poseKey2(2);
83 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
84 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
85 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
86 TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model, body_P_sensor);
87 TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model, body_P_sensor);
88
89 CHECK(assert_equal(factor1, factor2));
90
91 // Create a third, different factor and check for inequality
92 Pose3 body_P_sensor2(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.30, -0.10, 1.0));
93 TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement, model, body_P_sensor2);
94
95 CHECK(assert_inequal(factor1, factor3));
96}
97
98/* ************************************************************************* */
99TEST( PoseBetweenFactor, Error ) {
100 // Create the measurement and linearization point
101 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: 0.15, z: -0.20), Point3(+0.5, -1.0, +1.0));
102 Pose3 pose1(Rot3::RzRyRx(x: 0.00, y: -0.15, z: 0.30), Point3(-4.0, 7.0, -10.0));
103 Pose3 pose2(Rot3::RzRyRx(x: 0.15, y: 0.00, z: 0.20), Point3(-3.5, 6.0, -9.0));
104
105 // The expected error
106 Vector expectedError(6);
107 // The solution depends on choice of Pose3 and Rot3 Expmap mode!
108#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
109 expectedError << -0.0298135267953815,
110 0.0131341515747393,
111 0.0968868439682154,
112#if defined(GTSAM_POSE3_EXPMAP)
113 -0.145701634472172,
114 -0.134898525569125,
115 -0.0421026389164264;
116#else
117 -0.13918755,
118 -0.142346243,
119 -0.0390885321;
120#endif
121#else
122 expectedError << -0.029839512616488,
123 0.013145599455949,
124 0.096971291682578,
125 -0.139187549519821,
126 -0.142346243063553,
127 -0.039088532100977;
128#endif
129
130 // Create a factor and calculate the error
131 Key poseKey1(1);
132 Key poseKey2(2);
133 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
134 TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
135 Vector actualError(factor.evaluateError(x: pose1, x: pose2));
136
137 // Verify we get the expected error
138 CHECK(assert_equal(expectedError, actualError, 1e-9));
139}
140
141/* ************************************************************************* */
142TEST( PoseBetweenFactor, ErrorWithTransform ) {
143 // Create the measurement and linearization point
144 Pose3 measurement(Rot3::RzRyRx(x: -0.15, y: 0.10, z: 0.15), Point3(+1.25, -0.90, +.45));
145 Pose3 pose1(Rot3::RzRyRx(x: 0.00, y: -0.15, z: 0.30), Point3(-4.0, 7.0, -10.0));
146 Pose3 pose2(Rot3::RzRyRx(x: 0.15, y: 0.00, z: 0.20), Point3(-3.5, 6.0, -9.0));
147 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
148
149 // The expected error
150 Vector expectedError(6);
151 // The solution depends on choice of Pose3 and Rot3 Expmap mode!
152#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
153 expectedError << 0.0173358202010741,
154 0.0222210698409755,
155 -0.0125032003886145,
156#if defined(GTSAM_POSE3_EXPMAP)
157 0.0263800787416566,
158 0.00540285006310398,
159 0.000175859555693563;
160#else
161 0.0264132886,
162 0.0052376953,
163 -7.16127036e-05;
164#endif
165#else
166 expectedError << 0.017337193670445,
167 0.022222830355243,
168 -0.012504190982804,
169 0.026413288603739,
170 0.005237695303536,
171 -0.000071612703633;
172#endif
173
174
175 // Create a factor and calculate the error
176 Key poseKey1(1);
177 Key poseKey2(2);
178 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
179 TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
180 Vector actualError(factor.evaluateError(x: pose1, x: pose2));
181
182 // Verify we get the expected error
183 CHECK(assert_equal(expectedError, actualError, 1e-9));
184}
185
186/* ************************************************************************* */
187TEST( PoseBetweenFactor, Jacobian ) {
188 // Create a factor
189 Key poseKey1(1);
190 Key poseKey2(2);
191 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: 0.15, z: -0.20), Point3(+0.5, -1.0, +1.0));
192 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
193 TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
194
195 // Create a linearization point at the zero-error point
196 Pose3 pose1(Rot3::RzRyRx(x: 0.00, y: -0.15, z: 0.30), Point3(-4.0, 7.0, -10.0));
197 Pose3 pose2(Rot3::RzRyRx(x: 0.179693265735950, y: 0.002945368776519, z: 0.102274823253840),
198 Point3(-3.37493895, 6.14660244, -8.93650986));
199
200 // Calculate numerical derivatives
201 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
202 h: [&factor, &pose2](const Pose3& p) { return factor.evaluateError(x: p, x: pose2); }, x: pose1);
203 Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
204 h: [&factor, &pose1](const Pose3& p) { return factor.evaluateError(x: pose1, x: p); }, x: pose2);
205
206 // Use the factor to calculate the derivative
207 Matrix actualH1;
208 Matrix actualH2;
209 factor.evaluateError(x: pose1, x: pose2, H&: actualH1, H&: actualH2);
210
211 // Verify we get the expected error
212 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
213 CHECK(assert_equal(expectedH2, actualH2, 1e-6));
214}
215
216/* ************************************************************************* */
217TEST( PoseBetweenFactor, JacobianWithTransform ) {
218 // Create a factor
219 Key poseKey1(1);
220 Key poseKey2(2);
221 Pose3 measurement(Rot3::RzRyRx(x: -0.15, y: 0.10, z: 0.15), Point3(+1.25, -0.90, +.45));
222 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
223 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
224 TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
225
226 // Create a linearization point at the zero-error point
227 Pose3 pose1(Rot3::RzRyRx(x: 0.00, y: -0.15, z: 0.30), Point3(-4.0, 7.0, -10.0));
228 Pose3 pose2(Rot3::RzRyRx(x: 0.162672458989103, y: 0.013665177349534, z: 0.224649482928184),
229 Point3(-3.5257579, 6.02637531, -8.98382384));
230
231 // Calculate numerical derivatives
232 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
233 h: [&factor, &pose2](const Pose3& p) { return factor.evaluateError(x: p, x: pose2); }, x: pose1);
234 Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
235 h: [&factor, &pose1](const Pose3& p) { return factor.evaluateError(x: pose1, x: p); }, x: pose2);
236
237 // Use the factor to calculate the derivative
238 Matrix actualH1;
239 Matrix actualH2;
240 Vector error = factor.evaluateError(x: pose1, x: pose2, H&: actualH1, H&: actualH2);
241
242 // Verify we get the expected error
243 CHECK(assert_equal(expectedH1, actualH1, 1e-6));
244 CHECK(assert_equal(expectedH2, actualH2, 1e-5));
245}
246
247/* ************************************************************************* */
248int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
249/* ************************************************************************* */
250
251