25 #include <boost/assign/std/vector.hpp> 31 using namespace gtsam;
32 using namespace noiseModel;
42 TEST(NoiseModel, constructors)
48 vector<Gaussian::shared_ptr>
m;
49 m.push_back(Gaussian::SqrtInformation(
R,
false));
50 m.push_back(Gaussian::Covariance(
kCovariance,
false));
51 m.push_back(Gaussian::Information(
kCovariance.inverse(),
false));
52 m.push_back(Diagonal::Sigmas(
kSigmas,
false));
55 m.push_back(Isotropic::Sigma(3,
kSigma,
false));
56 m.push_back(Isotropic::Variance(3,
kVariance,
false));
57 m.push_back(Isotropic::Precision(3,
prc,
false));
72 double distance = 5*5+10*10+15*15;
74 DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1
e-9);
92 1.0, 0.0, 0.0, 1.0).finished());
98 m[0]->WhitenInPlace(H);
114 g2 = Gaussian::SqrtInformation(I_3x3);
116 d2 = Diagonal::Sigmas(
Vector3(0.1, 0.2, 0.3));
118 i2 = Isotropic::Sigma(3, 0.7);
148 TEST(NoiseModel, ConstrainedConstructors )
155 actual = Constrained::All(d);
162 actual = Constrained::All(d, m);
165 actual = Constrained::All(d, mu);
168 actual = Constrained::MixedSigmas(mu, sigmas);
171 actual = Constrained::MixedSigmas(m, sigmas);
176 TEST(NoiseModel, ConstrainedMixed )
179 infeasible =
Vector3(1.0, 1.0, 1.0);
185 DOUBLES_EQUAL(0.5 * (1000.0 + 0.25 + 0.25),d->loss(d->squaredMahalanobisDistance(infeasible)),1
e-9);
191 TEST(NoiseModel, ConstrainedAll )
194 infeasible =
Vector3(1.0, 1.0, 1.0);
201 DOUBLES_EQUAL(0.5 * 1000.0 * 3.0,i->loss(i->squaredMahalanobisDistance(infeasible)),1
e-9);
202 DOUBLES_EQUAL(0.0, i->squaredMahalanobisDistance(feasible), 1
e-9);
210 -1., 0., 1., 0., 0., 0., -0.2,
211 0., -1., 0., 1., 0., 0., 0.3,
212 1., 0., 0., 0., -1., 0., 0.2,
213 0., 1., 0., 0., 0., -1., -0.1).finished();
218 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607,
219 0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525,
220 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0,
221 0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427).finished();
234 EXPECT(actual1->isUnit());
238 Vector expectedSigmas = (
Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
239 SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
241 1., 0., -0.2, 0., -0.8, 0., 0.2,
242 0., 1., 0.,-0.2, 0., -0.8,-0.14,
243 0., 0., 1., 0., -1., 0., 0.0,
244 0., 0., 0., 1., 0., -1., 0.2).finished();
254 TEST(NoiseModel, OverdeterminedQR) {
262 Vector9 sigmas = Vector9::Ones() ;
265 EXPECT(actual1->isUnit());
267 expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131,
274 Vector3 expectedSigmas(0.377964473, 1, 1);
275 SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
278 SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
281 expectedRd.row(0) *= 0.377964473;
300 Vector mixed_sigmas = (
Vector(5) << 0, 1, 0, 1, 1).finished();
301 SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(mixed_sigmas);
304 Vector expectedSigmas = (
Vector(5) << 0, 1, 0, 1, 1).finished();
305 SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
306 Matrix expectedRd(5, 6+1);
307 expectedRd << 1, 0, 0, 0, 0, 1, 0,
341 SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
344 Vector3 expectedSigmas(0,0,1.0/3);
345 SharedDiagonal expectedModel = noiseModel::Constrained::MixedSigmas(expectedSigmas);
346 Matrix expectedRd(11, 3+1);
347 expectedRd.setZero();
348 expectedRd.row(0) << -1, 0, 1, 0;
349 expectedRd.row(1) << 0, -1, 1, 0;
350 expectedRd.row(2) << 0, 0, 1, 0;
358 TEST( NoiseModel, FullyConstrained )
370 expectedRd << 1, 0, 0, 0, 0, 1, 2,
384 Matrix Ab = (Matrix25() << 2, 4, 2, 4, 6, 2, 1, 2, 4, 4).finished();
387 Matrix expectedAb = (Matrix25() << 1, 2, 1, 2, 3, 0, 1, 0, 0, 2.0/3).finished();
395 TEST(NoiseModel, SmartSqrtInformation )
404 TEST(NoiseModel, SmartSqrtInformation2 )
413 TEST(NoiseModel, SmartInformation )
424 TEST(NoiseModel, SmartCovariance )
433 TEST(NoiseModel, ScalarOrVector )
442 TEST(NoiseModel, WhitenInPlace)
447 model->WhitenInPlace(A);
462 TEST(NoiseModel, robustFunctionFair)
464 const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
478 TEST(NoiseModel, robustFunctionHuber)
480 const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
494 TEST(NoiseModel, robustFunctionCauchy)
496 const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
510 TEST(NoiseModel, robustFunctionGemanMcClure)
512 const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
525 TEST(NoiseModel, robustFunctionWelsch)
527 const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
541 TEST(NoiseModel, robustFunctionTukey)
543 const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
557 TEST(NoiseModel, robustFunctionDCS)
559 const double k = 1.0, error1 = 1.0, error2 = 10.0;
569 TEST(NoiseModel, robustFunctionL2WithDeadZone)
571 const double k = 1.0, e0 = -10.0, e1 = -1.01, e2 = -0.99, e3 = 0.99, e4 = 1.01, e5 = 10.0;
590 TEST(NoiseModel, robustNoiseHuber)
592 const double k = 10.0, error1 = 1.0, error2 = 100.0;
593 Matrix A = (
Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished();
599 robust->WhitenSystem(A, b);
610 TEST(NoiseModel, robustNoiseGemanMcClure)
612 const double k = 1.0, error1 = 1.0, error2 = 100.0;
613 const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
620 robust->WhitenSystem(A, b);
622 const double k2 = k*k;
623 const double k4 = k2*k2;
624 const double k2error = k2 + error2*error2;
626 const double sqrt_weight_error1 =
sqrt(0.25);
627 const double sqrt_weight_error2 =
sqrt(k4/(k2error*k2error));
638 TEST(NoiseModel, robustNoiseDCS)
640 const double k = 1.0, error1 = 1.0, error2 = 100.0;
641 const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
648 robust->WhitenSystem(A, b);
650 const double sqrt_weight = 2.0*k/(k + error2*error2);
661 TEST(NoiseModel, robustNoiseL2WithDeadZone)
663 double dead_zone_size = 1.0;
665 noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
686 TEST(NoiseModel, lossFunctionAtZero)
688 const double k = 5.0;
689 auto fair = mEstimator::Fair::Create(k);
692 auto huber = mEstimator::Huber::Create(k);
695 auto cauchy = mEstimator::Cauchy::Create(k);
698 auto gmc = mEstimator::GemanMcClure::Create(k);
701 auto welsch = mEstimator::Welsch::Create(k);
704 auto tukey = mEstimator::Tukey::Create(k);
707 auto dcs = mEstimator::DCS::Create(k);
717 #define TEST_GAUSSIAN(gaussian)\ 718 EQUALITY(info, gaussian->information());\ 719 EQUALITY(cov, gaussian->covariance());\ 720 EXPECT(assert_equal(white, gaussian->whiten(e)));\ 721 EXPECT(assert_equal(e, gaussian->unwhiten(white)));\ 722 EXPECT_DOUBLES_EQUAL(251.0, gaussian->squaredMahalanobisDistance(e), 1e-9);\ 723 EXPECT_DOUBLES_EQUAL(0.5 * 251.0, gaussian->loss(251.0), 1e-9);\ 724 Matrix A = R.inverse(); Vector b = e;\ 725 gaussian->WhitenSystem(A, b);\ 726 EXPECT(assert_equal(I, A));\ 727 EXPECT(assert_equal(white, b)); 729 TEST(NoiseModel, NonDiagonalGaussian)
732 R << 6, 5, 4, 0, 3, 2, 0, 0, 1;
733 const Matrix3
info = R.transpose() *
R;
734 const Matrix3 cov = info.inverse();
736 Matrix I = Matrix3::Identity();
boost::shared_ptr< Huber > shared_ptr
Provides additional testing facilities for common data structures.
Matrix< RealScalar, Dynamic, Dynamic > M
static const Matrix kCovariance
static int runAllTests(TestResult &result)
noiseModel::Diagonal::shared_ptr model
boost::shared_ptr< Robust > shared_ptr
#define DOUBLES_EQUAL(expected, actual, threshold)
void diagonal(const MatrixType &m)
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
TEST(NoiseModel, constructors)
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
boost::shared_ptr< L2WithDeadZone > shared_ptr
static const double kSigma
boost::shared_ptr< GemanMcClure > shared_ptr
#define EXPECT(condition)
static const Vector3 kSigmas(kSigma, kSigma, kSigma)
boost::shared_ptr< Tukey > shared_ptr
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define TEST_GAUSSIAN(gaussian)
boost::shared_ptr< Cauchy > shared_ptr
boost::shared_ptr< Diagonal > shared_ptr
boost::shared_ptr< Constrained > shared_ptr
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
boost::shared_ptr< DCS > shared_ptr
boost::optional< Vector > checkIfDiagonal(const Matrix M)
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
boost::shared_ptr< Isotropic > shared_ptr
boost::shared_ptr< Gaussian > shared_ptr
static const double kInverseSigma
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
boost::shared_ptr< Fair > shared_ptr
Pose3 g2(g1.expmap(h *V1_g1))
boost::shared_ptr< Welsch > shared_ptr
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
noiseModel::Gaussian::shared_ptr SharedGaussian
noiseModel::Base::shared_ptr SharedNoiseModel