testNonlinearFactor.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 
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 
36 using namespace std;
37 using namespace gtsam;
38 using namespace example;
39 
40 // Convenience for named keys
41 using symbol_shorthand::X;
42 using symbol_shorthand::L;
43 
44 typedef std::shared_ptr<NonlinearFactor > shared_nlf;
45 
46 /* ************************************************************************* */
47 TEST( NonlinearFactor, equals )
48 {
49  SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2,1.0));
50 
51  // create two nonlinear2 factors
52  Point2 z3(0.,-1.);
53  simulated2D::Measurement f0(z3, sigma, X(1),L(1));
54 
55  // measurement between x2 and l1
56  Point2 z4(-1.5, -1.);
57  simulated2D::Measurement f1(z4, sigma, X(2),L(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 /* ************************************************************************* */
66 TEST( 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 /* ************************************************************************* */
80 TEST( 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>(factor)->unwhitenedError(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(cfg);
101  DOUBLES_EQUAL(expected,actual,0.00000001);
102 }
103 
104 /* ************************************************************************* */
105 TEST(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(1), Point2(0, 0));
111  values.insert(X(1), 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(2, 0.2);
117  PriorFactor<Point2> factor2(X(2), Point2(1, 1), noise);
118  values.insert(X(2), 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(2, 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  noiseModel::mEstimator::Fair::Create(1.3998), gaussian);
135  robust_models.push_back(fair);
136 
137  // Huber noise model
138  auto huber = noiseModel::Robust::Create(
139  noiseModel::mEstimator::Huber::Create(1.345), gaussian);
140  robust_models.push_back(huber);
141 
142  // Cauchy noise model
143  auto cauchy = noiseModel::Robust::Create(
144  noiseModel::mEstimator::Cauchy::Create(0.1), gaussian);
145  robust_models.push_back(cauchy);
146 
147  // Tukey noise model
148  auto tukey = noiseModel::Robust::Create(
149  noiseModel::mEstimator::Tukey::Create(4.6851), gaussian);
150  robust_models.push_back(tukey);
151 
152  // Welsch noise model
153  auto welsch = noiseModel::Robust::Create(
154  noiseModel::mEstimator::Welsch::Create(2.9846), gaussian);
155  robust_models.push_back(welsch);
156 
157  // Geman-McClure noise model
158  auto gm = noiseModel::Robust::Create(
159  noiseModel::mEstimator::GemanMcClure::Create(1.0), gaussian);
160  robust_models.push_back(gm);
161 
162  // DCS noise model
163  auto dcs = noiseModel::Robust::Create(
164  noiseModel::mEstimator::DCS::Create(1.0), gaussian);
165  robust_models.push_back(dcs);
166 
167  // L2WithDeadZone noise model
168  auto l2 = noiseModel::Robust::Create(
169  noiseModel::mEstimator::L2WithDeadZone::Create(1.0), gaussian);
170  robust_models.push_back(l2);
171 
172  for(auto&& model: robust_models) {
173  factor = PriorFactor<Point2>(X(3), prior, model);
174  values.clear();
175  values.insert(X(3), estimate);
176  CHECK(assert_equal(model->robust()->weight(distance), factor.weight(values)));
177  }
178 }
179 
180 /* ************************************************************************* */
181 TEST( 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 /* ************************************************************************* */
203 TEST( 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 /* ************************************************************************* */
221 TEST( 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 /* ************************************************************************* */
238 TEST( 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 /* ************************************************************************* */
255 TEST( 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 /* ************************************************************************* */
273 TEST( NonlinearFactor, linearize_constraint1 )
274 {
275  SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
276 
277  Point2 mu(1., -1.);
278  NonlinearFactorGraph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1)));
279 
280  Values config;
281  config.insert(X(1), Point2(1.0, 2.0));
282  GaussianFactor::shared_ptr actual = f0->linearize(config);
283 
284  // create expected
285  Vector2 b(0., -3.);
286  JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0).finished(), b,
287  noiseModel::Constrained::MixedSigmas(Vector2(1,0)));
288  CHECK(assert_equal((const GaussianFactor&)expected, *actual));
289 }
290 
291 /* ************************************************************************* */
292 TEST( NonlinearFactor, linearize_constraint2 )
293 {
294  SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
295 
296  Point2 z3(1.,-1.);
297  simulated2D::Measurement f0(z3, constraint, X(1),L(1));
298 
299  Values config;
300  config.insert(X(1), Point2(1.0, 2.0));
301  config.insert(L(1), Point2(5.0, 4.0));
302  GaussianFactor::shared_ptr actual = f0.linearize(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(1), -1*A, L(1), A, b,
308  noiseModel::Constrained::MixedSigmas(Vector2(1,0)));
309  CHECK(assert_equal((const GaussianFactor&)expected, *actual));
310 }
311 
312 /* ************************************************************************* */
313 TEST( NonlinearFactor, cloneWithNewNoiseModel )
314 {
315  // create original factor
316  double sigma1 = 0.1;
317  NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1);
318 
319  // create expected
320  double sigma2 = 10;
321  NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2);
322 
323  // create actual
324  NonlinearFactorGraph actual;
325  SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2);
326  actual.push_back( std::dynamic_pointer_cast<NoiseModelFactor>(nfg[0])->cloneWithNewNoiseModel(noise2) );
327 
328  // check it's all good
329  CHECK(assert_equal(expected, actual));
330 }
331 
332 /* ************************************************************************* */
333 class 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(Vector1(2.0)), L(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  gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this)));
357  }
358 };
359 
360 /* ************************************ */
361 TEST(NonlinearFactor, NoiseModelFactor1) {
362  TestFactor1 tf;
363  Values tv;
364  tv.insert(L(1), 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>(tf.linearize(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(1), L(1));
383  TestFactor1 tf3(noiseModel::Unit::Create(1), {L(1)});
384  TestFactor1 tf4(noiseModel::Unit::Create(1), gtsam::Symbol('L', 1));
385 }
386 
387 /* ************************************************************************* */
388 class 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((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(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  gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
422 };
423 
424 /* ************************************ */
425 TEST(NonlinearFactor, NoiseModelFactor4) {
426  TestFactor4 tf;
427  Values tv;
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)));
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>(tf.linearize(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(1), L(1), L(2), L(3), L(4));
481  TestFactor4 tf3(noiseModel::Unit::Create(1), {L(1), L(2), L(3), L(4)});
482  TestFactor4 tf4(noiseModel::Unit::Create(1),
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)};
485  TestFactor4 tf5(noiseModel::Unit::Create(1), keys);
486 }
487 
488 /* ************************************************************************* */
489 class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
490 public:
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((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(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 /* ************************************ */
515 TEST(NonlinearFactor, NoiseModelFactor5) {
516  TestFactor5 tf;
517  Values tv;
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)));
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>(tf.linearize(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 /* ************************************************************************* */
540 class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
541 public:
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((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(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 /* ************************************ */
569 TEST(NonlinearFactor, NoiseModelFactor6) {
570  TestFactor6 tf;
571  Values tv;
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)));
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>(tf.linearize(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 /* ************************************************************************* */
598 class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
599 public:
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((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(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 /* ************************************ */
624 TEST(NonlinearFactor, NoiseModelFactorN) {
625  TestFactorN tf;
626  Values tv;
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)));
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>(tf.linearize(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(9, 8, 7, 6, H1_expected, H2_expected,
647  H3_expected, 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 /* ************************************************************************* */
697 TEST( 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(5),X(6),X(7),X(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 /* ************************************************************************* */
729 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
730 /* ************************************************************************* */
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:187
CHECK
#define CHECK(condition)
Definition: Test.h:108
E1::A
gtsam::Values::clear
void clear()
Definition: Values.h:298
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:194
gtsam::example::createNoisyValues
Values createNoisyValues()
Definition: smallExample.h:243
TestFactorN::Type1
ValueType< 1 > Type1
Definition: testNonlinearFactor.cpp:605
b
Scalar * b
Definition: benchVecAdd.cpp:17
Testable.h
Concept check for values that can be used in unit tests.
simple_graph::sigma1
double sigma1
Definition: testJacobianFactor.cpp:184
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Measurement
This is the base class for all measurement types.
Definition: Measurement.h:11
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:270
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
simulated2D::prior
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
TEST
TEST(NonlinearFactor, equals)
Definition: testNonlinearFactor.cpp:47
TestFactor5::Base
NoiseModelFactor5< double, double, double, double, double > Base
Definition: testNonlinearFactor.cpp:491
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
leaf::values
leaf::MyValues values
gtsam::example::nonlinearFactorGraphWithGivenSigma
NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma)
Definition: smallExample.h:354
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: BFloat16.h:88
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
NoiseModelFactor4
#define NoiseModelFactor4
Definition: NonlinearFactor.h:767
TestFactorN::Base
NoiseModelFactorN< double, double, double, double > Base
Definition: testNonlinearFactor.cpp:600
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:12
x5
static string x5("x5")
NoiseModelFactor5
#define NoiseModelFactor5
Definition: NonlinearFactor.h:768
TestFactor6::Base
NoiseModelFactor6< double, double, double, double, double, double > Base
Definition: testNonlinearFactor.cpp:542
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
TestFactor5::evaluateError
Vector evaluateError(const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override
Definition: testNonlinearFactor.cpp:499
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
z3
static const Unit3 z3
Definition: testRotateFactor.cpp:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
test_callbacks.value
value
Definition: test_callbacks.py:148
TestFactor4::Base
NoiseModelFactor4< double, double, double, double > Base
Definition: testNonlinearFactor.cpp:389
TestFactor1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: testNonlinearFactor.cpp:354
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::example::createNonlinearFactorGraph
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:206
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:201
gtsam::NoiseModelFactor::weight
double weight(const Values &c) const
Definition: NonlinearFactor.cpp:121
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::NoiseModelFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:150
NoiseModelFactor6
#define NoiseModelFactor6
Definition: NonlinearFactor.h:769
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
main
int main()
Definition: testNonlinearFactor.cpp:729
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam
traits
Definition: chartTesting.h:28
NoiseModelFactor1
#define NoiseModelFactor1
Definition: NonlinearFactor.h:764
gtsam::NoiseModelFactorN::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: NonlinearFactor.h:606
simulated2D.h
measurement functions and derivatives for simulated 2D robot
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
unary::f1
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Definition: testExpression.cpp:79
x1
Pose3 x1
Definition: testPose3.cpp:663
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::NoiseModelFactorN< double, double, double, double >::ValueType
typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type ValueType
Definition: NonlinearFactor.h:525
TestFactor4::evaluateError
Vector evaluateError(const double &x1, const double &x2, const double &x3, const double &x4, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4) const override
Definition: testNonlinearFactor.cpp:407
smallExample.h
Create small example with two poses and one landmark.
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
TestFactor4::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: testNonlinearFactor.cpp:419
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:169
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
TestFactor1::Base
NoiseModelFactor1< double > Base
Definition: testNonlinearFactor.cpp:334
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
x4
static string x4("x4")
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
X
#define X
Definition: icosphere.cpp:20
simple_graph::sigma2
double sigma2
Definition: testJacobianFactor.cpp:190
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
TestFactor1::evaluateError
Vector evaluateError(const double &x1, OptionalMatrixType H1) const override
Definition: testNonlinearFactor.cpp:349
TestFactor6::evaluateError
Vector evaluateError(const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const override
Definition: testNonlinearFactor.cpp:550
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
shared_nlf
std::shared_ptr< NonlinearFactor > shared_nlf
Definition: testNonlinearFactor.cpp:44
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
TestFactorN::evaluateError
Vector evaluateError(const double &x1, const double &x2, const double &x3, const double &x4, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4) const override
Definition: testNonlinearFactor.cpp:610

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