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/PosePriorFactor.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 PosePriorFactor<Pose3> TestPosePriorFactor;
32
33/// traits
34namespace gtsam {
35template<>
36struct traits<TestPosePriorFactor> : public Testable<TestPosePriorFactor> {
37};
38}
39
40/* ************************************************************************* */
41TEST( PosePriorFactor, Constructor) {
42 Key poseKey(1);
43 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
44 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
45 TestPosePriorFactor factor(poseKey, measurement, model);
46}
47
48/* ************************************************************************* */
49TEST( PosePriorFactor, ConstructorWithTransform) {
50 Key poseKey(1);
51 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
52 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
53 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
54 TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
55}
56
57/* ************************************************************************* */
58TEST( PosePriorFactor, Equals ) {
59 // Create two identical factors and make sure they're equal
60 Key poseKey(1);
61 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
62 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
63 TestPosePriorFactor factor1(poseKey, measurement, model);
64 TestPosePriorFactor factor2(poseKey, measurement, model);
65
66 CHECK(assert_equal(factor1, factor2));
67
68 // Create a third, different factor and check for inequality
69 Pose3 measurement2(Rot3::RzRyRx(x: 0.20, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
70 TestPosePriorFactor factor3(poseKey, measurement2, model);
71
72 CHECK(assert_inequal(factor1, factor3));
73}
74
75/* ************************************************************************* */
76TEST( PosePriorFactor, EqualsWithTransform ) {
77 // Create two identical factors and make sure they're equal
78 Key poseKey(1);
79 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
80 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
81 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
82 TestPosePriorFactor factor1(poseKey, measurement, model, body_P_sensor);
83 TestPosePriorFactor factor2(poseKey, measurement, model, body_P_sensor);
84
85 CHECK(assert_equal(factor1, factor2));
86
87 // Create a third, different factor and check for inequality
88 Pose3 body_P_sensor2(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.30, -0.10, 1.0));
89 TestPosePriorFactor factor3(poseKey, measurement, model, body_P_sensor2);
90
91 CHECK(assert_inequal(factor1, factor3));
92}
93
94/* ************************************************************************* */
95TEST( PosePriorFactor, Error ) {
96 // Create the measurement and linearization point
97 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
98 Pose3 pose(Rot3::RzRyRx(x: 0.0, y: -0.15, z: 0.30), Point3(-4.0, 7.0, -10.0));
99
100 // The expected error
101 Vector expectedError(6);
102 // The solution depends on choice of Pose3 and Rot3 Expmap mode!
103#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
104 expectedError << -0.182948257976108,
105 0.13851858011118,
106 -0.157375974517456,
107#if defined(GTSAM_POSE3_EXPMAP)
108 0.766913166076379,
109 -1.22976117053126,
110 0.949345561430261;
111#else
112 0.740211734,
113 -1.19821028,
114 1.00815609;
115#endif
116#else
117 expectedError << -0.184137861505414,
118 0.139419283914526,
119 -0.158399296722242,
120 0.740211733817804,
121 -1.198210282560946,
122 1.008156093015192;
123#endif
124
125
126 // Create a factor and calculate the error
127 Key poseKey(1);
128 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
129 TestPosePriorFactor factor(poseKey, measurement, model);
130 Vector actualError(factor.evaluateError(x: pose));
131
132 // Verify we get the expected error
133 CHECK(assert_equal(expectedError, actualError, 1e-8));
134}
135
136/* ************************************************************************* */
137TEST( PosePriorFactor, ErrorWithTransform ) {
138 // Create the measurement and linearization point
139 Pose3 measurement(Rot3::RzRyRx(x: -M_PI_2+0.15, y: -0.30, z: -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0));
140 Pose3 pose(Rot3::RzRyRx(x: 0.0, y: -0.15, z: 0.30), Point3(-4.0, 7.0, -10.0));
141 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
142
143 // The expected error
144 Vector expectedError(6);
145 // The solution depends on choice of Pose3 and Rot3 Expmap mode!
146#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
147 expectedError << -0.0224998729281528,
148 0.191947887288328,
149 0.273826035236257,
150#if defined(GTSAM_POSE3_EXPMAP)
151 1.36483391560855,
152 -0.754590051075035,
153 0.585710674473659;
154#else
155 1.49751986,
156 -0.549375791,
157 0.452761203;
158#endif
159#else
160 expectedError << -0.022712885347328,
161 0.193765110165872,
162 0.276418420819283,
163 1.497519863757366,
164 -0.549375791422721,
165 0.452761203187666;
166#endif
167
168 // Create a factor and calculate the error
169 Key poseKey(1);
170 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
171 TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
172 Vector actualError(factor.evaluateError(x: pose));
173
174 // Verify we get the expected error
175 CHECK(assert_equal(expectedError, actualError, 1e-8));
176}
177
178/* ************************************************************************* */
179TEST( PosePriorFactor, Jacobian ) {
180 // Create a factor
181 Key poseKey(1);
182 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
183 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
184 TestPosePriorFactor factor(poseKey, measurement, model);
185
186 // Create a linearization point at the zero-error point
187 Pose3 pose(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
188
189 // Calculate numerical derivatives
190 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
191 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
192
193 // Use the factor to calculate the derivative
194 Matrix actualH1;
195 factor.evaluateError(x: pose, H&: actualH1);
196
197 // Verify we get the expected error
198 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
199}
200
201/* ************************************************************************* */
202TEST( PosePriorFactor, JacobianWithTransform ) {
203 // Create a factor
204 Key poseKey(1);
205 Pose3 measurement(Rot3::RzRyRx(x: -M_PI_2+0.15, y: -0.30, z: -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0));
206 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.25);
207 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
208 TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
209
210 // Create a linearization point at the zero-error point
211 Pose3 pose(Rot3::RzRyRx(x: -0.303202977317447, y: -0.143253159173011, z: 0.494633847678171),
212 Point3(-4.74767676, 7.67044942, -11.00985));
213
214 // Calculate numerical derivatives
215 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
216 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
217
218 // Use the factor to calculate the derivative
219 Matrix actualH1;
220 factor.evaluateError(x: pose, H&: actualH1);
221
222 // Verify we get the expected error
223 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
224}
225
226/* ************************************************************************* */
227int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
228/* ************************************************************************* */
229
230