testExpressionFactor.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 
21 #include <gtsam/base/Testable.h>
28 #include <gtsam/slam/expressions.h>
29 
30 using namespace std::placeholders;
31 
32 using namespace std;
33 using namespace gtsam;
34 
35 Point2 measured(-17, 30);
36 SharedNoiseModel model = noiseModel::Unit::Create(2);
37 
38 // This deals with the overload problem and makes the expressions factor
39 // understand that we work on Point3
41 
42 namespace leaf {
43 // Create some values
44 struct MyValues: public Values {
46  insert(2, Point2(3, 5));
47  }
48 } values;
49 
50 // Create leaf
51 Point2_ p(2);
52 }
53 
54 /* ************************************************************************* */
55 // Leaf
57  using namespace leaf;
58 
59  // Create old-style factor to create expected value and derivatives.
60  PriorFactor<Point2> old(2, Point2(0, 0), model);
61 
62  // Create the equivalent factor with expression.
64 
65  // Check values and derivatives.
67  EXPECT_LONGS_EQUAL(2, f.dim())
68  std::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
69  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9))
70 }
71 
72 /* ************************************************************************* */
73 // Test leaf expression with noise model of different variance.
75  using namespace leaf;
76 
77  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
78 
79  // Create old-style factor to create expected value and derivatives.
80  PriorFactor<Point2> old(2, Point2(0, 0), model);
81 
82  // Create the equivalent factor with expression.
83  ExpressionFactor<Point2> f(model, Point2(0, 0), p);
84 
85  // Check values and derivatives.
87  EXPECT_LONGS_EQUAL(2, f.dim())
88  std::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
89  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9))
90  EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5) // another way
91 }
92 
93 /* ************************************************************************* */
94 // Test leaf expression with constrained noise model.
95 TEST(ExpressionFactor, Constrained) {
96  using namespace leaf;
97 
98  SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0));
99 
100  // Create old-style factor to create expected value and derivatives
101  PriorFactor<Point2> old(2, Point2(0, 0), model);
102 
103  // Concise version
104  ExpressionFactor<Point2> f(model, Point2(0, 0), p);
106  EXPECT_LONGS_EQUAL(2, f.dim())
107  std::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
108  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9))
109 }
110 
111 /* ************************************************************************* */
112 // Unary(Leaf))
114 
115  // Create some values
116  Values values;
117  values.insert(2, Point3(0, 0, 1));
118 
120  2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0).finished(), //
121  Vector2(-17, 30));
122 
123  // Create leaves
124  Point3_ p(2);
125 
126  // Concise version
128  EXPECT_LONGS_EQUAL(2, f.dim())
129  std::shared_ptr<GaussianFactor> gf = f.linearize(values);
130  std::shared_ptr<JacobianFactor> jf = //
131  std::dynamic_pointer_cast<JacobianFactor>(gf);
132  EXPECT(assert_equal(expected, *jf, 1e-9))
133 }
134 
135 /* ************************************************************************* */
136 // Unary(Leaf)) and Unary(Unary(Leaf)))
137 // wide version (not handled in fixed-size pipeline)
139 Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
140  Vector9 v;
141  v << p, p, p;
142  if (H) *H << I_3x3, I_3x3, I_3x3;
143  return v;
144 }
145 
147 Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
148  if (H) *H = Matrix9::Identity();
149  return v;
150 }
151 
153  // Create some values
154  Values values;
155  values.insert(2, Point3(0, 0, 1));
156  Point3_ point(2);
157  Vector9 measured;
158  measured.setZero();
159  Expression<Vector9> expression(wide,point);
160  SharedNoiseModel model = noiseModel::Unit::Create(9);
161 
162  ExpressionFactor<Vector9> f1(model, measured, expression);
163  EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9)
164 
165  Expression<Vector9> expression2(id9,expression);
166  ExpressionFactor<Vector9> f2(model, measured, expression2);
167  EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9)
168 }
169 
170 /* ************************************************************************* */
171 static Point2 myUncal(const Cal3_S2& K, const Point2& p,
173  return K.uncalibrate(p, Dcal, Dp);
174 }
175 
176 // Binary(Leaf,Leaf)
178 
179  typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary;
180 
181  Cal3_S2_ K_(1);
182  Point2_ p_(2);
183  Binary binary(myUncal, K_, p_);
184 
185  // Create some values
186  Values values;
187  values.insert(1, Cal3_S2());
188  values.insert(2, Point2(0, 0));
189 
190  // Check size
191  auto traceStorage = allocAligned(binary.traceSize());
192  internal::ExecutionTrace<Point2> trace;
193  Point2 value = binary.traceExecution(values, trace, reinterpret_cast<char *>(traceStorage.get()));
194  EXPECT(assert_equal(Point2(0,0),value, 1e-9))
195  // trace.print();
196 
197  // Expected Jacobians
198  Matrix25 expected25;
199  expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
200  Matrix2 expected22;
201  expected22 << 1, 0, 0, 1;
202 
203  // Check matrices
204  std::optional<Binary::Record*> r = trace.record<Binary::Record>();
205  CHECK(r)
206  EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9))
207  EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9))
208 }
209 
210 /* ************************************************************************* */
211 // Unary(Binary(Leaf,Leaf))
213 
214  // Create some values
215  Values values;
216  values.insert(1, Pose3());
217  values.insert(2, Point3(0, 0, 1));
218 
219  // Create old-style factor to create expected value and derivatives
221  std::make_shared<Cal3_S2>());
222  double expected_error = old.error(values);
223  GaussianFactor::shared_ptr expected = old.linearize(values);
224 
225  // Create leaves
226  Pose3_ x_(1);
227  Point3_ p_(2);
228 
229  // Construct expression, concise version
230  Point2_ expression = project(transformTo(x_, p_));
231 
232  // Get and check keys and dims
233  const auto [keys, dims] = expression.keysAndDims();
234  LONGS_EQUAL(2,keys.size())
235  LONGS_EQUAL(2,dims.size())
236  LONGS_EQUAL(1,keys[0])
237  LONGS_EQUAL(2,keys[1])
238  LONGS_EQUAL(6,dims[0])
239  LONGS_EQUAL(3,dims[1])
240 
241  // traceExecution of shallow tree
242  typedef internal::UnaryExpression<Point2, Point3> Unary;
243  auto traceStorage = allocAligned(expression.traceSize());
244  internal::ExecutionTrace<Point2> trace;
245  Point2 value = expression.traceExecution(values, trace, reinterpret_cast<char *>(traceStorage.get()));
246  EXPECT(assert_equal(Point2(0,0),value, 1e-9))
247  // trace.print();
248 
249  // Expected Jacobians
250  Matrix23 expected23;
251  expected23 << 1, 0, 0, 0, 1, 0;
252 
253  // Check matrices
254  std::optional<Unary::Record*> r = trace.record<Unary::Record>();
255  CHECK(r)
256  EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9))
257 
258  // Linearization
260  EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9)
261  EXPECT_LONGS_EQUAL(2, f2.dim())
262  std::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
263  EXPECT(assert_equal(*expected, *gf2, 1e-9))
264 }
265 
266 /* ************************************************************************* */
267 // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
269 
270  // Create some values
271  Values values;
272  values.insert(1, Pose3());
273  values.insert(2, Point3(0, 0, 1));
274  values.insert(3, Cal3_S2());
275 
276  // Create old-style factor to create expected value and derivatives
278  double expected_error = old.error(values);
279  GaussianFactor::shared_ptr expected = old.linearize(values);
280 
281  // Create leaves
282  Pose3_ x(1);
283  Point3_ p(2);
284  Cal3_S2_ K(3);
285 
286  // Create expression tree
288  Point2_ xy_hat(Project, p_cam);
289  Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
290 
291  // Create factor and check value, dimension, linearization
293  EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9)
294  EXPECT_LONGS_EQUAL(2, f.dim())
295  std::shared_ptr<GaussianFactor> gf = f.linearize(values);
296  EXPECT(assert_equal(*expected, *gf, 1e-9))
297 
298  // Concise version
300  uncalibrate(K, project(transformTo(x, p))));
301  EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9)
302  EXPECT_LONGS_EQUAL(2, f2.dim())
303  std::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
304  EXPECT(assert_equal(*expected, *gf2, 1e-9))
305 
306  // Try ternary version
308  EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9)
309  EXPECT_LONGS_EQUAL(2, f3.dim())
310  std::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
311  EXPECT(assert_equal(*expected, *gf3, 1e-9))
312 }
313 
314 /* ************************************************************************* */
315 TEST(ExpressionFactor, Compose1) {
316 
317  // Create expression
318  Rot3_ R1(1), R2(2);
319  Rot3_ R3 = R1 * R2;
320 
321  // Create factor
322  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
323 
324  // Create some values
325  Values values;
326  values.insert(1, Rot3());
327  values.insert(2, Rot3());
328 
329  // Check unwhitenedError
330  std::vector<Matrix> H(2);
331  Vector actual = f.unwhitenedError(values, H);
332  EXPECT(assert_equal(I_3x3, H[0],1e-9))
333  EXPECT(assert_equal(I_3x3, H[1],1e-9))
334 
335  // Check linearization
336  JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
337  std::shared_ptr<GaussianFactor> gf = f.linearize(values);
338  std::shared_ptr<JacobianFactor> jf = //
339  std::dynamic_pointer_cast<JacobianFactor>(gf);
340  EXPECT(assert_equal(expected, *jf,1e-9))
341 }
342 
343 /* ************************************************************************* */
344 // Test compose with arguments referring to the same rotation
345 TEST(ExpressionFactor, compose2) {
346 
347  // Create expression
348  Rot3_ R1(1), R2(1);
349  Rot3_ R3 = R1 * R2;
350 
351  // Create factor
352  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
353 
354  // Create some values
355  Values values;
356  values.insert(1, Rot3());
357 
358  // Check unwhitenedError
359  std::vector<Matrix> H(1);
360  Vector actual = f.unwhitenedError(values, H);
361  EXPECT_LONGS_EQUAL(1, H.size())
362  EXPECT(assert_equal(2*I_3x3, H[0],1e-9))
363 
364  // Check linearization
365  JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
366  std::shared_ptr<GaussianFactor> gf = f.linearize(values);
367  std::shared_ptr<JacobianFactor> jf = //
368  std::dynamic_pointer_cast<JacobianFactor>(gf);
369  EXPECT(assert_equal(expected, *jf,1e-9))
370 }
371 
372 /* ************************************************************************* */
373 // Test compose with one arguments referring to a constant same rotation
374 TEST(ExpressionFactor, compose3) {
375 
376  // Create expression
377  Rot3_ R1(Rot3::Identity()), R2(3);
378  Rot3_ R3 = R1 * R2;
379 
380  // Create factor
381  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
382 
383  // Create some values
384  Values values;
385  values.insert(3, Rot3());
386 
387  // Check unwhitenedError
388  std::vector<Matrix> H(1);
389  Vector actual = f.unwhitenedError(values, H);
390  EXPECT_LONGS_EQUAL(1, H.size())
391  EXPECT(assert_equal(I_3x3, H[0],1e-9))
392 
393  // Check linearization
394  JacobianFactor expected(3, I_3x3, Z_3x1);
395  std::shared_ptr<GaussianFactor> gf = f.linearize(values);
396  std::shared_ptr<JacobianFactor> jf = //
397  std::dynamic_pointer_cast<JacobianFactor>(gf);
398  EXPECT(assert_equal(expected, *jf,1e-9))
399 }
400 
401 /* ************************************************************************* */
402 // Test compose with three arguments
403 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
405  // return dummy derivatives (not correct, but that's ok for testing here)
406  if (H1)
407  *H1 = I_3x3;
408  if (H2)
409  *H2 = I_3x3;
410  if (H3)
411  *H3 = I_3x3;
412  return R1 * (R2 * R3);
413 }
414 
415 TEST(ExpressionFactor, composeTernary) {
416 
417  // Create expression
418  Rot3_ A(1), B(2), C(3);
419  Rot3_ ABC(composeThree, A, B, C);
420 
421  // Create factor
422  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
423 
424  // Create some values
425  Values values;
426  values.insert(1, Rot3());
427  values.insert(2, Rot3());
428  values.insert(3, Rot3());
429 
430  // Check unwhitenedError
431  std::vector<Matrix> H(3);
432  Vector actual = f.unwhitenedError(values, H);
433  EXPECT_LONGS_EQUAL(3, H.size())
434  EXPECT(assert_equal(I_3x3, H[0],1e-9))
435  EXPECT(assert_equal(I_3x3, H[1],1e-9))
436  EXPECT(assert_equal(I_3x3, H[2],1e-9))
437 
438  // Check linearization
439  JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
440  std::shared_ptr<GaussianFactor> gf = f.linearize(values);
441  std::shared_ptr<JacobianFactor> jf = //
442  std::dynamic_pointer_cast<JacobianFactor>(gf);
443  EXPECT(assert_equal(expected, *jf,1e-9))
444 }
445 
446 TEST(ExpressionFactor, tree_finite_differences) {
447 
448  // Create some values
449  Values values;
450  values.insert(1, Pose3());
451  values.insert(2, Point3(0, 0, 1));
452  values.insert(3, Cal3_S2());
453 
454  // Create leaves
455  Pose3_ x(1);
456  Point3_ p(2);
457  Cal3_S2_ K(3);
458 
459  // Create expression tree
461  Point2_ xy_hat(Project, p_cam);
462  Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
463 
464  const double fd_step = 1e-5;
465  const double tolerance = 1e-5;
466  EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance)
467 }
468 
469 TEST(ExpressionFactor, push_back) {
471  graph.addExpressionFactor(model, Point2(0, 0), leaf::p);
472 }
473 
474 /* ************************************************************************* */
475 // Test with multiple compositions on duplicate keys
476 struct Combine {
477  double a, b;
478  Combine(double a, double b) : a(a), b(b) {}
479  double operator()(const double& x, const double& y, OptionalJacobian<1, 1> H1,
481  if (H1) (*H1) << a;
482  if (H2) (*H2) << b;
483  return a * x + b * y;
484  }
485 };
486 
487 TEST(Expression, testMultipleCompositions) {
488  const double tolerance = 1e-5;
489  const double fd_step = 1e-5;
490 
491  Values values;
492  values.insert(1, 10.0);
493  values.insert(2, 20.0);
494 
495  Expression<double> v1_(Key(1));
496  Expression<double> v2_(Key(2));
497 
498  // BinaryExpression(1,2)
499  // Leaf, key = 1
500  // Leaf, key = 2
501  Expression<double> sum1_(Combine(1, 2), v1_, v2_);
502  EXPECT((sum1_.keys() == std::set<Key>{1, 2}))
503  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance)
504 
505  // BinaryExpression(3,4)
506  // BinaryExpression(1,2)
507  // Leaf, key = 1
508  // Leaf, key = 2
509  // Leaf, key = 1
510  Expression<double> sum2_(Combine(3, 4), sum1_, v1_);
511  EXPECT((sum2_.keys() == std::set<Key>{1, 2}))
512  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance)
513 
514  // BinaryExpression(5,6)
515  // BinaryExpression(3,4)
516  // BinaryExpression(1,2)
517  // Leaf, key = 1
518  // Leaf, key = 2
519  // Leaf, key = 1
520  // BinaryExpression(1,2)
521  // Leaf, key = 1
522  // Leaf, key = 2
523  Expression<double> sum3_(Combine(5, 6), sum1_, sum2_);
524  EXPECT((sum3_.keys() == std::set<Key>{1, 2}))
525  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance)
526 }
527 
528 /* ************************************************************************* */
529 // Another test, with Ternary Expressions
530 static double combine3(const double& x, const double& y, const double& z,
533  if (H1) (*H1) << 1.0;
534  if (H2) (*H2) << 2.0;
535  if (H3) (*H3) << 3.0;
536  return x + 2.0 * y + 3.0 * z;
537 }
538 
539 TEST(Expression, testMultipleCompositions2) {
540  const double tolerance = 1e-5;
541  const double fd_step = 1e-5;
542 
543  Values values;
544  values.insert(1, 10.0);
545  values.insert(2, 20.0);
546  values.insert(3, 30.0);
547 
548  Expression<double> v1_(Key(1));
549  Expression<double> v2_(Key(2));
550  Expression<double> v3_(Key(3));
551 
552  Expression<double> sum1_(Combine(4,5), v1_, v2_);
553  EXPECT((sum1_.keys() == std::set<Key>{1, 2}))
554  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance)
555 
556  Expression<double> sum2_(combine3, v1_, v2_, v3_);
557  EXPECT((sum2_.keys() == std::set<Key>{1, 2, 3}))
558  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance)
559 
560  Expression<double> sum3_(combine3, v3_, v2_, v1_);
561  EXPECT((sum3_.keys() == std::set<Key>{1, 2, 3}))
562  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance)
563 
564  Expression<double> sum4_(combine3, sum1_, sum2_, sum3_);
565  EXPECT((sum4_.keys() == std::set<Key>{1, 2, 3}))
566  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance)
567 }
568 
569 /* ************************************************************************* */
570 // Test multiplication with the inverse of a matrix
572  auto model = noiseModel::Isotropic::Sigma(3, 1);
573 
574  // Create expression
576 
577  // Check derivatives
578  Values values;
579  Matrix3 A = Vector3(1, 2, 3).asDiagonal();
580  A(0, 1) = 0.1;
581  A(0, 2) = 0.1;
582  const Vector3 b(0.1, 0.2, 0.3);
583  values.insert<Matrix3>(0, A);
584  values.insert<Vector3>(1, b);
585  ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
586  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
587 }
588 
589 /* ************************************************************************* */
590 // Test multiplication with the inverse of a matrix function
591 namespace test_operator {
594  Matrix3 A = Vector3(1, 2, 3).asDiagonal();
595  A(0, 1) = a.x();
596  A(0, 2) = a.y();
597  A(1, 0) = a.x();
598  if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0;
599  if (H2) *H2 = A;
600  return A * b;
601 }
602 }
603 
605  auto model = noiseModel::Isotropic::Sigma(3, 1);
606 
607  using test_operator::f;
610 
611  // Check derivatives
612  Point2 a(1, 2);
613  const Vector3 b(0.1, 0.2, 0.3);
614  Matrix32 H1;
615  Matrix3 A;
616  const Vector Ab = f(a, b, H1, A);
617  CHECK(assert_equal(A * b, Ab))
619  numericalDerivative11<Vector3, Point2>(
620  [&](const Point2& a) { return f(a, b, {}, {}); }, a),
621  H1))
622 
623  Values values;
624  values.insert<Point2>(0, a);
625  values.insert<Vector3>(1, b);
626  ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
627  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
628 }
629 
630 
631 /* ************************************************************************* */
632 // Test N-ary variadic template
634  : public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
635  gtsam::Rot3, gtsam::Point3,
636  gtsam::Rot3, gtsam::Point3> {
637 private:
639  using Base =
640  gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
642 
643 public:
645  TestNaryFactor() = default;
646 
648  const gtsam::SharedNoiseModel &model, const gtsam::Point3& measured)
649  : Base({kR1, kV1, kR2, kV2}, model, measured) {
650  this->initialize(expression({kR1, kV1, kR2, kV2}));
651  }
652 
655  return std::static_pointer_cast<gtsam::NonlinearFactor>(
657  }
658 
659  // Return measurement expression
661  const std::array<gtsam::Key, NARY_EXPRESSION_SIZE> &keys) const override {
662  gtsam::Expression<gtsam::Rot3> R1_(keys[0]);
664  gtsam::Expression<gtsam::Rot3> R2_(keys[2]);
666  return {gtsam::rotate(R1_, V1_) - gtsam::rotate(R2_, V2_)};
667  }
668 
670  void print(const std::string &s,
671  const gtsam::KeyFormatter &keyFormatter =
672  gtsam::DefaultKeyFormatter) const override {
673  std::cout << s << "TestNaryFactor("
674  << keyFormatter(Factor::keys_[0]) << ","
675  << keyFormatter(Factor::keys_[1]) << ","
676  << keyFormatter(Factor::keys_[2]) << ","
677  << keyFormatter(Factor::keys_[3]) << ")\n";
678  gtsam::traits<gtsam::Point3>::Print(measured_, " measured: ");
679  this->noiseModel_->print(" noise model: ");
680  }
681 
684  double tol = 1e-9) const override {
685  const This *e = dynamic_cast<const This *>(&expected);
686  return e != nullptr && Base::equals(*e, tol) &&
687  gtsam::traits<gtsam::Point3>::Equals(measured_,e->measured_, tol);
688  }
689 
690 private:
691 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
692 
693  friend class boost::serialization::access;
694  template <class ARCHIVE>
695  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
696  ar &boost::serialization::make_nvp(
697  "TestNaryFactor",
698  boost::serialization::base_object<Base>(*this));
699  ar &BOOST_SERIALIZATION_NVP(measured_);
700  }
701 #endif
702 };
703 
704 TEST(ExpressionFactor, variadicTemplate) {
707 
708  // Create factor
709  TestNaryFactor f(R(0),V(0), R(1), V(1), noiseModel::Unit::Create(3), Point3(0,0,0));
710 
711  // Create some values
712  Values values;
713  values.insert(R(0), Rot3::Ypr(0.1, 0.2, 0.3));
714  values.insert(V(0), Point3(1, 2, 3));
715  values.insert(R(1), Rot3::Ypr(0.2, 0.5, 0.2));
716  values.insert(V(1), Point3(5, 6, 7));
717 
718  // Check unwhitenedError
719  std::vector<Matrix> H(4);
720  Vector actual = f.unwhitenedError(values, H);
721  EXPECT_LONGS_EQUAL(4, H.size())
722  EXPECT(assert_equal(Eigen::Vector3d(-5.63578115, -4.85353243, -1.4801204), actual, 1e-5))
723 
724  EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5)
725 }
726 
728  auto model = noiseModel::Isotropic::Sigma(3, 1);
729 
730  // Create expression
731  const auto x = Vector3_(1);
732  Vector3_ f_expr = normalize(x);
733 
734  // Check derivatives
735  Values values;
736  values.insert(1, Vector3(1, 2, 3));
737  ExpressionFactor<Vector3> factor(model, Vector3(1.0/sqrt(14), 2.0/sqrt(14), 3.0/sqrt(14)), f_expr);
738  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
739 }
740 
741 TEST(ExpressionFactor, crossProduct) {
742  auto model = noiseModel::Isotropic::Sigma(3, 1);
743 
744  // Create expression
745  const auto a = Vector3_(1);
746  const auto b = Vector3_(2);
747  Vector3_ f_expr = cross(a, b);
748 
749  // Check derivatives
750  Values values;
751  values.insert(1, Vector3(0.1, 0.2, 0.3));
752  values.insert(2, Vector3(0.4, 0.5, 0.6));
753  ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
754  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
755 }
756 
757 TEST(ExpressionFactor, dotProduct) {
758  auto model = noiseModel::Isotropic::Sigma(1, 1);
759 
760  // Create expression
761  const auto a = Vector3_(1);
762  const auto b = Vector3_(2);
763  Double_ f_expr = dot(a, b);
764 
765  // Check derivatives
766  Values values;
767  values.insert(1, Vector3(0.1, 0.2, 0.3));
768  values.insert(2, Vector3(0.4, 0.5, 0.6));
769  ExpressionFactor<double> factor(model, .0, f_expr);
770  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
771 }
772 
773 
774 /* ************************************************************************* */
775 int main() {
776  TestResult tr;
777  return TestRegistry::runAllTests(tr);
778 }
779 /* ************************************************************************* */
780 
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
Point2 measured(-17, 30)
Combine(double a, double b)
#define CHECK(condition)
Definition: Test.h:108
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
A insert(1, 2)=0
size_t dim() const override
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
Vector9 id9(const Vector9 &v, OptionalJacobian< 9, 9 > H)
Scalar * y
Scalar * b
Definition: benchVecAdd.cpp:17
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Factor Graph consisting of non-linear factors.
std::string serialize(const T &input)
serializes to a string
Matrix expected
Definition: testMatrix.cpp:971
bool equals(const gtsam::NonlinearFactor &expected, double tol=1e-9) const override
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
leaf::MyValues values
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
noiseModel::Isotropic::shared_ptr Model
Definition: BFloat16.h:88
static double combine3(const double &x, const double &y, const double &z, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2, OptionalJacobian< 1, 1 > H3)
double f2(const Vector2 &x)
void print(const std::string &s, const gtsam::KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
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
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TEST(ExpressionFactor, Leaf)
KeysAndDims keysAndDims() const
std::unique_ptr< internal::ExecutionTraceStorage[]> allocAligned(size_t size)
Rot3 composeThree(const Rot3 &R1, const Rot3 &R2, const Rot3 &R3, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2, OptionalJacobian< 3, 3 > H3)
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Eigen::VectorXd Vector
Definition: Vector.h:38
double operator()(const double &x, const double &y, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Test harness methods for expressions.
Eigen::Matrix< double, 9, 3 > Matrix93
gtsam::NonlinearFactor::shared_ptr clone() const override
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
SharedNoiseModel model
Point3_ p_cam(x, &Pose3::transformTo, p)
Eigen::Matrix< double, 9, 9 > Matrix9
static sharedNode Leaf(Key key, const SymbolicFactorGraph &factors)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
Reprojection of a LANDMARK to a 2D point.
#define This
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
Vector9 wide(const Point3 &p, OptionalJacobian< 9, 3 > H)
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
static Rot3 R3
TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2, const gtsam::SharedNoiseModel &model, const gtsam::Point3 &measured)
std::set< Key > keys() const
Return keys that play in this expression.
Eigen::Vector2d Vector2
Definition: Vector.h:42
double error(const Values &c) const override
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Key R(std::uint64_t j)
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
std::shared_ptr< This > shared_ptr
Key V(std::uint64_t j)
float * p
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
double f3(double x1, double x2)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
Point2_ p(2)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
static Point2 myUncal(const Cal3_S2 &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
const KeyVector keys
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
a general SFM factor with an unknown calibration
gtsam::Expression< gtsam::Point3 > expression(const std::array< gtsam::Key, NARY_EXPRESSION_SIZE > &keys) const override
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 x
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, char *traceStorage) const
trace execution, very unsafe
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
Vector3 f(const Point2 &a, const Vector3 &b, OptionalJacobian< 3, 2 > H1, OptionalJacobian< 3, 3 > H2)
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
int main()
Expression< Vector3 > Vector3_
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:44


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