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


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:09:46