1/**
2 * @file testBTransformBtwRobotsUnaryFactorEM.cpp
3 * @brief Unit test for the TransformBtwRobotsUnaryFactorEM
4 * @author Vadim Indelman
5 */
6
7#include <CppUnitLite/TestHarness.h>
8
9
10#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h>
11#include <gtsam/geometry/Pose2.h>
12#include <gtsam/nonlinear/Values.h>
13#include <gtsam/base/numericalDerivative.h>
14
15#include <gtsam/slam/BetweenFactor.h>
16
17#include <gtsam/nonlinear/NonlinearOptimizer.h>
18#include <gtsam/nonlinear/NonlinearFactorGraph.h>
19#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
20
21using namespace std::placeholders;
22using namespace std;
23using namespace gtsam;
24
25/* ************************************************************************* */
26Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){
27 gtsam::Values values;
28 values.insert(j: key, val: org1_T_org2);
29 return factor.whitenedError(x: values);
30}
31
32/* ************************************************************************* */
33//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
34// gtsam::Values values;
35// values.insert(keyA, p1);
36// values.insert(keyB, p2);
37// // Vector err = factor.whitenedError(values);
38// // return err;
39// return Vector::Expmap(factor.whitenedError(values));
40//}
41
42/* ************************************************************************* */
43TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals)
44{
45 gtsam::Key key(0);
46 gtsam::Key keyA(1);
47 gtsam::Key keyB(2);
48
49 gtsam::Pose2 p1(10.0, 15.0, 0.1);
50 gtsam::Pose2 p2(15.0, 15.0, 0.3);
51 gtsam::Pose2 noise(0.5, 0.4, 0.01);
52 gtsam::Pose2 rel_pose_ideal = p1.between(g: p2);
53 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
54
55 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
56 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(5, 5, 1.0)));
57
58 double prior_outlier = 0.5;
59 double prior_inlier = 0.5;
60
61 gtsam::Values valA, valB;
62 valA.insert(j: keyA, val: p1);
63 valB.insert(j: keyB, val: p2);
64
65 // Constructor
66 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
67 model_inlier, model_outlier,prior_inlier, prior_outlier);
68 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> h(key, rel_pose_msr, keyA, keyB, valA, valB,
69 model_inlier, model_outlier,prior_inlier, prior_outlier);
70
71 // Equals
72 CHECK(assert_equal(g, h, 1e-5));
73}
74
75/* ************************************************************************* */
76TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError)
77{
78 gtsam::Key key(0);
79 gtsam::Key keyA(1);
80 gtsam::Key keyB(2);
81
82 gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
83 gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
84
85 gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
86
87 gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(g: orgB_T_2);
88
89 gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(g: orgA_T_2);
90 gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
91
92 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
93 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(5, 5, 1.0)));
94
95 double prior_outlier = 0.01;
96 double prior_inlier = 0.99;
97
98 gtsam::Values valA, valB;
99 valA.insert(j: keyA, val: orgA_T_1);
100 valB.insert(j: keyB, val: orgB_T_2);
101
102 // Constructor
103 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
104 model_inlier, model_outlier,prior_inlier, prior_outlier);
105
106 gtsam::Values values;
107 values.insert(j: key, val: orgA_T_orgB);
108 Vector err = g.unwhitenedError(x: values);
109
110 // Equals
111 CHECK(assert_equal(err, Z_3x1, 1e-5));
112}
113
114/* ************************************************************************* */
115TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2)
116{
117 gtsam::Key key(0);
118 gtsam::Key keyA(1);
119 gtsam::Key keyB(2);
120
121 gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
122 gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1);
123
124 gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0);
125
126 gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(g: orgB_T_currB);
127
128 gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(g: orgA_T_currB);
129 gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
130
131 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
132 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(5, 5, 1.0)));
133
134 double prior_outlier = 0.01;
135 double prior_inlier = 0.99;
136
137 gtsam::Values valA, valB;
138 valA.insert(j: keyA, val: orgA_T_currA);
139 valB.insert(j: keyB, val: orgB_T_currB);
140
141 // Constructor
142 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
143 model_inlier, model_outlier,prior_inlier, prior_outlier);
144
145 gtsam::Values values;
146 values.insert(j: key, val: orgA_T_orgB);
147 Vector err = g.unwhitenedError(x: values);
148
149 // Equals
150 CHECK(assert_equal(err, Z_3x1, 1e-5));
151}
152
153/* ************************************************************************* */
154TEST( TransformBtwRobotsUnaryFactorEM, Optimize)
155{
156 gtsam::Key key(0);
157 gtsam::Key keyA(1);
158 gtsam::Key keyB(2);
159
160 gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
161 gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05);
162
163 gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0);
164 gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(g: orgB_T_currB);
165 gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(g: orgA_T_currB_tr);
166
167 // some error in measurements
168 // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01));
169 // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01));
170 // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01));
171 // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01));
172
173 // ideal measurements
174 gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(g: gtsam::Pose2(0.0, 0.0, 0.0));
175 gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1;
176 gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
177 gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
178
179 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
180 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(5, 5, 1.0)));
181
182 double prior_outlier = 0.01;
183 double prior_inlier = 0.99;
184
185 gtsam::Values valA, valB;
186 valA.insert(j: keyA, val: orgA_T_currA);
187 valB.insert(j: keyB, val: orgB_T_currB);
188
189 // Constructor
190 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB,
191 model_inlier, model_outlier,prior_inlier, prior_outlier);
192
193 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB,
194 model_inlier, model_outlier,prior_inlier, prior_outlier);
195
196 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB,
197 model_inlier, model_outlier,prior_inlier, prior_outlier);
198
199 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB,
200 model_inlier, model_outlier,prior_inlier, prior_outlier);
201
202 gtsam::Values values;
203 values.insert(j: key, val: gtsam::Pose2());
204
205 gtsam::NonlinearFactorGraph graph;
206 graph.push_back(factor: g1);
207 graph.push_back(factor: g2);
208 graph.push_back(factor: g3);
209 graph.push_back(factor: g4);
210
211 gtsam::GaussNewtonParams params;
212 gtsam::GaussNewtonOptimizer optimizer(graph, values, params);
213 gtsam::Values result = optimizer.optimize();
214
215 gtsam::Pose2 orgA_T_orgB_opt = result.at<gtsam::Pose2>(j: key);
216
217 CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5));
218}
219
220
221/* ************************************************************************* */
222TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
223{
224 gtsam::Key key(0);
225 gtsam::Key keyA(1);
226 gtsam::Key keyB(2);
227
228 gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
229 gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
230
231 gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
232
233 gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(g: orgB_T_2);
234
235 gtsam::Pose2 noise(0.5, 0.4, 0.01);
236
237 gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(g: orgA_T_2);
238 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
239
240 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
241 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(5, 5, 1.0)));
242
243 double prior_outlier = 0.5;
244 double prior_inlier = 0.5;
245
246 gtsam::Values valA, valB;
247 valA.insert(j: keyA, val: orgA_T_1);
248 valB.insert(j: keyB, val: orgB_T_2);
249
250 // Constructor
251 TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
252 model_inlier, model_outlier,prior_inlier, prior_outlier);
253
254 gtsam::Values values;
255 values.insert(j: key, val: orgA_T_orgB);
256
257 std::vector<gtsam::Matrix> H_actual(1);
258 Vector actual_err_wh = g.whitenedError(x: values, H&: H_actual);
259
260 Matrix H1_actual = H_actual[0];
261
262 double stepsize = 1.0e-9;
263 Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(
264 h: std::bind(f: &predictionError, args: std::placeholders::_1, args&: key, args&: g), x: orgA_T_orgB,
265 delta: stepsize);
266 // CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
267}
268/////* ************************************************************************** */
269//TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) {
270//
271// gtsam::Key keyA(1);
272// gtsam::Key keyB(2);
273//
274// // Inlier test
275// gtsam::Pose2 p1(10.0, 15.0, 0.1);
276// gtsam::Pose2 p2(15.0, 15.0, 0.3);
277// gtsam::Pose2 noise(0.5, 0.4, 0.01);
278// gtsam::Pose2 rel_pose_ideal = p1.between(p2);
279// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
280//
281// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
282// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
283//
284// gtsam::Values values;
285// values.insert(keyA, p1);
286// values.insert(keyB, p2);
287//
288// double prior_outlier = 0.0;
289// double prior_inlier = 1.0;
290//
291// TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier,
292// prior_inlier, prior_outlier);
293//
294// std::vector<gtsam::Matrix> H_actual(2);
295// Vector actual_err_wh = f.whitenedError(values, H_actual);
296//
297// Matrix H1_actual = H_actual[0];
298// Matrix H2_actual = H_actual[1];
299//
300// // compare to standard between factor
301// BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
302// Vector actual_err_wh_stnd = h.whitenedError(values);
303// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
304// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
305// std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
306// (void)h.unwhitenedError(values, H_actual_stnd_unwh);
307// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
308// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
309// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
310// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
311//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
312//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
313//
314// double stepsize = 1.0e-9;
315// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
316// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
317//
318//
319// // try to check numerical derivatives of a standard between factor
320// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
321// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
322//
323//
324// CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
325// CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
326//
327//}
328
329/* ************************************************************************* */
330 int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
331/* ************************************************************************* */
332