26 #define GTSAM_MAGIC_GAUSSIAN 2
37 using namespace gtsam;
72 NonlinearFactorGraph::sharedFactor
f0 = fg[0],
f1 = fg[1];
89 NonlinearFactorGraph::sharedFactor factor = fg[0];
93 Vector actual_e = std::dynamic_pointer_cast<NoiseModelFactor>(factor)->unwhitenedError(cfg);
100 double actual = factor->error(cfg);
116 auto noise = noiseModel::Isotropic::Sigma(2, 0.2);
125 auto gaussian = noiseModel::Isotropic::Sigma(2, 0.2);
130 vector<noiseModel::Robust::shared_ptr> robust_models;
133 auto fair = noiseModel::Robust::Create(
134 noiseModel::mEstimator::Fair::Create(1.3998), gaussian);
135 robust_models.push_back(fair);
138 auto huber = noiseModel::Robust::Create(
139 noiseModel::mEstimator::Huber::Create(1.345), gaussian);
140 robust_models.push_back(huber);
143 auto cauchy = noiseModel::Robust::Create(
144 noiseModel::mEstimator::Cauchy::Create(0.1), gaussian);
145 robust_models.push_back(cauchy);
148 auto tukey = noiseModel::Robust::Create(
149 noiseModel::mEstimator::Tukey::Create(4.6851), gaussian);
150 robust_models.push_back(tukey);
153 auto welsch = noiseModel::Robust::Create(
154 noiseModel::mEstimator::Welsch::Create(2.9846), gaussian);
155 robust_models.push_back(welsch);
158 auto gm = noiseModel::Robust::Create(
159 noiseModel::mEstimator::GemanMcClure::Create(1.0), gaussian);
160 robust_models.push_back(gm);
163 auto dcs = noiseModel::Robust::Create(
164 noiseModel::mEstimator::DCS::Create(1.0), gaussian);
165 robust_models.push_back(dcs);
168 auto l2 = noiseModel::Robust::Create(
169 noiseModel::mEstimator::L2WithDeadZone::Create(1.0), gaussian);
170 robust_models.push_back(
l2);
172 for(
auto&&
model: robust_models) {
187 NonlinearFactorGraph::sharedFactor nlf = nfg[0];
209 NonlinearFactorGraph::sharedFactor nlf = nfg[1];
225 NonlinearFactorGraph::sharedFactor nlf = nfg[2];
242 NonlinearFactorGraph::sharedFactor nlf = nfg[3];
264 NonlinearFactorGraph::sharedFactor
factor1 = fg[0],
factor2 = fg[1],
287 noiseModel::Constrained::MixedSigmas(
Vector2(1,0)));
305 Matrix2
A;
A << 5.0, 0.0, 0.0, 1.0;
308 noiseModel::Constrained::MixedSigmas(
Vector2(1,0)));
335 static_assert(std::is_same<
This, NoiseModelFactor1<double>>::
value,
339 typedef NoiseModelFactor1<double>
Base;
342 using Base::evaluateError;
350 if (H1) *H1 = (
Matrix(1, 1) << 1.0).finished();
351 return (
Vector(1) <<
x1).finished();
355 return std::static_pointer_cast<gtsam::NonlinearFactor>(
364 tv.
insert(
L(1),
double((1.0)));
368 *std::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
378 std::vector<Matrix>
H = {
Matrix()};
392 NoiseModelFactor4<double, double, double, double>>::
value,
396 typedef NoiseModelFactor4<double, double, double, double>
Base;
399 using Base::evaluateError;
411 *H1 = (
Matrix(1, 1) << 1.0).finished();
412 *H2 = (
Matrix(1, 1) << 2.0).finished();
413 *H3 = (
Matrix(1, 1) << 3.0).finished();
414 *H4 = (
Matrix(1, 1) << 4.0).finished();
416 return (
Vector(1) <<
x1 + 2.0 *
x2 + 3.0 *
x3 + 4.0 *
x4).finished();
420 return std::static_pointer_cast<gtsam::NonlinearFactor>(
428 tv.
insert(
X(1),
double((1.0)));
429 tv.
insert(
X(2),
double((2.0)));
430 tv.
insert(
X(3),
double((3.0)));
431 tv.
insert(
X(4),
double((4.0)));
434 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
447 "X1 type incorrect");
449 "X2 type incorrect");
451 "X3 type incorrect");
453 "X4 type incorrect");
466 static_assert(std::is_same<TestFactor4::ValueType<1>,
double>::
value,
467 "ValueType<1> type incorrect");
468 static_assert(std::is_same<TestFactor4::ValueType<2>,
double>::
value,
469 "ValueType<2> type incorrect");
470 static_assert(std::is_same<TestFactor4::ValueType<3>,
double>::
value,
471 "ValueType<3> type incorrect");
472 static_assert(std::is_same<TestFactor4::ValueType<4>,
double>::
value,
473 "ValueType<4> type incorrect");
481 TestFactor4 tf3(noiseModel::Unit::Create(1), {
L(1),
L(2),
L(3),
L(4)});
483 std::array<Key, 4>{
L(1),
L(2),
L(3),
L(4)});
484 std::vector<Key>
keys = {
L(1),
L(2),
L(3),
L(4)};
491 typedef NoiseModelFactor5<double, double, double, double, double>
Base;
494 using Base::evaluateError;
503 *H1 = (
Matrix(1, 1) << 1.0).finished();
504 *H2 = (
Matrix(1, 1) << 2.0).finished();
505 *H3 = (
Matrix(1, 1) << 3.0).finished();
506 *H4 = (
Matrix(1, 1) << 4.0).finished();
507 *H5 = (
Matrix(1, 1) << 5.0).finished();
518 tv.
insert(
X(1),
double((1.0)));
519 tv.
insert(
X(2),
double((2.0)));
520 tv.
insert(
X(3),
double((3.0)));
521 tv.
insert(
X(4),
double((4.0)));
522 tv.
insert(
X(5),
double((5.0)));
525 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
542 typedef NoiseModelFactor6<double, double, double, double, double, double>
Base;
545 using Base::evaluateError;
554 *H1 = (
Matrix(1, 1) << 1.0).finished();
555 *H2 = (
Matrix(1, 1) << 2.0).finished();
556 *H3 = (
Matrix(1, 1) << 3.0).finished();
557 *H4 = (
Matrix(1, 1) << 4.0).finished();
558 *H5 = (
Matrix(1, 1) << 5.0).finished();
559 *H6 = (
Matrix(1, 1) << 6.0).finished();
572 tv.
insert(
X(1),
double((1.0)));
573 tv.
insert(
X(2),
double((2.0)));
574 tv.
insert(
X(3),
double((3.0)));
575 tv.
insert(
X(4),
double((4.0)));
576 tv.
insert(
X(5),
double((5.0)));
577 tv.
insert(
X(6),
double((6.0)));
580 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
603 using Base::evaluateError;
613 if (H1) *H1 = (
Matrix(1, 1) << 1.0).finished();
614 if (H2) *H2 = (
Matrix(1, 1) << 2.0).finished();
615 if (H3) *H3 = (
Matrix(1, 1) << 3.0).finished();
616 if (H4) *H4 = (
Matrix(1, 1) << 4.0).finished();
617 return (
Vector(1) <<
x1 + 2.0 *
x2 + 3.0 *
x3 + 4.0 *
x4).finished();
627 tv.
insert(
X(1),
double((1.0)));
628 tv.
insert(
X(2),
double((2.0)));
629 tv.
insert(
X(3),
double((3.0)));
630 tv.
insert(
X(4),
double((4.0)));
645 Matrix H1_expected, H2_expected, H3_expected, H4_expected;
647 H3_expected, H4_expected);
649 std::unique_ptr<NoiseModelFactorN<double, double, double, double>> base_ptr(
656 base_ptr->evaluateError(9, 8, 7, 6, H1, H2)));
660 base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3)));
665 base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3, H4)));
674 "X1 type incorrect");
680 "ValueType<1> type incorrect");
682 "ValueType<2> type incorrect");
684 "ValueType<3> type incorrect");
686 "ValueType<4> type incorrect");
688 "TestFactorN::Type1 type incorrect");