1/**
2 * @file testBetweenFactorEM.cpp
3 * @brief Unit test for the BetweenFactorEM
4 * @author Vadim Indelman
5 */
6
7#include <CppUnitLite/TestHarness.h>
8
9
10#include <gtsam_unstable/slam/BetweenFactorEM.h>
11#include <gtsam/geometry/Pose2.h>
12#include <gtsam/nonlinear/Values.h>
13#include <gtsam/base/numericalDerivative.h>
14#include <gtsam/base/Vector.h>
15
16#include <gtsam/slam/BetweenFactor.h>
17
18
19using namespace std;
20using namespace gtsam;
21
22
23// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
24// to reenable the test.
25// #if 0
26
27/* ************************************************************************* */
28Vector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM<gtsam::Pose2>& factor){
29 gtsam::Values values;
30 values.insert(j: key1, val: p1);
31 values.insert(j: key2, val: p2);
32 return factor.whitenedError(x: values);
33}
34
35/* ************************************************************************* */
36Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor<gtsam::Pose2>& factor){
37 gtsam::Values values;
38 values.insert(j: key1, val: p1);
39 values.insert(j: key2, val: p2);
40 // Vector err = factor.whitenedError(values);
41 // return err;
42 return factor.whitenedError(c: values);
43}
44
45/* ************************************************************************* */
46TEST( BetweenFactorEM, ConstructorAndEquals)
47{
48 gtsam::Key key1(1);
49 gtsam::Key key2(2);
50
51 gtsam::Pose2 p1(10.0, 15.0, 0.1);
52 gtsam::Pose2 p2(15.0, 15.0, 0.3);
53 gtsam::Pose2 noise(0.5, 0.4, 0.01);
54 gtsam::Pose2 rel_pose_ideal = p1.between(g: p2);
55 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
56
57 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(0.5, 0.5, 0.05)));
58 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(5, 5, 1.0)));
59
60 double prior_outlier = 0.5;
61 double prior_inlier = 0.5;
62
63 // Constructor
64 BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
65 prior_inlier, prior_outlier);
66 BetweenFactorEM<gtsam::Pose2> g(key1, key2, rel_pose_msr, model_inlier, model_outlier,
67 prior_inlier, prior_outlier);
68
69 // Equals
70 CHECK(assert_equal(f, g, 1e-5));
71}
72
73/* ************************************************************************* */
74TEST( BetweenFactorEM, EvaluateError)
75{
76 gtsam::Key key1(1);
77 gtsam::Key key2(2);
78
79 // Inlier test
80 gtsam::Pose2 p1(10.0, 15.0, 0.1);
81 gtsam::Pose2 p2(15.0, 15.0, 0.3);
82 gtsam::Pose2 noise(0.5, 0.4, 0.01);
83 gtsam::Pose2 rel_pose_ideal = p1.between(g: p2);
84 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
85
86 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(0.5, 0.5, 0.05)));
87 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(50.0, 50.0, 10.0)));
88
89 gtsam::Values values;
90 values.insert(j: key1, val: p1);
91 values.insert(j: key2, val: p2);
92
93 double prior_outlier = 0.5;
94 double prior_inlier = 0.5;
95
96 BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
97 prior_inlier, prior_outlier);
98
99 Vector actual_err_wh = f.whitenedError(x: values);
100
101 Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
102 Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
103
104 // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl;
105 // cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
106
107 // in case of inlier, inlier-mode whitented error should be dominant
108 // CHECK(actual_err_wh_inlier.norm() > 1000.0*actual_err_wh_outlier.norm());
109
110 // Outlier test
111 noise = gtsam::Pose2(10.5, 20.4, 2.01);
112 gtsam::Pose2 rel_pose_msr_test2 = rel_pose_ideal.compose(g: noise);
113
114 BetweenFactorEM<gtsam::Pose2> g(key1, key2, rel_pose_msr_test2, model_inlier, model_outlier,
115 prior_inlier, prior_outlier);
116
117 actual_err_wh = g.whitenedError(x: values);
118
119 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
120 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
121
122 // in case of outlier, outlier-mode whitented error should be dominant
123 // CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
124 //
125 // cout << "Outlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier<<endl;
126 // cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
127
128 // Compare with standard between factor for the inlier case
129 prior_outlier = 0.0;
130 prior_inlier = 1.0;
131 BetweenFactorEM<gtsam::Pose2> h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier,
132 prior_inlier, prior_outlier);
133 actual_err_wh = h_EM.whitenedError(x: values);
134 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
135
136 BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
137 Vector actual_err_wh_stnd = h.whitenedError(c: values);
138
139 // cout<<"actual_err_wh: "<<actual_err_wh_inlier[0]<<", "<<actual_err_wh_inlier[1]<<", "<<actual_err_wh_inlier[2]<<endl;
140 // cout<<"actual_err_wh_stnd: "<<actual_err_wh_stnd[0]<<", "<<actual_err_wh_stnd[1]<<", "<<actual_err_wh_stnd[2]<<endl;
141 //
142 // CHECK( assert_equal(actual_err_wh_inlier, actual_err_wh_stnd, 1e-8));
143}
144
145///* ************************************************************************** */
146TEST (BetweenFactorEM, jacobian ) {
147
148 gtsam::Key key1(1);
149 gtsam::Key key2(2);
150
151 // Inlier test
152 gtsam::Pose2 p1(10.0, 15.0, 0.1);
153 gtsam::Pose2 p2(15.0, 15.0, 0.3);
154 gtsam::Pose2 noise(0.5, 0.4, 0.01);
155 gtsam::Pose2 rel_pose_ideal = p1.between(g: p2);
156 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
157
158 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(0.5, 0.5, 0.05)));
159 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(50.0, 50.0, 10.0)));
160
161 gtsam::Values values;
162 values.insert(j: key1, val: p1);
163 values.insert(j: key2, val: p2);
164
165 double prior_outlier = 0.0;
166 double prior_inlier = 1.0;
167
168 BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
169 prior_inlier, prior_outlier);
170
171 std::vector<gtsam::Matrix> H_actual(2);
172 Vector actual_err_wh = f.whitenedError(x: values, H&: H_actual);
173
174 Matrix H1_actual = H_actual[0];
175 Matrix H2_actual = H_actual[1];
176
177 // compare to standard between factor
178 BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
179 Vector actual_err_wh_stnd = h.whitenedError(c: values);
180 Vector actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
181// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
182 std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
183 (void)h.unwhitenedError(x: values, H&: H_actual_stnd_unwh);
184 Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
185 Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
186 Matrix H1_actual_stnd = model_inlier->Whiten(H: H1_actual_stnd_unwh);
187 Matrix H2_actual_stnd = model_inlier->Whiten(H: H2_actual_stnd_unwh);
188// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
189// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
190
191 double stepsize = 1.0e-9;
192 using std::placeholders::_1;
193 Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(h: std::bind(f: &predictionError, args: _1, args&: p2, args&: key1, args&: key2, args&: f), x: p1, delta: stepsize);
194 Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(h: std::bind(f: &predictionError, args&: p1, args: _1, args&: key1, args&: key2, args&: f), x: p2, delta: stepsize);
195
196
197 // try to check numerical derivatives of a standard between factor
198 Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(h: std::bind(f: &predictionError_standard, args: _1, args&: p2, args&: key1, args&: key2, args&: h), x: p1, delta: stepsize);
199// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
200//
201//
202// CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
203// CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
204
205}
206
207
208/* ************************************************************************* */
209TEST( BetweenFactorEM, CaseStudy)
210{
211
212 bool debug = false;
213
214 gtsam::Key key1(1);
215 gtsam::Key key2(2);
216
217 // Inlier test
218 gtsam::Pose2 p1;
219 gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962);
220 gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182);
221
222 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(0.4021, 0.286, 0.428)));
223 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: gtsam::Vector3(4.9821, 4.614, 1.8387)));
224
225 gtsam::Values values;
226 values.insert(j: key1, val: p1);
227 values.insert(j: key2, val: p2);
228
229 double prior_outlier = 0.5;
230 double prior_inlier = 0.5;
231
232 BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
233 prior_inlier, prior_outlier);
234
235 if (debug)
236 cout << "==== inside CaseStudy ===="<<endl;
237
238 gtsam::Vector p_inlier_outler = f.calcIndicatorProb(x: values);
239
240 Vector actual_err_unw = f.unwhitenedError(x: values);
241 Vector actual_err_wh = f.whitenedError(x: values);
242
243 Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
244 Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
245
246 if (debug){
247 cout << "p_inlier_outler: "<<p_inlier_outler[0]<<", "<<p_inlier_outler[1]<<endl;
248 cout<<"actual_err_unw: "<<actual_err_unw[0]<<", "<<actual_err_unw[1]<<", "<<actual_err_unw[2]<<endl;
249 cout<<"actual_err_wh_inlier: "<<actual_err_wh_inlier[0]<<", "<<actual_err_wh_inlier[1]<<", "<<actual_err_wh_inlier[2]<<endl;
250 cout<<"actual_err_wh_outlier: "<<actual_err_wh_outlier[0]<<", "<<actual_err_wh_outlier[1]<<", "<<actual_err_wh_outlier[2]<<endl;
251 }
252}
253
254
255///* ************************************************************************** */
256TEST (BetweenFactorEM, updateNoiseModel ) {
257 gtsam::Key key1(1);
258 gtsam::Key key2(2);
259
260 gtsam::Pose2 p1(10.0, 15.0, 0.1);
261 gtsam::Pose2 p2(15.0, 15.0, 0.3);
262 gtsam::Pose2 noise(0.5, 0.4, 0.01);
263 gtsam::Pose2 rel_pose_ideal = p1.between(g: p2);
264 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
265
266 SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(1.5, 2.5, 4.05)));
267 SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(sigmas: Vector3(50.0, 50.0, 10.0)));
268
269 gtsam::Values values;
270 values.insert(j: key1, val: p1);
271 values.insert(j: key2, val: p2);
272
273 double prior_outlier = 0.0;
274 double prior_inlier = 1.0;
275
276 BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
277 prior_inlier, prior_outlier);
278
279 SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(dim: 3, sigma: 1e2));
280
281 NonlinearFactorGraph graph;
282 graph.addPrior(key: key1, prior: p1, model);
283 graph.addPrior(key: key2, prior: p2, model);
284
285 f.updateNoiseModels(values, graph);
286
287 SharedGaussian model_inlier_new = f.get_model_inlier();
288 SharedGaussian model_outlier_new = f.get_model_outlier();
289
290 // model_inlier->print("model_inlier:");
291 // model_outlier->print("model_outlier:");
292 // model_inlier_new->print("model_inlier_new:");
293 // model_outlier_new->print("model_outlier_new:");
294}
295
296
297// #endif
298
299/* ************************************************************************* */
300 int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
301/* ************************************************************************* */
302