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 testNonlinearFactor.cpp
14 * @brief Unit tests for Non-Linear Factor,
15 * create a non linear factor graph and a values structure for it and
16 * calculate the error for the factor.
17 * @author Christian Potthast
18 **/
19
20/*STL/C++*/
21#include <iostream>
22
23#include <CppUnitLite/TestHarness.h>
24
25// TODO: DANGEROUS, create shared pointers
26#define GTSAM_MAGIC_GAUSSIAN 2
27
28#include <gtsam/base/Testable.h>
29#include <gtsam/base/Matrix.h>
30#include <tests/smallExample.h>
31#include <tests/simulated2D.h>
32#include <gtsam/linear/GaussianFactor.h>
33#include <gtsam/nonlinear/NonlinearFactorGraph.h>
34#include <gtsam/inference/Symbol.h>
35
36using namespace std;
37using namespace gtsam;
38using namespace example;
39
40// Convenience for named keys
41using symbol_shorthand::X;
42using symbol_shorthand::L;
43
44typedef std::shared_ptr<NonlinearFactor > shared_nlf;
45
46/* ************************************************************************* */
47TEST( NonlinearFactor, equals )
48{
49 SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(dim: 2,sigma: 1.0));
50
51 // create two nonlinear2 factors
52 Point2 z3(0.,-1.);
53 simulated2D::Measurement f0(z3, sigma, X(j: 1),L(j: 1));
54
55 // measurement between x2 and l1
56 Point2 z4(-1.5, -1.);
57 simulated2D::Measurement f1(z4, sigma, X(j: 2),L(j: 1));
58
59 CHECK(assert_equal(f0,f0));
60 CHECK(f0.equals(f0));
61 CHECK(!f0.equals(f1));
62 CHECK(!f1.equals(f0));
63}
64
65/* ************************************************************************* */
66TEST( NonlinearFactor, equals2 )
67{
68 // create a non linear factor graph
69 NonlinearFactorGraph fg = createNonlinearFactorGraph();
70
71 // get two factors
72 NonlinearFactorGraph::sharedFactor f0 = fg[0], f1 = fg[1];
73
74 CHECK(f0->equals(*f0));
75 CHECK(!f0->equals(*f1));
76 CHECK(!f1->equals(*f0));
77}
78
79/* ************************************************************************* */
80TEST( NonlinearFactor, NonlinearFactor )
81{
82 // create a non linear factor graph
83 NonlinearFactorGraph fg = createNonlinearFactorGraph();
84
85 // create a values structure for the non linear factor graph
86 Values cfg = createNoisyValues();
87
88 // get the factor "f1" from the factor graph
89 NonlinearFactorGraph::sharedFactor factor = fg[0];
90
91 // calculate the error_vector from the factor "f1"
92 // error_vector = [0.1 0.1]
93 Vector actual_e = std::dynamic_pointer_cast<NoiseModelFactor>(r: factor)->unwhitenedError(x: cfg);
94 CHECK(assert_equal(0.1*Vector::Ones(2),actual_e));
95
96 // error = 0.5 * [1 1] * [1;1] = 1
97 double expected = 1.0;
98
99 // calculate the error from the factor "f1"
100 double actual = factor->error(c: cfg);
101 DOUBLES_EQUAL(expected,actual,0.00000001);
102}
103
104/* ************************************************************************* */
105TEST(NonlinearFactor, Weight) {
106 // create a values structure for the non linear factor graph
107 Values values;
108
109 // Instantiate a concrete class version of a NoiseModelFactor
110 PriorFactor<Point2> factor1(X(j: 1), Point2(0, 0));
111 values.insert(j: X(j: 1), val: Point2(0.1, 0.1));
112
113 CHECK(assert_equal(1.0, factor1.weight(values)));
114
115 // Factor with noise model
116 auto noise = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.2);
117 PriorFactor<Point2> factor2(X(j: 2), Point2(1, 1), noise);
118 values.insert(j: X(j: 2), val: Point2(1.1, 1.1));
119
120 CHECK(assert_equal(1.0, factor2.weight(values)));
121
122 Point2 estimate(3, 3), prior(1, 1);
123 double distance = (estimate - prior).norm();
124
125 auto gaussian = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.2);
126
127 PriorFactor<Point2> factor;
128
129 // vector to store all the robust models in so we can test iteratively.
130 vector<noiseModel::Robust::shared_ptr> robust_models;
131
132 // Fair noise model
133 auto fair = noiseModel::Robust::Create(
134 robust: noiseModel::mEstimator::Fair::Create(c: 1.3998), noise: gaussian);
135 robust_models.push_back(x: fair);
136
137 // Huber noise model
138 auto huber = noiseModel::Robust::Create(
139 robust: noiseModel::mEstimator::Huber::Create(k: 1.345), noise: gaussian);
140 robust_models.push_back(x: huber);
141
142 // Cauchy noise model
143 auto cauchy = noiseModel::Robust::Create(
144 robust: noiseModel::mEstimator::Cauchy::Create(k: 0.1), noise: gaussian);
145 robust_models.push_back(x: cauchy);
146
147 // Tukey noise model
148 auto tukey = noiseModel::Robust::Create(
149 robust: noiseModel::mEstimator::Tukey::Create(k: 4.6851), noise: gaussian);
150 robust_models.push_back(x: tukey);
151
152 // Welsch noise model
153 auto welsch = noiseModel::Robust::Create(
154 robust: noiseModel::mEstimator::Welsch::Create(k: 2.9846), noise: gaussian);
155 robust_models.push_back(x: welsch);
156
157 // Geman-McClure noise model
158 auto gm = noiseModel::Robust::Create(
159 robust: noiseModel::mEstimator::GemanMcClure::Create(k: 1.0), noise: gaussian);
160 robust_models.push_back(x: gm);
161
162 // DCS noise model
163 auto dcs = noiseModel::Robust::Create(
164 robust: noiseModel::mEstimator::DCS::Create(k: 1.0), noise: gaussian);
165 robust_models.push_back(x: dcs);
166
167 // L2WithDeadZone noise model
168 auto l2 = noiseModel::Robust::Create(
169 robust: noiseModel::mEstimator::L2WithDeadZone::Create(k: 1.0), noise: gaussian);
170 robust_models.push_back(x: l2);
171
172 for(auto&& model: robust_models) {
173 factor = PriorFactor<Point2>(X(j: 3), prior, model);
174 values.clear();
175 values.insert(j: X(j: 3), val: estimate);
176 CHECK(assert_equal(model->robust()->weight(distance), factor.weight(values)));
177 }
178}
179
180/* ************************************************************************* */
181TEST( NonlinearFactor, linearize_f1 )
182{
183 Values c = createNoisyValues();
184
185 // Grab a non-linear factor
186 NonlinearFactorGraph nfg = createNonlinearFactorGraph();
187 NonlinearFactorGraph::sharedFactor nlf = nfg[0];
188
189 // We linearize at noisy config from SmallExample
190 GaussianFactor::shared_ptr actual = nlf->linearize(c);
191
192 GaussianFactorGraph lfg = createGaussianFactorGraph();
193 GaussianFactor::shared_ptr expected = lfg[0];
194
195 CHECK(assert_equal(*expected,*actual));
196
197 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
198 // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
199 //CHECK(assert_equal(nlf->error_vector(c),actual->get_b()));
200}
201
202/* ************************************************************************* */
203TEST( NonlinearFactor, linearize_f2 )
204{
205 Values c = createNoisyValues();
206
207 // Grab a non-linear factor
208 NonlinearFactorGraph nfg = createNonlinearFactorGraph();
209 NonlinearFactorGraph::sharedFactor nlf = nfg[1];
210
211 // We linearize at noisy config from SmallExample
212 GaussianFactor::shared_ptr actual = nlf->linearize(c);
213
214 GaussianFactorGraph lfg = createGaussianFactorGraph();
215 GaussianFactor::shared_ptr expected = lfg[1];
216
217 CHECK(assert_equal(*expected,*actual));
218}
219
220/* ************************************************************************* */
221TEST( NonlinearFactor, linearize_f3 )
222{
223 // Grab a non-linear factor
224 NonlinearFactorGraph nfg = createNonlinearFactorGraph();
225 NonlinearFactorGraph::sharedFactor nlf = nfg[2];
226
227 // We linearize at noisy config from SmallExample
228 Values c = createNoisyValues();
229 GaussianFactor::shared_ptr actual = nlf->linearize(c);
230
231 GaussianFactorGraph lfg = createGaussianFactorGraph();
232 GaussianFactor::shared_ptr expected = lfg[2];
233
234 CHECK(assert_equal(*expected,*actual));
235}
236
237/* ************************************************************************* */
238TEST( NonlinearFactor, linearize_f4 )
239{
240 // Grab a non-linear factor
241 NonlinearFactorGraph nfg = createNonlinearFactorGraph();
242 NonlinearFactorGraph::sharedFactor nlf = nfg[3];
243
244 // We linearize at noisy config from SmallExample
245 Values c = createNoisyValues();
246 GaussianFactor::shared_ptr actual = nlf->linearize(c);
247
248 GaussianFactorGraph lfg = createGaussianFactorGraph();
249 GaussianFactor::shared_ptr expected = lfg[3];
250
251 CHECK(assert_equal(*expected,*actual));
252}
253
254/* ************************************************************************* */
255TEST( NonlinearFactor, size )
256{
257 // create a non linear factor graph
258 NonlinearFactorGraph fg = createNonlinearFactorGraph();
259
260 // create a values structure for the non linear factor graph
261 Values cfg = createNoisyValues();
262
263 // get some factors from the graph
264 NonlinearFactorGraph::sharedFactor factor1 = fg[0], factor2 = fg[1],
265 factor3 = fg[2];
266
267 CHECK(factor1->size() == 1);
268 CHECK(factor2->size() == 2);
269 CHECK(factor3->size() == 2);
270}
271
272/* ************************************************************************* */
273TEST( NonlinearFactor, linearize_constraint1 )
274{
275 SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas: Vector2(0.2,0));
276
277 Point2 mu(1., -1.);
278 NonlinearFactorGraph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(j: 1)));
279
280 Values config;
281 config.insert(j: X(j: 1), val: Point2(1.0, 2.0));
282 GaussianFactor::shared_ptr actual = f0->linearize(c: config);
283
284 // create expected
285 Vector2 b(0., -3.);
286 JacobianFactor expected(X(j: 1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0).finished(), b,
287 noiseModel::Constrained::MixedSigmas(sigmas: Vector2(1,0)));
288 CHECK(assert_equal((const GaussianFactor&)expected, *actual));
289}
290
291/* ************************************************************************* */
292TEST( NonlinearFactor, linearize_constraint2 )
293{
294 SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas: Vector2(0.2,0));
295
296 Point2 z3(1.,-1.);
297 simulated2D::Measurement f0(z3, constraint, X(j: 1),L(j: 1));
298
299 Values config;
300 config.insert(j: X(j: 1), val: Point2(1.0, 2.0));
301 config.insert(j: L(j: 1), val: Point2(5.0, 4.0));
302 GaussianFactor::shared_ptr actual = f0.linearize(x: config);
303
304 // create expected
305 Matrix2 A; A << 5.0, 0.0, 0.0, 1.0;
306 Vector2 b(-15., -3.);
307 JacobianFactor expected(X(j: 1), -1*A, L(j: 1), A, b,
308 noiseModel::Constrained::MixedSigmas(sigmas: Vector2(1,0)));
309 CHECK(assert_equal((const GaussianFactor&)expected, *actual));
310}
311
312/* ************************************************************************* */
313TEST( NonlinearFactor, cloneWithNewNoiseModel )
314{
315 // create original factor
316 double sigma1 = 0.1;
317 NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma: sigma1);
318
319 // create expected
320 double sigma2 = 10;
321 NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma: sigma2);
322
323 // create actual
324 NonlinearFactorGraph actual;
325 SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(dim: 2,sigma: sigma2);
326 actual.push_back(factor: nfg.at<NoiseModelFactor>(i: 0)->cloneWithNewNoiseModel(newNoise: noise2));
327
328 // check it's all good
329 CHECK(assert_equal(expected, actual));
330}
331
332/* ************************************************************************* */
333class TestFactor1 : public NoiseModelFactor1<double> {
334 static_assert(std::is_same<Base, NoiseModelFactor>::value, "Base type wrong");
335 static_assert(std::is_same<This, NoiseModelFactor1<double>>::value,
336 "This type wrong");
337
338 public:
339 typedef NoiseModelFactor1<double> Base;
340
341 // Provide access to the Matrix& version of evaluateError:
342 using Base::evaluateError;
343
344 TestFactor1() : Base(noiseModel::Diagonal::Sigmas(sigmas: Vector1(2.0)), L(j: 1)) {}
345
346 // Provide access to the Matrix& version of evaluateError:
347 using Base::NoiseModelFactor1; // inherit constructors
348
349 Vector evaluateError(const double& x1, OptionalMatrixType H1) const override {
350 if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
351 return (Vector(1) << x1).finished();
352 }
353
354 gtsam::NonlinearFactor::shared_ptr clone() const override {
355 return std::static_pointer_cast<gtsam::NonlinearFactor>(
356 r: gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this)));
357 }
358};
359
360/* ************************************ */
361TEST(NonlinearFactor, NoiseModelFactor1) {
362 TestFactor1 tf;
363 Values tv;
364 tv.insert(j: L(j: 1), val: double((1.0)));
365 EXPECT(assert_equal((Vector(1) << 1.0).finished(), tf.unwhitenedError(tv)));
366 DOUBLES_EQUAL(0.25 / 2.0, tf.error(tv), 1e-9);
367 JacobianFactor jf(
368 *std::dynamic_pointer_cast<JacobianFactor>(r: tf.linearize(x: tv)));
369 LONGS_EQUAL((long)L(1), (long)jf.keys()[0]);
370 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(),
371 jf.getA(jf.begin())));
372 EXPECT(assert_equal((Vector)(Vector(1) << -0.5).finished(), jf.getb()));
373
374 // Test all functions/types for backwards compatibility
375 static_assert(std::is_same<TestFactor1::X, double>::value,
376 "X type incorrect");
377 EXPECT(assert_equal(tf.key(), L(1)));
378 std::vector<Matrix> H = {Matrix()};
379 EXPECT(assert_equal(Vector1(1.0), tf.unwhitenedError(tv, H)));
380
381 // Test constructors
382 TestFactor1 tf2(noiseModel::Unit::Create(dim: 1), L(j: 1));
383 TestFactor1 tf3(noiseModel::Unit::Create(dim: 1), {L(j: 1)});
384 TestFactor1 tf4(noiseModel::Unit::Create(dim: 1), gtsam::Symbol('L', 1));
385}
386
387/* ************************************************************************* */
388class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
389 static_assert(std::is_same<Base, NoiseModelFactor>::value, "Base type wrong");
390 static_assert(
391 std::is_same<This,
392 NoiseModelFactor4<double, double, double, double>>::value,
393 "This type wrong");
394
395 public:
396 typedef NoiseModelFactor4<double, double, double, double> Base;
397
398 // Provide access to the Matrix& version of evaluateError:
399 using Base::evaluateError;
400
401 TestFactor4() : Base(noiseModel::Diagonal::Sigmas(sigmas: (Vector(1) << 2.0).finished()), X(j: 1), X(j: 2), X(j: 3), X(j: 4)) {}
402
403 // Provide access to the Matrix& version of evaluateError:
404 using Base::NoiseModelFactor4; // inherit constructors
405
406 Vector
407 evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
408 OptionalMatrixType H1, OptionalMatrixType H2,
409 OptionalMatrixType H3, OptionalMatrixType H4) const override {
410 if(H1) {
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();
415 }
416 return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished();
417 }
418
419 gtsam::NonlinearFactor::shared_ptr clone() const override {
420 return std::static_pointer_cast<gtsam::NonlinearFactor>(
421 r: gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
422};
423
424/* ************************************ */
425TEST(NonlinearFactor, NoiseModelFactor4) {
426 TestFactor4 tf;
427 Values tv;
428 tv.insert(j: X(j: 1), val: double((1.0)));
429 tv.insert(j: X(j: 2), val: double((2.0)));
430 tv.insert(j: X(j: 3), val: double((3.0)));
431 tv.insert(j: X(j: 4), val: double((4.0)));
432 EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv)));
433 DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9);
434 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(r: tf.linearize(x: tv)));
435 LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
436 LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
437 LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
438 LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
439 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
440 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
441 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
442 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
443 EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb()));
444
445 // Test all functions/types for backwards compatibility
446 static_assert(std::is_same<TestFactor4::X1, double>::value,
447 "X1 type incorrect");
448 static_assert(std::is_same<TestFactor4::X2, double>::value,
449 "X2 type incorrect");
450 static_assert(std::is_same<TestFactor4::X3, double>::value,
451 "X3 type incorrect");
452 static_assert(std::is_same<TestFactor4::X4, double>::value,
453 "X4 type incorrect");
454 EXPECT(assert_equal(tf.key1(), X(1)));
455 EXPECT(assert_equal(tf.key2(), X(2)));
456 EXPECT(assert_equal(tf.key3(), X(3)));
457 EXPECT(assert_equal(tf.key4(), X(4)));
458 std::vector<Matrix> H = {Matrix(), Matrix(), Matrix(), Matrix()};
459 EXPECT(assert_equal(Vector1(30.0), tf.unwhitenedError(tv, H)));
460 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.).finished(), H.at(0)));
461 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1)));
462 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2)));
463 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3)));
464
465 // And test "forward compatibility" using `key<N>` and `ValueType<N>` too
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");
474 EXPECT(assert_equal(tf.key<1>(), X(1)));
475 EXPECT(assert_equal(tf.key<2>(), X(2)));
476 EXPECT(assert_equal(tf.key<3>(), X(3)));
477 EXPECT(assert_equal(tf.key<4>(), X(4)));
478
479 // Test constructors
480 TestFactor4 tf2(noiseModel::Unit::Create(dim: 1), L(j: 1), L(j: 2), L(j: 3), L(j: 4));
481 TestFactor4 tf3(noiseModel::Unit::Create(dim: 1), {L(j: 1), L(j: 2), L(j: 3), L(j: 4)});
482 TestFactor4 tf4(noiseModel::Unit::Create(dim: 1),
483 std::array<Key, 4>{L(j: 1), L(j: 2), L(j: 3), L(j: 4)});
484 std::vector<Key> keys = {L(j: 1), L(j: 2), L(j: 3), L(j: 4)};
485 TestFactor4 tf5(noiseModel::Unit::Create(dim: 1), keys);
486}
487
488/* ************************************************************************* */
489class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
490public:
491 typedef NoiseModelFactor5<double, double, double, double, double> Base;
492
493 // Provide access to the Matrix& version of evaluateError:
494 using Base::evaluateError;
495
496 TestFactor5() : Base(noiseModel::Diagonal::Sigmas(sigmas: (Vector(1) << 2.0).finished()), X(j: 1), X(j: 2), X(j: 3), X(j: 4), X(j: 5)) {}
497
498 Vector
499 evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
500 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3,
501 OptionalMatrixType H4, OptionalMatrixType H5) const override {
502 if(H1) {
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();
508 }
509 return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4 + 5.0 * x5)
510 .finished();
511 }
512};
513
514/* ************************************ */
515TEST(NonlinearFactor, NoiseModelFactor5) {
516 TestFactor5 tf;
517 Values tv;
518 tv.insert(j: X(j: 1), val: double((1.0)));
519 tv.insert(j: X(j: 2), val: double((2.0)));
520 tv.insert(j: X(j: 3), val: double((3.0)));
521 tv.insert(j: X(j: 4), val: double((4.0)));
522 tv.insert(j: X(j: 5), val: double((5.0)));
523 EXPECT(assert_equal((Vector(1) << 55.0).finished(), tf.unwhitenedError(tv)));
524 DOUBLES_EQUAL(0.5 * 55.0 * 55.0 / 4.0, tf.error(tv), 1e-9);
525 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(r: tf.linearize(x: tv)));
526 LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
527 LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
528 LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
529 LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
530 LONGS_EQUAL((long)X(5), (long)jf.keys()[4]);
531 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
532 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
533 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
534 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
535 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4)));
536 EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -55.).finished(), jf.getb()));
537}
538
539/* ************************************************************************* */
540class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
541public:
542 typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
543
544 // Provide access to the Matrix& version of evaluateError:
545 using Base::evaluateError;
546
547 TestFactor6() : Base(noiseModel::Diagonal::Sigmas(sigmas: (Vector(1) << 2.0).finished()), X(j: 1), X(j: 2), X(j: 3), X(j: 4), X(j: 5), X(j: 6)) {}
548
549 Vector
550 evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
551 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
552 OptionalMatrixType H5, OptionalMatrixType H6) const override {
553 if(H1) {
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();
560 }
561 return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4 + 5.0 * x5 +
562 6.0 * x6)
563 .finished();
564 }
565
566};
567
568/* ************************************ */
569TEST(NonlinearFactor, NoiseModelFactor6) {
570 TestFactor6 tf;
571 Values tv;
572 tv.insert(j: X(j: 1), val: double((1.0)));
573 tv.insert(j: X(j: 2), val: double((2.0)));
574 tv.insert(j: X(j: 3), val: double((3.0)));
575 tv.insert(j: X(j: 4), val: double((4.0)));
576 tv.insert(j: X(j: 5), val: double((5.0)));
577 tv.insert(j: X(j: 6), val: double((6.0)));
578 EXPECT(assert_equal((Vector(1) << 91.0).finished(), tf.unwhitenedError(tv)));
579 DOUBLES_EQUAL(0.5 * 91.0 * 91.0 / 4.0, tf.error(tv), 1e-9);
580 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(r: tf.linearize(x: tv)));
581 LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
582 LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
583 LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
584 LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
585 LONGS_EQUAL((long)X(5), (long)jf.keys()[4]);
586 LONGS_EQUAL((long)X(6), (long)jf.keys()[5]);
587 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
588 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
589 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
590 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
591 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4)));
592 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0).finished(), jf.getA(jf.begin()+5)));
593 EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -91.).finished(), jf.getb()));
594
595}
596
597/* ************************************************************************* */
598class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
599public:
600 typedef NoiseModelFactorN<double, double, double, double> Base;
601
602 // Provide access to the Matrix& version of evaluateError:
603 using Base::evaluateError;
604
605 using Type1 = ValueType<1>; // Test that we can use the ValueType<> template
606
607 TestFactorN() : Base(noiseModel::Diagonal::Sigmas(sigmas: (Vector(1) << 2.0).finished()), X(j: 1), X(j: 2), X(j: 3), X(j: 4)) {}
608
609 Vector
610 evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
611 OptionalMatrixType H1, OptionalMatrixType H2,
612 OptionalMatrixType H3, OptionalMatrixType H4) const override {
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();
618 }
619
620 Key key1() const { return key<1>(); } // Test that we can use key<> template
621};
622
623/* ************************************ */
624TEST(NonlinearFactor, NoiseModelFactorN) {
625 TestFactorN tf;
626 Values tv;
627 tv.insert(j: X(j: 1), val: double((1.0)));
628 tv.insert(j: X(j: 2), val: double((2.0)));
629 tv.insert(j: X(j: 3), val: double((3.0)));
630 tv.insert(j: X(j: 4), val: double((4.0)));
631 EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv)));
632 DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9);
633 JacobianFactor jf(*std::dynamic_pointer_cast<JacobianFactor>(r: tf.linearize(x: tv)));
634 LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
635 LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
636 LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
637 LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
638 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
639 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
640 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
641 EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
642 EXPECT(assert_equal((Vector)(Vector(1) << -0.5 * 30.).finished(), jf.getb()));
643
644 // Test all evaluateError argument overloads to ensure backward compatibility
645 Matrix H1_expected, H2_expected, H3_expected, H4_expected;
646 Vector e_expected = tf.evaluateError(x: 9, x: 8, x: 7, x: 6, H&: H1_expected, H&: H2_expected,
647 H&: H3_expected, H&: H4_expected);
648
649 std::unique_ptr<NoiseModelFactorN<double, double, double, double>> base_ptr(
650 new TestFactorN(tf));
651 Matrix H1, H2, H3, H4;
652 EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6)));
653 EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6, H1)));
654 EXPECT(assert_equal(H1_expected, H1));
655 EXPECT(assert_equal(e_expected, //
656 base_ptr->evaluateError(9, 8, 7, 6, H1, H2)));
657 EXPECT(assert_equal(H1_expected, H1));
658 EXPECT(assert_equal(H2_expected, H2));
659 EXPECT(assert_equal(e_expected,
660 base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3)));
661 EXPECT(assert_equal(H1_expected, H1));
662 EXPECT(assert_equal(H2_expected, H2));
663 EXPECT(assert_equal(H3_expected, H3));
664 EXPECT(assert_equal(e_expected,
665 base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3, H4)));
666 EXPECT(assert_equal(H1_expected, H1));
667 EXPECT(assert_equal(H2_expected, H2));
668 EXPECT(assert_equal(H3_expected, H3));
669 EXPECT(assert_equal(H4_expected, H4));
670
671 // Test all functions/types for backwards compatibility
672
673 static_assert(std::is_same<TestFactor4::X1, double>::value,
674 "X1 type incorrect");
675 EXPECT(assert_equal(tf.key3(), X(3)));
676
677
678 // Test using `key<N>` and `ValueType<N>`
679 static_assert(std::is_same<TestFactorN::ValueType<1>, double>::value,
680 "ValueType<1> type incorrect");
681 static_assert(std::is_same<TestFactorN::ValueType<2>, double>::value,
682 "ValueType<2> type incorrect");
683 static_assert(std::is_same<TestFactorN::ValueType<3>, double>::value,
684 "ValueType<3> type incorrect");
685 static_assert(std::is_same<TestFactorN::ValueType<4>, double>::value,
686 "ValueType<4> type incorrect");
687 static_assert(std::is_same<TestFactorN::Type1, double>::value,
688 "TestFactorN::Type1 type incorrect");
689 EXPECT(assert_equal(tf.key<1>(), X(1)));
690 EXPECT(assert_equal(tf.key<2>(), X(2)));
691 EXPECT(assert_equal(tf.key<3>(), X(3)));
692 EXPECT(assert_equal(tf.key<4>(), X(4)));
693 EXPECT(assert_equal(tf.key1(), X(1)));
694}
695
696/* ************************************************************************* */
697TEST( NonlinearFactor, clone_rekey )
698{
699 shared_nlf init(new TestFactor4());
700 EXPECT_LONGS_EQUAL((long)X(1), (long)init->keys()[0]);
701 EXPECT_LONGS_EQUAL((long)X(2), (long)init->keys()[1]);
702 EXPECT_LONGS_EQUAL((long)X(3), (long)init->keys()[2]);
703 EXPECT_LONGS_EQUAL((long)X(4), (long)init->keys()[3]);
704
705 // Standard clone
706 shared_nlf actClone = init->clone();
707 EXPECT(actClone.get() != init.get()); // Ensure different pointers
708 EXPECT(assert_equal(*init, *actClone));
709
710 // Re-key factor - clones with different keys
711 KeyVector new_keys {X(j: 5),X(j: 6),X(j: 7),X(j: 8)};
712 shared_nlf actRekey = init->rekey(new_keys);
713 EXPECT(actRekey.get() != init.get()); // Ensure different pointers
714
715 // Ensure init is unchanged
716 EXPECT_LONGS_EQUAL((long)X(1), (long)init->keys()[0]);
717 EXPECT_LONGS_EQUAL((long)X(2), (long)init->keys()[1]);
718 EXPECT_LONGS_EQUAL((long)X(3), (long)init->keys()[2]);
719 EXPECT_LONGS_EQUAL((long)X(4), (long)init->keys()[3]);
720
721 // Check new keys
722 EXPECT_LONGS_EQUAL((long)X(5), (long)actRekey->keys()[0]);
723 EXPECT_LONGS_EQUAL((long)X(6), (long)actRekey->keys()[1]);
724 EXPECT_LONGS_EQUAL((long)X(7), (long)actRekey->keys()[2]);
725 EXPECT_LONGS_EQUAL((long)X(8), (long)actRekey->keys()[3]);
726}
727
728/* ************************************************************************* */
729int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
730/* ************************************************************************* */
731