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 testGncOptimizer.cpp
14 * @brief Unit tests for GncOptimizer class
15 * @author Jingnan Shi
16 * @author Luca Carlone
17 * @author Frank Dellaert
18 *
19 * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated
20 * Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to
21 * Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version:
22 * https://arxiv.org/pdf/1909.08605.pdf)
23 *
24 * See also:
25 * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness,
26 * Minimally-Tuned Algorithms, and Applications", arxiv:
27 * https://arxiv.org/pdf/2007.15109.pdf, 2020.
28 */
29
30#include <CppUnitLite/TestHarness.h>
31#include <gtsam/nonlinear/GncOptimizer.h>
32#include <gtsam/nonlinear/LinearContainerFactor.h>
33#include <gtsam/slam/dataset.h>
34#include <tests/smallExample.h>
35
36#include <gtsam/sam/BearingFactor.h>
37#include <gtsam/geometry/Pose2.h>
38
39using namespace std;
40using namespace gtsam;
41
42using symbol_shorthand::L;
43using symbol_shorthand::X;
44static double tol = 1e-7;
45
46/* ************************************************************************* */
47TEST(GncOptimizer, gncParamsConstructor) {
48 // check params are correctly parsed
49 LevenbergMarquardtParams lmParams;
50 GncParams<LevenbergMarquardtParams> gncParams1(lmParams);
51 CHECK(lmParams.equals(gncParams1.baseOptimizerParams));
52
53 // check also default constructor
54 GncParams<LevenbergMarquardtParams> gncParams1b;
55 CHECK(lmParams.equals(gncParams1b.baseOptimizerParams));
56
57 // and check params become different if we change lmParams
58 lmParams.setVerbosity("DELTA");
59 CHECK(!lmParams.equals(gncParams1.baseOptimizerParams));
60
61 // and same for GN
62 GaussNewtonParams gnParams;
63 GncParams<GaussNewtonParams> gncParams2(gnParams);
64 CHECK(gnParams.equals(gncParams2.baseOptimizerParams));
65
66 // check default constructor
67 GncParams<GaussNewtonParams> gncParams2b;
68 CHECK(gnParams.equals(gncParams2b.baseOptimizerParams));
69
70 // change something at the gncParams level
71 GncParams<GaussNewtonParams> gncParams2c(gncParams2b);
72 gncParams2c.setLossType(GncLossType::GM);
73 CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams));
74}
75
76/* ************************************************************************* */
77TEST(GncOptimizer, gncConstructor) {
78 // has to have Gaussian noise models !
79 auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor
80 // on a 2D point
81
82 Point2 p0(3, 3);
83 Values initial;
84 initial.insert(j: X(j: 1), val: p0);
85
86 GncParams<LevenbergMarquardtParams> gncParams;
87 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
88 gncParams);
89
90 CHECK(gnc.getFactors().equals(fg));
91 CHECK(gnc.getState().equals(initial));
92 CHECK(gnc.getParams().equals(gncParams));
93
94 auto gnc2 = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
95 gncParams);
96
97 // check the equal works
98 CHECK(gnc.equals(gnc2));
99}
100
101/* ************************************************************************* */
102TEST(GncOptimizer, solverParameterParsing) {
103 // has to have Gaussian noise models !
104 auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor
105 // on a 2D point
106
107 Point2 p0(3, 3);
108 Values initial;
109 initial.insert(j: X(j: 1), val: p0);
110
111 LevenbergMarquardtParams lmParams;
112 lmParams.setMaxIterations(0); // forces not to perform optimization
113 GncParams<LevenbergMarquardtParams> gncParams(lmParams);
114 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
115 gncParams);
116 Values result = gnc.optimize();
117
118 // check that LM did not perform optimization and result is the same as the initial guess
119 DOUBLES_EQUAL(fg.error(initial), fg.error(result), tol);
120
121 // also check the params:
122 DOUBLES_EQUAL(0.0, gncParams.baseOptimizerParams.maxIterations, tol);
123}
124
125/* ************************************************************************* */
126TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
127 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
128 // same graph with robust noise model
129 auto fg_robust = example::sharedRobustFactorGraphWithOutliers();
130
131 Point2 p0(3, 3);
132 Values initial;
133 initial.insert(j: X(j: 1), val: p0);
134
135 GncParams<LevenbergMarquardtParams> gncParams;
136 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust,
137 initial,
138 gncParams);
139
140 // make sure that when parsing the graph is transformed into one without
141 // robust loss
142 CHECK(fg.equals(gnc.getFactors()));
143}
144
145/* ************************************************************************* */
146TEST(GncOptimizer, initializeMu) {
147 auto fg = example::createReallyNonlinearFactorGraph();
148
149 Point2 p0(3, 3);
150 Values initial;
151 initial.insert(j: X(j: 1), val: p0);
152
153 // testing GM mu initialization
154 GncParams<LevenbergMarquardtParams> gncParams;
155 gncParams.setLossType(GncLossType::GM);
156 auto gnc_gm = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
157 gncParams);
158 gnc_gm.setInlierCostThresholds(1.0);
159 // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq
160 // (barcSq=1 in this example)
161 EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3);
162
163 // testing TLS mu initialization
164 gncParams.setLossType(GncLossType::TLS);
165 auto gnc_tls = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
166 gncParams);
167 gnc_tls.setInlierCostThresholds(1.0);
168 // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq)
169 // (barcSq=1 in this example)
170 EXPECT_DOUBLES_EQUAL(gnc_tls.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3);
171}
172
173/* ************************************************************************* */
174TEST(GncOptimizer, updateMuGM) {
175 // has to have Gaussian noise models !
176 auto fg = example::createReallyNonlinearFactorGraph();
177
178 Point2 p0(3, 3);
179 Values initial;
180 initial.insert(j: X(j: 1), val: p0);
181
182 GncParams<LevenbergMarquardtParams> gncParams;
183 gncParams.setLossType(GncLossType::GM);
184 gncParams.setMuStep(1.4);
185 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
186 gncParams);
187
188 double mu = 5.0;
189 EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol);
190
191 // check it correctly saturates to 1 for GM
192 mu = 1.2;
193 EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), 1.0, tol);
194}
195
196/* ************************************************************************* */
197TEST(GncOptimizer, updateMuTLS) {
198 // has to have Gaussian noise models !
199 auto fg = example::createReallyNonlinearFactorGraph();
200
201 Point2 p0(3, 3);
202 Values initial;
203 initial.insert(j: X(j: 1), val: p0);
204
205 GncParams<LevenbergMarquardtParams> gncParams;
206 gncParams.setMuStep(1.4);
207 gncParams.setLossType(GncLossType::TLS);
208 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
209 gncParams);
210
211 double mu = 5.0;
212 EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu * 1.4, tol);
213}
214
215/* ************************************************************************* */
216TEST(GncOptimizer, checkMuConvergence) {
217 // has to have Gaussian noise models !
218 auto fg = example::createReallyNonlinearFactorGraph();
219
220 Point2 p0(3, 3);
221 Values initial;
222 initial.insert(j: X(j: 1), val: p0);
223
224 {
225 GncParams<LevenbergMarquardtParams> gncParams;
226 gncParams.setLossType(GncLossType::GM);
227 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
228 gncParams);
229
230 double mu = 1.0;
231 CHECK(gnc.checkMuConvergence(mu));
232 }
233 {
234 GncParams<LevenbergMarquardtParams> gncParams;
235 gncParams.setLossType(
236 GncLossType::TLS);
237 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
238 gncParams);
239
240 double mu = 1.0;
241 CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS
242 }
243}
244
245/* ************************************************************************* */
246TEST(GncOptimizer, checkCostConvergence) {
247 // has to have Gaussian noise models !
248 auto fg = example::createReallyNonlinearFactorGraph();
249
250 Point2 p0(3, 3);
251 Values initial;
252 initial.insert(j: X(j: 1), val: p0);
253
254 {
255 GncParams<LevenbergMarquardtParams> gncParams;
256 gncParams.setRelativeCostTol(0.49);
257 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
258 gncParams);
259
260 double prev_cost = 1.0;
261 double cost = 0.5;
262 // relative cost reduction = 0.5 > 0.49, hence checkCostConvergence = false
263 CHECK(!gnc.checkCostConvergence(cost, prev_cost));
264 }
265 {
266 GncParams<LevenbergMarquardtParams> gncParams;
267 gncParams.setRelativeCostTol(0.51);
268 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
269 gncParams);
270
271 double prev_cost = 1.0;
272 double cost = 0.5;
273 // relative cost reduction = 0.5 < 0.51, hence checkCostConvergence = true
274 CHECK(gnc.checkCostConvergence(cost, prev_cost));
275 }
276}
277
278/* ************************************************************************* */
279TEST(GncOptimizer, checkWeightsConvergence) {
280 // has to have Gaussian noise models !
281 auto fg = example::createReallyNonlinearFactorGraph();
282
283 Point2 p0(3, 3);
284 Values initial;
285 initial.insert(j: X(j: 1), val: p0);
286
287 {
288 GncParams<LevenbergMarquardtParams> gncParams;
289 gncParams.setLossType(GncLossType::GM);
290 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
291 gncParams);
292
293 Vector weights = Vector::Ones(newSize: fg.size());
294 CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM
295 }
296 {
297 GncParams<LevenbergMarquardtParams> gncParams;
298 gncParams.setLossType(
299 GncLossType::TLS);
300 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
301 gncParams);
302
303 Vector weights = Vector::Ones(newSize: fg.size());
304 // weights are binary, so checkWeightsConvergence = true
305 CHECK(gnc.checkWeightsConvergence(weights));
306 }
307 {
308 GncParams<LevenbergMarquardtParams> gncParams;
309 gncParams.setLossType(
310 GncLossType::TLS);
311 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
312 gncParams);
313
314 Vector weights = Vector::Ones(newSize: fg.size());
315 weights[0] = 0.9; // more than weightsTol = 1e-4 from 1, hence checkWeightsConvergence = false
316 CHECK(!gnc.checkWeightsConvergence(weights));
317 }
318 {
319 GncParams<LevenbergMarquardtParams> gncParams;
320 gncParams.setLossType(
321 GncLossType::TLS);
322 gncParams.setWeightsTol(0.1);
323 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
324 gncParams);
325
326 Vector weights = Vector::Ones(newSize: fg.size());
327 weights[0] = 0.9; // exactly weightsTol = 0.1 from 1, hence checkWeightsConvergence = true
328 CHECK(gnc.checkWeightsConvergence(weights));
329 }
330}
331
332/* ************************************************************************* */
333TEST(GncOptimizer, checkConvergenceTLS) {
334 // has to have Gaussian noise models !
335 auto fg = example::createReallyNonlinearFactorGraph();
336
337 Point2 p0(3, 3);
338 Values initial;
339 initial.insert(j: X(j: 1), val: p0);
340
341 GncParams<LevenbergMarquardtParams> gncParams;
342 gncParams.setRelativeCostTol(1e-5);
343 gncParams.setLossType(GncLossType::TLS);
344 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
345 gncParams);
346
347 CHECK(gnc.checkCostConvergence(1.0, 1.0));
348 CHECK(!gnc.checkCostConvergence(1.0, 2.0));
349}
350
351/* ************************************************************************* */
352TEST(GncOptimizer, calculateWeightsGM) {
353 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
354
355 Point2 p0(0, 0);
356 Values initial;
357 initial.insert(j: X(j: 1), val: p0);
358
359 // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 *
360 // 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier)
361 Vector weights_expected = Vector::Zero(size: 4);
362 weights_expected[0] = 1.0; // zero error
363 weights_expected[1] = 1.0; // zero error
364 weights_expected[2] = 1.0; // zero error
365 weights_expected[3] = std::pow(x: 1.0 / (50.0 + 1.0), y: 2); // outlier, error = 50
366
367 GaussNewtonParams gnParams;
368 GncParams<GaussNewtonParams> gncParams(gnParams);
369 gncParams.setLossType(GncLossType::GM);
370 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
371 gnc.setInlierCostThresholds(1.0);
372 double mu = 1.0;
373 Vector weights_actual = gnc.calculateWeights(currentEstimate: initial, mu);
374 CHECK(assert_equal(weights_expected, weights_actual, tol));
375
376 mu = 2.0;
377 double barcSq = 5.0;
378 weights_expected[3] = std::pow(x: mu * barcSq / (50.0 + mu * barcSq), y: 2); // outlier, error = 50
379
380 auto gnc2 = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
381 gncParams);
382 gnc2.setInlierCostThresholds(barcSq);
383 weights_actual = gnc2.calculateWeights(currentEstimate: initial, mu);
384 CHECK(assert_equal(weights_expected, weights_actual, tol));
385}
386
387/* ************************************************************************* */
388TEST(GncOptimizer, calculateWeightsTLS) {
389 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
390
391 Point2 p0(0, 0);
392 Values initial;
393 initial.insert(j: X(j: 1), val: p0);
394
395 // we have 4 factors, 3 with zero errors (inliers), 1 with error
396 Vector weights_expected = Vector::Zero(size: 4);
397 weights_expected[0] = 1.0; // zero error
398 weights_expected[1] = 1.0; // zero error
399 weights_expected[2] = 1.0; // zero error
400 weights_expected[3] = 0; // outliers
401
402 GaussNewtonParams gnParams;
403 GncParams<GaussNewtonParams> gncParams(gnParams);
404 gncParams.setLossType(GncLossType::TLS);
405 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
406 double mu = 1.0;
407 Vector weights_actual = gnc.calculateWeights(currentEstimate: initial, mu);
408 CHECK(assert_equal(weights_expected, weights_actual, tol));
409}
410
411/* ************************************************************************* */
412TEST(GncOptimizer, calculateWeightsTLS2) {
413
414 // create values
415 Point2 x_val(0.0, 0.0);
416 Point2 x_prior(1.0, 0.0);
417 Values initial;
418 initial.insert(j: X(j: 1), val: x_val);
419
420 // create very simple factor graph with a single factor 0.5 * 1/sigma^2 * || x - [1;0] ||^2
421 double sigma = 1;
422 SharedDiagonal noise = noiseModel::Diagonal::Sigmas(sigmas: Vector2(sigma, sigma));
423 NonlinearFactorGraph nfg;
424 nfg.add(factorOrContainer: PriorFactor<Point2>(X(j: 1), x_prior, noise));
425
426 // cost of the factor:
427 DOUBLES_EQUAL(0.5 * 1 / (sigma * sigma), nfg.error(initial), tol);
428
429 // check the TLS weights are correct: CASE 1: residual below barcsq
430 {
431 // expected:
432 Vector weights_expected = Vector::Zero(size: 1);
433 weights_expected[0] = 1.0; // inlier
434 // actual:
435 GaussNewtonParams gnParams;
436 GncParams<GaussNewtonParams> gncParams(gnParams);
437 gncParams.setLossType(GncLossType::TLS);
438 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial,
439 gncParams);
440 gnc.setInlierCostThresholds(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier
441
442 double mu = 1e6;
443 Vector weights_actual = gnc.calculateWeights(currentEstimate: initial, mu);
444 CHECK(assert_equal(weights_expected, weights_actual, tol));
445 }
446 // check the TLS weights are correct: CASE 2: residual above barcsq
447 {
448 // expected:
449 Vector weights_expected = Vector::Zero(size: 1);
450 weights_expected[0] = 0.0; // outlier
451 // actual:
452 GaussNewtonParams gnParams;
453 GncParams<GaussNewtonParams> gncParams(gnParams);
454 gncParams.setLossType(GncLossType::TLS);
455 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial,
456 gncParams);
457 gnc.setInlierCostThresholds(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier
458 double mu = 1e6; // very large mu recovers original TLS cost
459 Vector weights_actual = gnc.calculateWeights(currentEstimate: initial, mu);
460 CHECK(assert_equal(weights_expected, weights_actual, tol));
461 }
462 // check the TLS weights are correct: CASE 2: residual at barcsq
463 {
464 // expected:
465 Vector weights_expected = Vector::Zero(size: 1);
466 weights_expected[0] = 0.5; // undecided
467 // actual:
468 GaussNewtonParams gnParams;
469 GncParams<GaussNewtonParams> gncParams(gnParams);
470 gncParams.setLossType(GncLossType::TLS);
471 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial,
472 gncParams);
473 gnc.setInlierCostThresholds(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier
474 double mu = 1e6; // very large mu recovers original TLS cost
475 Vector weights_actual = gnc.calculateWeights(currentEstimate: initial, mu);
476 CHECK(assert_equal(weights_expected, weights_actual, 1e-5));
477 }
478}
479
480/* ************************************************************************* */
481TEST(GncOptimizer, makeWeightedGraph) {
482 // create original factor
483 double sigma1 = 0.1;
484 NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(
485 sigma: sigma1);
486
487 // create expected
488 double sigma2 = 10;
489 NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(
490 sigma: sigma2);
491
492 // create weights
493 Vector weights = Vector::Ones(newSize: 1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4
494 weights[0] = 1e-4;
495
496 // create actual
497 Point2 p0(3, 3);
498 Values initial;
499 initial.insert(j: X(j: 1), val: p0);
500
501 LevenbergMarquardtParams lmParams;
502 GncParams<LevenbergMarquardtParams> gncParams(lmParams);
503 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(nfg, initial,
504 gncParams);
505 NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights);
506
507 // check it's all good
508 CHECK(assert_equal(expected, actual));
509}
510
511/* ************************************************************************* */
512TEST(GncOptimizer, optimizeSimple) {
513 auto fg = example::createReallyNonlinearFactorGraph();
514
515 Point2 p0(3, 3);
516 Values initial;
517 initial.insert(j: X(j: 1), val: p0);
518
519 LevenbergMarquardtParams lmParams;
520 GncParams<LevenbergMarquardtParams> gncParams(lmParams);
521 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
522 gncParams);
523
524 Values actual = gnc.optimize();
525 DOUBLES_EQUAL(0, fg.error(actual), tol);
526}
527
528/* ************************************************************************* */
529TEST(GncOptimizer, optimize) {
530 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
531
532 Point2 p0(1, 0);
533 Values initial;
534 initial.insert(j: X(j: 1), val: p0);
535
536 // try with nonrobust cost function and standard GN
537 GaussNewtonParams gnParams;
538 GaussNewtonOptimizer gn(fg, initial, gnParams);
539 Values gn_results = gn.optimize();
540 // converges to incorrect point due to lack of robustness to an outlier, ideal
541 // solution is Point2(0,0)
542 CHECK(assert_equal(Point2(0.25, 0.0), gn_results.at<Point2>(X(1)), 1e-3));
543
544 // try with robust loss function and standard GN
545 auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with
546 // factors wrapped in
547 // Geman McClure losses
548 GaussNewtonOptimizer gn2(fg_robust, initial, gnParams);
549 Values gn2_results = gn2.optimize();
550 // converges to incorrect point, this time due to the nonconvexity of the loss
551 CHECK(assert_equal(Point2(0.999706, 0.0), gn2_results.at<Point2>(X(1)), 1e-3));
552
553 // .. but graduated nonconvexity ensures both robustness and convergence in
554 // the face of nonconvexity
555 GncParams<GaussNewtonParams> gncParams(gnParams);
556 // gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
557 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
558 Values gnc_result = gnc.optimize();
559 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
560}
561
562/* ************************************************************************* */
563TEST(GncOptimizer, optimizeWithKnownInliers) {
564 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
565
566 Point2 p0(1, 0);
567 Values initial;
568 initial.insert(j: X(j: 1), val: p0);
569
570 GncParams<GaussNewtonParams>::IndexVector knownInliers;
571 knownInliers.push_back(x: 0);
572 knownInliers.push_back(x: 1);
573 knownInliers.push_back(x: 2);
574
575 // nonconvexity with known inliers
576 {
577 GncParams<GaussNewtonParams> gncParams;
578 gncParams.setKnownInliers(knownInliers);
579 gncParams.setLossType(GncLossType::GM);
580 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
581 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
582 gncParams);
583 gnc.setInlierCostThresholds(1.0);
584 Values gnc_result = gnc.optimize();
585 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
586
587 // check weights were actually fixed:
588 Vector finalWeights = gnc.getWeights();
589 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
590 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
591 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
592 }
593 {
594 GncParams<GaussNewtonParams> gncParams;
595 gncParams.setKnownInliers(knownInliers);
596 gncParams.setLossType(GncLossType::TLS);
597 // gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
598 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
599 gncParams);
600
601 Values gnc_result = gnc.optimize();
602 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
603
604 // check weights were actually fixed:
605 Vector finalWeights = gnc.getWeights();
606 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
607 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
608 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
609 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
610 }
611 {
612 // if we set the threshold large, they are all inliers
613 GncParams<GaussNewtonParams> gncParams;
614 gncParams.setKnownInliers(knownInliers);
615 gncParams.setLossType(GncLossType::TLS);
616 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::VALUES);
617 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
618 gncParams);
619 gnc.setInlierCostThresholds(100.0);
620
621 Values gnc_result = gnc.optimize();
622 CHECK(assert_equal(Point2(0.25, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
623
624 // check weights were actually fixed:
625 Vector finalWeights = gnc.getWeights();
626 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
627 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
628 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
629 DOUBLES_EQUAL(1.0, finalWeights[3], tol);
630 }
631}
632
633/* ************************************************************************* */
634TEST(GncOptimizer, chi2inv) {
635 DOUBLES_EQUAL(8.807468393511950, Chi2inv(0.997, 1), tol); // from MATLAB: chi2inv(0.997, 1) = 8.807468393511950
636 DOUBLES_EQUAL(13.931422665512077, Chi2inv(0.997, 3), tol); // from MATLAB: chi2inv(0.997, 3) = 13.931422665512077
637}
638
639/* ************************************************************************* */
640TEST(GncOptimizer, barcsq) {
641 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
642
643 Point2 p0(1, 0);
644 Values initial;
645 initial.insert(j: X(j: 1), val: p0);
646
647 GncParams<GaussNewtonParams>::IndexVector knownInliers;
648 knownInliers.push_back(x: 0);
649 knownInliers.push_back(x: 1);
650 knownInliers.push_back(x: 2);
651
652 GncParams<GaussNewtonParams> gncParams;
653 gncParams.setKnownInliers(knownInliers);
654 gncParams.setLossType(GncLossType::GM);
655 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
656 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
657 gncParams);
658 // expected: chi2inv(0.99, 2)/2
659 CHECK(assert_equal(4.605170185988091 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3));
660}
661
662/* ************************************************************************* */
663TEST(GncOptimizer, barcsq_heterogeneousFactors) {
664 NonlinearFactorGraph fg;
665 // specify noise model, otherwise it segfault if we leave default noise model
666 SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.5));
667 fg.add( factorOrContainer: PriorFactor<Pose2>( 0, Pose2(0.0, 0.0, 0.0) , model3D )); // size 3
668 SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.5));
669 fg.add( factorOrContainer: PriorFactor<Point2>( 1, Point2(0.0,0.0), model2D )); // size 2
670 SharedNoiseModel model1D(noiseModel::Isotropic::Sigma(dim: 1, sigma: 0.5));
671 fg.add( factorOrContainer: BearingFactor<Pose2, Point2>( 0, 1, 1.0, model1D) ); // size 1
672
673 Values initial;
674 initial.insert(j: 0, val: Pose2(0.0, 0.0, 0.0));
675 initial.insert(j: 1, val: Point2(0.0,0.0));
676
677 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial);
678 CHECK(assert_equal(Vector3(5.672433365072185, 4.605170185988091, 3.317448300510607),
679 gnc.getInlierCostThresholds(), 1e-3));
680
681 // extra test:
682 // fg.add( PriorFactor<Pose2>( 0, Pose2(0.0, 0.0, 0.0) )); // works if we add model3D as noise model
683 // std::cout << "fg[3]->dim() " << fg[3]->dim() << std::endl; // this segfaults?
684}
685
686/* ************************************************************************* */
687TEST(GncOptimizer, setInlierCostThresholds) {
688 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
689
690 Point2 p0(1, 0);
691 Values initial;
692 initial.insert(j: X(j: 1), val: p0);
693
694 GncParams<GaussNewtonParams>::IndexVector knownInliers;
695 knownInliers.push_back(x: 0);
696 knownInliers.push_back(x: 1);
697 knownInliers.push_back(x: 2);
698 {
699 GncParams<GaussNewtonParams> gncParams;
700 gncParams.setKnownInliers(knownInliers);
701 gncParams.setLossType(GncLossType::GM);
702 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
703 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
704 gncParams);
705 gnc.setInlierCostThresholds(2.0);
706 Values gnc_result = gnc.optimize();
707 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
708
709 // check weights were actually fixed:
710 Vector finalWeights = gnc.getWeights();
711 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
712 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
713 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
714 CHECK(assert_equal(2.0 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3));
715 }
716 {
717 GncParams<GaussNewtonParams> gncParams;
718 gncParams.setKnownInliers(knownInliers);
719 gncParams.setLossType(GncLossType::GM);
720 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
721 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
722 gncParams);
723 gnc.setInlierCostThresholds(2.0 * Vector::Ones(newSize: fg.size()));
724 Values gnc_result = gnc.optimize();
725 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
726
727 // check weights were actually fixed:
728 Vector finalWeights = gnc.getWeights();
729 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
730 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
731 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
732 CHECK(assert_equal(2.0 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3));
733 }
734}
735
736/* ************************************************************************* */
737TEST(GncOptimizer, optimizeSmallPoseGraph) {
738 /// load small pose graph
739 const string filename = findExampleDataFile(name: "w100.graph");
740 const auto [graph, initial] = load2D(filename);
741 // Add a Gaussian prior on first poses
742 Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
743 SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
744 sigmas: Vector3(0.01, 0.01, 0.01));
745 graph->addPrior(key: 0, prior: priorMean, model: priorNoise);
746
747 /// get expected values by optimizing outlier-free graph
748 Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
749
750 // add a few outliers
751 SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
752 sigmas: Vector3(0.1, 0.1, 0.01));
753 // some arbitrary and incorrect between factor
754 graph->push_back(factor: BetweenFactor<Pose2>(90, 50, Pose2(), betweenNoise));
755
756 /// get expected values by optimizing outlier-free graph
757 Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial)
758 .optimize();
759 // as expected, the following test fails due to the presence of an outlier!
760 // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3));
761
762 // GNC
763 // NOTE: in difficult instances, we set the odometry measurements to be
764 // inliers, but this problem is simple enough to succeed even without that
765 // assumption.
766 GncParams<GaussNewtonParams> gncParams;
767 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
768 gncParams);
769 Values actual = gnc.optimize();
770
771 // compare
772 CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers!
773}
774
775/* ************************************************************************* */
776TEST(GncOptimizer, knownInliersAndOutliers) {
777 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
778
779 Point2 p0(1, 0);
780 Values initial;
781 initial.insert(j: X(j: 1), val: p0);
782
783 // nonconvexity with known inliers and known outliers (check early stopping
784 // when all measurements are known to be inliers or outliers)
785 {
786 GncParams<GaussNewtonParams>::IndexVector knownInliers;
787 knownInliers.push_back(x: 0);
788 knownInliers.push_back(x: 1);
789 knownInliers.push_back(x: 2);
790
791 GncParams<GaussNewtonParams>::IndexVector knownOutliers;
792 knownOutliers.push_back(x: 3);
793
794 GncParams<GaussNewtonParams> gncParams;
795 gncParams.setKnownInliers(knownInliers);
796 gncParams.setKnownOutliers(knownOutliers);
797 gncParams.setLossType(GncLossType::GM);
798 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
799 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
800 gncParams);
801 gnc.setInlierCostThresholds(1.0);
802 Values gnc_result = gnc.optimize();
803 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
804
805 // check weights were actually fixed:
806 Vector finalWeights = gnc.getWeights();
807 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
808 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
809 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
810 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
811 }
812
813 // nonconvexity with known inliers and known outliers
814 {
815 GncParams<GaussNewtonParams>::IndexVector knownInliers;
816 knownInliers.push_back(x: 2);
817 knownInliers.push_back(x: 0);
818
819 GncParams<GaussNewtonParams>::IndexVector knownOutliers;
820 knownOutliers.push_back(x: 3);
821
822 GncParams<GaussNewtonParams> gncParams;
823 gncParams.setKnownInliers(knownInliers);
824 gncParams.setKnownOutliers(knownOutliers);
825 gncParams.setLossType(GncLossType::GM);
826 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
827 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
828 gncParams);
829 gnc.setInlierCostThresholds(1.0);
830 Values gnc_result = gnc.optimize();
831 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
832
833 // check weights were actually fixed:
834 Vector finalWeights = gnc.getWeights();
835 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
836 DOUBLES_EQUAL(1.0, finalWeights[1], 1e-5);
837 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
838 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
839 }
840
841 // only known outliers
842 {
843 GncParams<GaussNewtonParams>::IndexVector knownOutliers;
844 knownOutliers.push_back(x: 3);
845
846 GncParams<GaussNewtonParams> gncParams;
847 gncParams.setKnownOutliers(knownOutliers);
848 gncParams.setLossType(GncLossType::GM);
849 //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
850 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
851 gncParams);
852 gnc.setInlierCostThresholds(1.0);
853 Values gnc_result = gnc.optimize();
854 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
855
856 // check weights were actually fixed:
857 Vector finalWeights = gnc.getWeights();
858 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
859 DOUBLES_EQUAL(1.0, finalWeights[1], 1e-5);
860 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
861 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
862 }
863}
864
865/* ************************************************************************* */
866TEST(GncOptimizer, setWeights) {
867 auto fg = example::sharedNonRobustFactorGraphWithOutliers();
868
869 Point2 p0(1, 0);
870 Values initial;
871 initial.insert(j: X(j: 1), val: p0);
872 // initialize weights to be the same
873 {
874 GncParams<GaussNewtonParams> gncParams;
875 gncParams.setLossType(GncLossType::TLS);
876
877 Vector weights = 0.5 * Vector::Ones(newSize: fg.size());
878 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
879 gncParams);
880 gnc.setWeights(weights);
881 gnc.setInlierCostThresholds(1.0);
882 Values gnc_result = gnc.optimize();
883 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
884
885 // check weights were actually fixed:
886 Vector finalWeights = gnc.getWeights();
887 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
888 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
889 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
890 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
891 }
892 // try a more challenging initialization
893 {
894 GncParams<GaussNewtonParams> gncParams;
895 gncParams.setLossType(GncLossType::TLS);
896
897 Vector weights = Vector::Zero(size: fg.size());
898 weights(2) = 1.0;
899 weights(3) = 1.0; // bad initialization: we say the outlier is inlier
900 // GNC can still recover (but if you omit weights(2) = 1.0, then it would fail)
901 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
902 gncParams);
903 gnc.setWeights(weights);
904 gnc.setInlierCostThresholds(1.0);
905 Values gnc_result = gnc.optimize();
906 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
907
908 // check weights were actually fixed:
909 Vector finalWeights = gnc.getWeights();
910 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
911 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
912 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
913 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
914 }
915 // initialize weights and also set known inliers/outliers
916 {
917 GncParams<GaussNewtonParams> gncParams;
918 GncParams<GaussNewtonParams>::IndexVector knownInliers;
919 knownInliers.push_back(x: 2);
920 knownInliers.push_back(x: 0);
921
922 GncParams<GaussNewtonParams>::IndexVector knownOutliers;
923 knownOutliers.push_back(x: 3);
924 gncParams.setKnownInliers(knownInliers);
925 gncParams.setKnownOutliers(knownOutliers);
926
927 gncParams.setLossType(GncLossType::TLS);
928
929 Vector weights = 0.5 * Vector::Ones(newSize: fg.size());
930 auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
931 gncParams);
932 gnc.setWeights(weights);
933 gnc.setInlierCostThresholds(1.0);
934 Values gnc_result = gnc.optimize();
935 CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
936
937 // check weights were actually fixed:
938 Vector finalWeights = gnc.getWeights();
939 DOUBLES_EQUAL(1.0, finalWeights[0], tol);
940 DOUBLES_EQUAL(1.0, finalWeights[1], tol);
941 DOUBLES_EQUAL(1.0, finalWeights[2], tol);
942 DOUBLES_EQUAL(0.0, finalWeights[3], tol);
943 }
944}
945
946/* ************************************************************************* */
947int main() {
948 TestResult tr;
949 return TestRegistry::runAllTests(result&: tr);
950}
951/* ************************************************************************* */
952