testNoiseModel.cpp
Go to the documentation of this file.
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 
22 
24 
25 #include <iostream>
26 #include <limits>
27 
28 using namespace std;
29 using namespace gtsam;
30 using namespace noiseModel;
31 
32 static const double kSigma = 2, kInverseSigma = 1.0 / kSigma,
34 static const Matrix R = I_3x3 * kInverseSigma;
35 static const Matrix kCovariance = I_3x3 * kVariance;
36 static const Vector3 kSigmas(kSigma, kSigma, kSigma);
37 
38 /* ************************************************************************* */
39 TEST(NoiseModel, constructors)
40 {
41  Vector whitened = Vector3(5.0,10.0,15.0);
42  Vector unwhitened = Vector3(10.0,20.0,30.0);
43 
44  // Construct noise models
45  vector<Gaussian::shared_ptr> m;
46  m.push_back(Gaussian::SqrtInformation(R,false));
47  m.push_back(Gaussian::Covariance(kCovariance,false));
48  m.push_back(Gaussian::Information(kCovariance.inverse(),false));
49  m.push_back(Diagonal::Sigmas(kSigmas,false));
50  m.push_back(Diagonal::Variances((Vector3(kVariance, kVariance, kVariance)),false));
51  m.push_back(Diagonal::Precisions(Vector3(prc, prc, prc),false));
52  m.push_back(Isotropic::Sigma(3, kSigma,false));
53  m.push_back(Isotropic::Variance(3, kVariance,false));
54  m.push_back(Isotropic::Precision(3, prc,false));
55 
56  // test kSigmas
57  for(Gaussian::shared_ptr mi: m)
58  EXPECT(assert_equal(kSigmas,mi->sigmas()));
59 
60  // test whiten
61  for(Gaussian::shared_ptr mi: m)
62  EXPECT(assert_equal(whitened,mi->whiten(unwhitened)));
63 
64  // test unwhiten
65  for(Gaussian::shared_ptr mi: m)
66  EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
67 
68  // test squared Mahalanobis distance
69  double distance = 5*5+10*10+15*15;
70  for(Gaussian::shared_ptr mi: m)
71  DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9);
72 
73  // test R matrix
74  for(Gaussian::shared_ptr mi: m)
75  EXPECT(assert_equal(R,mi->R()));
76 
77  // test covariance
78  for(Gaussian::shared_ptr mi: m)
79  EXPECT(assert_equal(kCovariance,mi->covariance()));
80 
81  // test covariance
82  for(Gaussian::shared_ptr mi: m)
83  EXPECT(assert_equal(kCovariance.inverse(),mi->information()));
84 
85  // test Whiten operator
86  Matrix H((Matrix(3, 4) <<
87  0.0, 0.0, 1.0, 1.0,
88  0.0, 1.0, 0.0, 1.0,
89  1.0, 0.0, 0.0, 1.0).finished());
91  for(Gaussian::shared_ptr mi: m)
92  EXPECT(assert_equal(expected,mi->Whiten(H)));
93 
94  // can only test inplace version once :-)
95  m[0]->WhitenInPlace(H);
96  EXPECT(assert_equal(expected,H));
97 }
98 
99 /* ************************************************************************* */
100 TEST(NoiseModel, Unit)
101 {
102  Vector v = Vector3(5.0,10.0,15.0);
103  Gaussian::shared_ptr u(Unit::Create(3));
104  EXPECT(assert_equal(v,u->whiten(v)));
105 }
106 
107 /* ************************************************************************* */
108 TEST(NoiseModel, equals)
109 {
110  Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
111  g2 = Gaussian::SqrtInformation(I_3x3);
112  Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)),
113  d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
114  Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
115  i2 = Isotropic::Sigma(3, 0.7);
116 
117  EXPECT(assert_equal(*g1,*g1));
118  EXPECT(assert_inequal(*g1, *g2));
119 
120  EXPECT(assert_equal(*d1,*d1));
121  EXPECT(assert_inequal(*d1,*d2));
122 
123  EXPECT(assert_equal(*i1,*i1));
124  EXPECT(assert_inequal(*i1,*i2));
125 }
126 
127 // TODO enable test once a mechanism for smart constraints exists
129 //TEST(NoiseModel, ConstrainedSmart )
130 //{
131 // Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true);
132 // Diagonal::shared_ptr n1 = std::dynamic_pointer_cast<Diagonal>(nonconstrained);
133 // Constrained::shared_ptr n2 = std::dynamic_pointer_cast<Constrained>(nonconstrained);
134 // EXPECT(n1);
135 // EXPECT(!n2);
136 //
137 // Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
138 // Diagonal::shared_ptr c1 = std::dynamic_pointer_cast<Diagonal>(constrained);
139 // Constrained::shared_ptr c2 = std::dynamic_pointer_cast<Constrained>(constrained);
140 // EXPECT(c1);
141 // EXPECT(c2);
142 //}
143 
144 /* ************************************************************************* */
145 TEST(NoiseModel, ConstrainedConstructors )
146 {
148  size_t d = 3;
149  double m = 100.0;
150  Vector3 sigmas(kSigma, 0.0, 0.0);
151  Vector3 mu(200.0, 300.0, 400.0);
152  actual = Constrained::All(d);
153  // TODO: why should this be a thousand ??? Dummy variable?
154  EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu()));
155  EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas()));
156  EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value
157  EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value
158 
159  actual = Constrained::All(d, m);
160  EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
161 
162  actual = Constrained::All(d, mu);
163  EXPECT(assert_equal(mu, actual->mu()));
164 
165  actual = Constrained::MixedSigmas(mu, sigmas);
166  EXPECT(assert_equal(mu, actual->mu()));
167 
168  actual = Constrained::MixedSigmas(m, sigmas);
169  EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
170 }
171 
172 /* ************************************************************************* */
173 TEST(NoiseModel, ConstrainedMixed )
174 {
175  Vector feasible = Vector3(1.0, 0.0, 1.0),
176  infeasible = Vector3(1.0, 1.0, 1.0);
177  Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector3(kSigma, 0.0, kSigma));
178  // NOTE: we catch constrained variables elsewhere, so whitening does nothing
179  EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
180  EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
181 
182  DOUBLES_EQUAL(0.5 * (1000.0 + 0.25 + 0.25),d->loss(d->squaredMahalanobisDistance(infeasible)),1e-9);
183  DOUBLES_EQUAL(0.5, d->squaredMahalanobisDistance(feasible),1e-9);
184  DOUBLES_EQUAL(0.5 * 0.5, d->loss(0.5),1e-9);
185 }
186 
187 /* ************************************************************************* */
188 TEST(NoiseModel, ConstrainedAll )
189 {
190  Vector feasible = Vector3(0.0, 0.0, 0.0),
191  infeasible = Vector3(1.0, 1.0, 1.0);
192 
193  Constrained::shared_ptr i = Constrained::All(3);
194  // NOTE: we catch constrained variables elsewhere, so whitening does nothing
195  EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible)));
196  EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible)));
197 
198  DOUBLES_EQUAL(0.5 * 1000.0 * 3.0,i->loss(i->squaredMahalanobisDistance(infeasible)),1e-9);
199  DOUBLES_EQUAL(0.0, i->squaredMahalanobisDistance(feasible), 1e-9);
200  DOUBLES_EQUAL(0.0, i->loss(0.0),1e-9);
201 }
202 
203 /* ************************************************************************* */
204 namespace exampleQR {
205  // create a matrix to eliminate
206  Matrix Ab = (Matrix(4, 7) <<
207  -1., 0., 1., 0., 0., 0., -0.2,
208  0., -1., 0., 1., 0., 0., 0.3,
209  1., 0., 0., 0., -1., 0., 0.2,
210  0., 1., 0., 0., 0., -1., -0.1).finished();
211  Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished();
212 
213  // the matrix AB yields the following factorized version:
214  Matrix Rd = (Matrix(4, 7) <<
215  11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607,
216  0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525,
217  0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0,
218  0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427).finished();
219 
220  SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
221 }
222 
223 /* ************************************************************************* */
224 TEST( NoiseModel, QR )
225 {
226  Matrix Ab1 = exampleQR::Ab;
227  Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !
228 
229  // Call Gaussian version
230  SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1);
231  EXPECT(actual1->isUnit());
232  EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!!
233 
234  // Expected result for constrained version
235  Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
236  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
237  Matrix expectedRd2 = (Matrix(4, 7) <<
238  1., 0., -0.2, 0., -0.8, 0., 0.2,
239  0., 1., 0.,-0.2, 0., -0.8,-0.14,
240  0., 0., 1., 0., -1., 0., 0.0,
241  0., 0., 0., 1., 0., -1., 0.2).finished();
242 
243  // Call Constrained version
244  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas);
245  SharedDiagonal actual2 = constrained->QR(Ab2);
246  EXPECT(assert_equal(*expectedModel, *actual2, 1e-6));
247  EXPECT(linear_dependent(expectedRd2, Ab2, 1e-6)); // Ab was modified in place !!!
248 }
249 
250 /* ************************************************************************* */
251 TEST(NoiseModel, OverdeterminedQR) {
252  Matrix Ab1(9, 4);
253  Ab1 << 0, 1, 0, 0, //
254  0, 0, 1, 0, //
255  Matrix74::Ones();
256  Matrix Ab2 = Ab1; // otherwise overwritten !
257 
258  // Call Gaussian version
259  Vector9 sigmas = Vector9::Ones() ;
260  SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
261  SharedDiagonal actual1 = diagonal->QR(Ab1);
262  EXPECT(actual1->isUnit());
263  Matrix expectedRd(9,4);
264  expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, //
265  0.0, -1, 0, 0, //
266  0.0, 0.0, -1, 0, //
267  Matrix64::Zero();
268  EXPECT(assert_equal(expectedRd, Ab1, 1e-4)); // Ab was modified in place !!!
269 
270  // Expected result for constrained version
271  Vector3 expectedSigmas(0.377964473, 1, 1);
272  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
273 
274  // Call Constrained version
275  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
276  SharedDiagonal actual2 = constrained->QR(Ab2);
277  EXPECT(assert_equal(*expectedModel, *actual2, 1e-6));
278  expectedRd.row(0) *= 0.377964473; // not divided by sigma!
279  EXPECT(assert_equal(-expectedRd, Ab2, 1e-6)); // Ab was modified in place !!!
280 }
281 
282 /* ************************************************************************* */
283 TEST( NoiseModel, MixedQR )
284 {
285  // Call Constrained version, with first and third row treated as constraints
286  // Naming the 6 variables u,v,w,x,y,z, we have
287  // u = -z
288  // w = -x
289  // And let's have simple priors on variables
290  Matrix Ab(5,6+1);
291  Ab <<
292  1,0,0,0,0,1, 0, // u+z = 0
293  0,0,0,0,1,0, 0, // y^2
294  0,0,1,1,0,0, 0, // w+x = 0
295  0,1,0,0,0,0, 0, // v^2
296  0,0,0,0,0,1, 0; // z^2
297  Vector mixed_sigmas = (Vector(5) << 0, 1, 0, 1, 1).finished();
298  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(mixed_sigmas);
299 
300  // Expected result
301  Vector expectedSigmas = (Vector(5) << 0, 1, 0, 1, 1).finished();
302  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
303  Matrix expectedRd(5, 6+1);
304  expectedRd << 1, 0, 0, 0, 0, 1, 0, //
305  0, 1, 0, 0, 0, 0, 0, //
306  0, 0, 1, 1, 0, 0, 0, //
307  0, 0, 0, 0, 1, 0, 0, //
308  0, 0, 0, 0, 0, 1, 0; //
309 
310  SharedDiagonal actual = constrained->QR(Ab);
311  EXPECT(assert_equal(*expectedModel,*actual,1e-6));
312  EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
313 }
314 
315 /* ************************************************************************* */
316 TEST( NoiseModel, MixedQR2 )
317 {
318  // Let's have three variables x,y,z, but x=z and y=z
319  // Hence, all non-constraints are really measurements on z
320  Matrix Ab(11,3+1);
321  Ab <<
322  1,0,0, 0, //
323  0,1,0, 0, //
324  0,0,1, 0, //
325  -1,0,1, 0, // x=z
326  1,0,0, 0, //
327  0,1,0, 0, //
328  0,0,1, 0, //
329  0,-1,1, 0, // y=z
330  1,0,0, 0, //
331  0,1,0, 0, //
332  0,0,1, 0; //
333 
334  Vector sigmas(11);
335  sigmas.setOnes();
336  sigmas[3] = 0;
337  sigmas[7] = 0;
338  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
339 
340  // Expected result
341  Vector3 expectedSigmas(0,0,1.0/3);
342  SharedDiagonal expectedModel = noiseModel::Constrained::MixedSigmas(expectedSigmas);
343  Matrix expectedRd(11, 3+1);
344  expectedRd.setZero();
345  expectedRd.row(0) << -1, 0, 1, 0; // x=z
346  expectedRd.row(1) << 0, -1, 1, 0; // y=z
347  expectedRd.row(2) << 0, 0, 1, 0; // z=0 +/- 1/3
348 
349  SharedDiagonal actual = constrained->QR(Ab);
350  EXPECT(assert_equal(*expectedModel,*actual,1e-6));
351  EXPECT(assert_equal(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
352 }
353 
354 /* ************************************************************************* */
355 TEST( NoiseModel, FullyConstrained )
356 {
357  Matrix Ab(3,7);
358  Ab <<
359  1,0,0,0,0,1, 2, // u+z = 2
360  0,0,1,1,0,0, 4, // w+x = 4
361  0,1,0,1,1,1, 8; // v+x+y+z=8
362  SharedDiagonal constrained = noiseModel::Constrained::All(3);
363 
364  // Expected result
365  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(Vector3 (0,0,0));
366  Matrix expectedRd(3, 7);
367  expectedRd << 1, 0, 0, 0, 0, 1, 2, //
368  0, 1, 0, 1, 1, 1, 8, //
369  0, 0, 1, 1, 0, 0, 4; //
370 
371  SharedDiagonal actual = constrained->QR(Ab);
372  EXPECT(assert_equal(*expectedModel,*actual,1e-6));
373  EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
374 }
375 
376 /* ************************************************************************* */
377 // This matches constraint_eliminate2 in testJacobianFactor
378 TEST(NoiseModel, QRNan )
379 {
380  SharedDiagonal constrained = noiseModel::Constrained::All(2);
381  Matrix Ab = (Matrix25() << 2, 4, 2, 4, 6, 2, 1, 2, 4, 4).finished();
382 
383  SharedDiagonal expected = noiseModel::Constrained::All(2);
384  Matrix expectedAb = (Matrix25() << 1, 2, 1, 2, 3, 0, 1, 0, 0, 2.0/3).finished();
385 
386  SharedDiagonal actual = constrained->QR(Ab);
387  EXPECT(assert_equal(*expected,*actual));
388  EXPECT(linear_dependent(expectedAb,Ab));
389 }
390 
391 /* ************************************************************************* */
392 TEST(NoiseModel, SmartSqrtInformation )
393 {
394  bool smart = true;
395  gtsam::SharedGaussian expected = Unit::Create(3);
396  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart);
397  EXPECT(assert_equal(*expected,*actual));
398 }
399 
400 /* ************************************************************************* */
401 TEST(NoiseModel, SmartSqrtInformation2 )
402 {
403  bool smart = true;
404  gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
405  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart);
406  EXPECT(assert_equal(*expected,*actual));
407 }
408 
409 /* ************************************************************************* */
410 TEST(NoiseModel, SmartInformation )
411 {
412  bool smart = true;
413  gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
414  Matrix M = 0.5*I_3x3;
416  gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
417  EXPECT(assert_equal(*expected,*actual));
418 }
419 
420 /* ************************************************************************* */
421 TEST(NoiseModel, SmartCovariance )
422 {
423  bool smart = true;
424  gtsam::SharedGaussian expected = Unit::Create(3);
425  gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
426  EXPECT(assert_equal(*expected,*actual));
427 }
428 
429 /* ************************************************************************* */
430 TEST(NoiseModel, ScalarOrVector )
431 {
432  bool smart = true;
433  SharedGaussian expected = Unit::Create(3);
434  SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
435  EXPECT(assert_equal(*expected,*actual));
436 }
437 
438 /* ************************************************************************* */
439 TEST(NoiseModel, WhitenInPlace)
440 {
441  Vector sigmas = Vector3(0.1, 0.1, 0.1);
442  SharedDiagonal model = Diagonal::Sigmas(sigmas);
443  Matrix A = I_3x3;
444  model->WhitenInPlace(A);
445  Matrix expected = I_3x3 * 10;
446  EXPECT(assert_equal(expected, A));
447 }
448 
449 /* ************************************************************************* */
450 
451 /*
452  * These tests are responsible for testing the weight functions for the m-estimators in GTSAM.
453  * The weight function is related to the analytic derivative of the loss function. See
454  * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
455  * for details. This weight function is required when optimizing cost functions with robust
456  * penalties using iteratively re-weighted least squares.
457  */
458 
459 TEST(NoiseModel, robustFunctionFair)
460 {
461  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
462  const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k);
463  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8);
464  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8);
465  // Test negative value to ensure we take absolute value of error.
466  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
467  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
468 
469  DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8);
470  DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8);
471  DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8);
472  DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8);
473 }
474 
475 TEST(NoiseModel, robustFunctionHuber)
476 {
477  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
478  const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
479  DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8);
480  DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8);
481  // Test negative value to ensure we take absolute value of error.
482  DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
483  DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
484 
485  DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8);
486  DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8);
487  DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8);
488  DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8);
489 }
490 
491 TEST(NoiseModel, robustFunctionCauchy)
492 {
493  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
494  const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k);
495  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8);
496  DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8);
497  // Test negative value to ensure we take absolute value of error.
498  DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
499  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
500 
501  DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8);
502  DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8);
503  DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8);
504  DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8);
505 }
506 
507 TEST(NoiseModel, robustFunctionGemanMcClure)
508 {
509  const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
510  const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
511  DOUBLES_EQUAL(0.25 , gmc->weight(error1), 1e-8);
512  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8);
513  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
514  DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8);
515 
516  DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8);
517  DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8);
518  DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8);
519  DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8);
520 }
521 
522 TEST(NoiseModel, robustFunctionWelsch)
523 {
524  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
525  const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k);
526  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8);
527  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8);
528  // Test negative value to ensure we take absolute value of error.
529  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
530  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
531 
532  DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8);
533  DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8);
534  DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8);
535  DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8);
536 }
537 
538 TEST(NoiseModel, robustFunctionTukey)
539 {
540  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
541  const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k);
542  DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8);
543  DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8);
544  // Test negative value to ensure we take absolute value of error.
545  DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
546  DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
547 
548  DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8);
549  DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8);
550  DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8);
551  DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8);
552 }
553 
554 TEST(NoiseModel, robustFunctionDCS)
555 {
556  const double k = 1.0, error1 = 1.0, error2 = 10.0;
557  const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k);
558 
559  DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8);
560  DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);
561 
562  DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8);
563  DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8);
564 }
565 
566 TEST(NoiseModel, robustFunctionL2WithDeadZone)
567 {
568  const double k = 1.0, e0 = -10.0, e1 = -1.01, e2 = -0.99, e3 = 0.99, e4 = 1.01, e5 = 10.0;
569  const mEstimator::L2WithDeadZone::shared_ptr lsdz = mEstimator::L2WithDeadZone::Create(k);
570 
571  DOUBLES_EQUAL(0.9, lsdz->weight(e0), 1e-8);
572  DOUBLES_EQUAL(0.00990099009, lsdz->weight(e1), 1e-8);
573  DOUBLES_EQUAL(0.0, lsdz->weight(e2), 1e-8);
574  DOUBLES_EQUAL(0.0, lsdz->weight(e3), 1e-8);
575  DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8);
576  DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8);
577 
578  DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8);
579  DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8);
580  DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8);
581  DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8);
582  DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8);
583  DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8);
584 }
585 
586 /* ************************************************************************* */
587 TEST(NoiseModel, robustNoiseHuber)
588 {
589  const double k = 10.0, error1 = 1.0, error2 = 100.0;
590  Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished();
591  Vector b = Vector2(error1, error2);
592  const Robust::shared_ptr robust = Robust::Create(
593  mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
594  Unit::Create(2));
595 
596  robust->WhitenSystem(A, b);
597 
598  DOUBLES_EQUAL(error1, b(0), 1e-8);
599  DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8);
600 
601  DOUBLES_EQUAL(1.0, A(0,0), 1e-8);
602  DOUBLES_EQUAL(10.0, A(0,1), 1e-8);
603  DOUBLES_EQUAL(sqrt(k*100.0), A(1,0), 1e-8);
604  DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8);
605 }
606 
607 TEST(NoiseModel, robustNoiseGemanMcClure)
608 {
609  const double k = 1.0, error1 = 1.0, error2 = 100.0;
610  const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
611  Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
612  Vector b = Vector2(error1, error2);
613  const Robust::shared_ptr robust = Robust::Create(
614  mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar),
615  Unit::Create(2));
616 
617  robust->WhitenSystem(A, b);
618 
619  const double k2 = k*k;
620  const double k4 = k2*k2;
621  const double k2error = k2 + error2*error2;
622 
623  const double sqrt_weight_error1 = sqrt(0.25);
624  const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error));
625 
626  DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8);
627  DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8);
628 
629  DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8);
630  DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8);
631  DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8);
632  DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8);
633 }
634 
635 TEST(NoiseModel, robustNoiseDCS)
636 {
637  const double k = 1.0, error1 = 1.0, error2 = 100.0;
638  const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
639  Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
640  Vector b = Vector2(error1, error2);
641  const Robust::shared_ptr robust = Robust::Create(
642  mEstimator::DCS::Create(k, mEstimator::DCS::Scalar),
643  Unit::Create(2));
644 
645  robust->WhitenSystem(A, b);
646 
647  const double sqrt_weight = 2.0*k/(k + error2*error2);
648 
649  DOUBLES_EQUAL(error1, b(0), 1e-8);
650  DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8);
651 
652  DOUBLES_EQUAL(a00, A(0,0), 1e-8);
653  DOUBLES_EQUAL(a01, A(0,1), 1e-8);
654  DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8);
655  DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8);
656 }
657 
658 TEST(NoiseModel, robustNoiseL2WithDeadZone)
659 {
660  double dead_zone_size = 1.0;
661  auto robust = noiseModel::Robust::Create(
662  noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
663  Unit::Create(3));
664 
665  for (int i = 0; i < 5; i++) {
666  Vector error = Vector3(i, 0, 0);
667  robust->WhitenSystem(error);
668  DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
669  robust->squaredMahalanobisDistance(error), 1e-8);
670  }
671 }
672 
673 TEST(NoiseModel, lossFunctionAtZero)
674 {
675  const double k = 5.0;
676  auto fair = mEstimator::Fair::Create(k);
677  DOUBLES_EQUAL(fair->loss(0), 0, 1e-8);
678  DOUBLES_EQUAL(fair->weight(0), 1, 1e-8);
679  auto huber = mEstimator::Huber::Create(k);
680  DOUBLES_EQUAL(huber->loss(0), 0, 1e-8);
681  DOUBLES_EQUAL(huber->weight(0), 1, 1e-8);
682  auto cauchy = mEstimator::Cauchy::Create(k);
683  DOUBLES_EQUAL(cauchy->loss(0), 0, 1e-8);
684  DOUBLES_EQUAL(cauchy->weight(0), 1, 1e-8);
685  auto gmc = mEstimator::GemanMcClure::Create(k);
686  DOUBLES_EQUAL(gmc->loss(0), 0, 1e-8);
687  DOUBLES_EQUAL(gmc->weight(0), 1, 1e-8);
688  auto welsch = mEstimator::Welsch::Create(k);
689  DOUBLES_EQUAL(welsch->loss(0), 0, 1e-8);
690  DOUBLES_EQUAL(welsch->weight(0), 1, 1e-8);
691  auto tukey = mEstimator::Tukey::Create(k);
692  DOUBLES_EQUAL(tukey->loss(0), 0, 1e-8);
693  DOUBLES_EQUAL(tukey->weight(0), 1, 1e-8);
694  auto dcs = mEstimator::DCS::Create(k);
695  DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
696  DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
697  auto lsdz = mEstimator::L2WithDeadZone::Create(k);
698  DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
699  DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8);
700 }
701 
702 
703 /* ************************************************************************* */
704 #define TEST_GAUSSIAN(gaussian)\
705  EQUALITY(info, gaussian->information());\
706  EQUALITY(cov, gaussian->covariance());\
707  EXPECT(assert_equal(white, gaussian->whiten(e)));\
708  EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
709  EXPECT_DOUBLES_EQUAL(251.0, gaussian->squaredMahalanobisDistance(e), 1e-9);\
710  EXPECT_DOUBLES_EQUAL(0.5 * 251.0, gaussian->loss(251.0), 1e-9);\
711  Matrix A = R.inverse(); Vector b = e;\
712  gaussian->WhitenSystem(A, b);\
713  EXPECT(assert_equal(I, A));\
714  EXPECT(assert_equal(white, b));
715 
716 TEST(NoiseModel, NonDiagonalGaussian)
717 {
718  Matrix3 R;
719  R << 6, 5, 4, 0, 3, 2, 0, 0, 1;
720  const Matrix3 info = R.transpose() * R;
721  const Matrix3 cov = info.inverse();
722  const Vector3 e(1, 1, 1), white = R * e;
723  Matrix I = I_3x3;
724 
725 
726  {
727  SharedGaussian gaussian = Gaussian::SqrtInformation(R);
728  TEST_GAUSSIAN(gaussian);
729  }
730 
731  {
732  SharedGaussian gaussian = Gaussian::Information(info);
733  TEST_GAUSSIAN(gaussian);
734  }
735 
736  {
737  SharedGaussian gaussian = Gaussian::Covariance(cov);
738  TEST_GAUSSIAN(gaussian);
739  }
740 }
741 
742 /* ************************************************************************* */
743 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
744 /* ************************************************************************* */
Matrix3f m
Provides additional testing facilities for common data structures.
std::shared_ptr< L2WithDeadZone > shared_ptr
SCALAR Scalar
Definition: bench_gemm.cpp:46
#define I
Definition: main.h:112
std::shared_ptr< DCS > shared_ptr
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
static const Matrix kCovariance
Scalar * b
Definition: benchVecAdd.cpp:17
std::shared_ptr< Tukey > shared_ptr
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:971
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
double mu
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Double_ distance(const OrientedPlane3_ &p)
Definition: BFloat16.h:88
TEST(NoiseModel, constructors)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
static const double kVariance
static const double prc
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmax(const bfloat16 &a, const bfloat16 &b)
Definition: BFloat16.h:587
static const double kSigma
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:114
static const Matrix R
else if n * info
std::shared_ptr< Constrained > shared_ptr
Definition: NoiseModel.h:405
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
static const Vector3 kSigmas(kSigma, kSigma, kSigma)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define TEST_GAUSSIAN(gaussian)
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:542
std::shared_ptr< Welsch > shared_ptr
SharedDiagonal diagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
std::shared_ptr< Fair > shared_ptr
std::optional< Vector > checkIfDiagonal(const Matrix &M)
Definition: NoiseModel.cpp:47
std::shared_ptr< GemanMcClure > shared_ptr
std::shared_ptr< Robust > shared_ptr
Definition: NoiseModel.h:659
Eigen::Vector2d Vector2
Definition: Vector.h:42
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
std::shared_ptr< Huber > shared_ptr
static double error
Definition: testRot3.cpp:37
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
static const double kInverseSigma
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:189
std::shared_ptr< Cauchy > shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
Pose3 g2(g1.expmap(h *V1_g1))
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
int main()
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:742


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:46