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 testExpressionFactor.cpp
14 * @date September 18, 2014
15 * @author Frank Dellaert
16 * @author Paul Furgale
17 * @brief unit tests for Block Automatic Differentiation
18 */
19
20#include <CppUnitLite/TestHarness.h>
21#include <gtsam/base/Testable.h>
22#include <gtsam/nonlinear/ExpressionFactor.h>
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
24#include <gtsam/nonlinear/PriorFactor.h>
25#include <gtsam/nonlinear/expressionTesting.h>
26#include <gtsam/slam/GeneralSFMFactor.h>
27#include <gtsam/slam/ProjectionFactor.h>
28#include <gtsam/slam/expressions.h>
29
30using namespace std::placeholders;
31
32using namespace std;
33using namespace gtsam;
34
35Point2 measured(-17, 30);
36SharedNoiseModel model = noiseModel::Unit::Create(dim: 2);
37
38// This deals with the overload problem and makes the expressions factor
39// understand that we work on Point3
40Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
41
42namespace leaf {
43// Create some values
44struct MyValues: public Values {
45 MyValues() {
46 insert(j: 2, val: Point2(3, 5));
47 }
48} values;
49
50// Create leaf
51Point2_ p(2);
52}
53
54/* ************************************************************************* */
55// Leaf
56TEST(ExpressionFactor, 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.
63 ExpressionFactor<Point2> f(model, Point2(0, 0), p);
64
65 // Check values and derivatives.
66 EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9)
67 EXPECT_LONGS_EQUAL(2, f.dim())
68 std::shared_ptr<GaussianFactor> gf2 = f.linearize(x: values);
69 EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9))
70}
71
72/* ************************************************************************* */
73// Test leaf expression with noise model of different variance.
74TEST(ExpressionFactor, Model) {
75 using namespace leaf;
76
77 SharedNoiseModel model = noiseModel::Diagonal::Sigmas(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.
86 EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9)
87 EXPECT_LONGS_EQUAL(2, f.dim())
88 std::shared_ptr<GaussianFactor> gf2 = f.linearize(x: 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.
95TEST(ExpressionFactor, Constrained) {
96 using namespace leaf;
97
98 SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas: 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);
105 EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9)
106 EXPECT_LONGS_EQUAL(2, f.dim())
107 std::shared_ptr<GaussianFactor> gf2 = f.linearize(x: values);
108 EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9))
109}
110
111/* ************************************************************************* */
112// Unary(Leaf))
113TEST(ExpressionFactor, Unary) {
114
115 // Create some values
116 Values values;
117 values.insert(j: 2, val: Point3(0, 0, 1));
118
119 JacobianFactor expected( //
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
127 ExpressionFactor<Point2> f(model, measured, project(p_cam: p));
128 EXPECT_LONGS_EQUAL(2, f.dim())
129 std::shared_ptr<GaussianFactor> gf = f.linearize(x: values);
130 std::shared_ptr<JacobianFactor> jf = //
131 std::dynamic_pointer_cast<JacobianFactor>(r: 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)
138typedef Eigen::Matrix<double,9,3> Matrix93;
139Vector9 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
146typedef Eigen::Matrix<double,9,9> Matrix9;
147Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
148 if (H) *H = Matrix9::Identity();
149 return v;
150}
151
152TEST(ExpressionFactor, Wide) {
153 // Create some values
154 Values values;
155 values.insert(j: 2, val: 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(dim: 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/* ************************************************************************* */
171static Point2 myUncal(const Cal3_S2& K, const Point2& p,
172 OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) {
173 return K.uncalibrate(p, Dcal, Dp);
174}
175
176// Binary(Leaf,Leaf)
177TEST(ExpressionFactor, Binary) {
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(j: 1, val: Cal3_S2());
188 values.insert(j: 2, val: Point2(0, 0));
189
190 // Check size
191 auto traceStorage = allocAligned(size: binary.traceSize());
192 internal::ExecutionTrace<Point2> trace;
193 Point2 value = binary.traceExecution(values, trace, ptr: 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))
212TEST(ExpressionFactor, Shallow) {
213
214 // Create some values
215 Values values;
216 values.insert(j: 1, val: Pose3());
217 values.insert(j: 2, val: Point3(0, 0, 1));
218
219 // Create old-style factor to create expected value and derivatives
220 GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
221 std::make_shared<Cal3_S2>());
222 double expected_error = old.error(c: values);
223 GaussianFactor::shared_ptr expected = old.linearize(x: values);
224
225 // Create leaves
226 Pose3_ x_(1);
227 Point3_ p_(2);
228
229 // Construct expression, concise version
230 Point2_ expression = project(p_cam: transformTo(x: x_, p: 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(size: expression.traceSize());
244 internal::ExecutionTrace<Point2> trace;
245 Point2 value = expression.traceExecution(values, trace, traceStorage: 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
259 ExpressionFactor<Point2> f2(model, measured, expression);
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(x: values);
263 EXPECT(assert_equal(*expected, *gf2, 1e-9))
264}
265
266/* ************************************************************************* */
267// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
268TEST(ExpressionFactor, tree) {
269
270 // Create some values
271 Values values;
272 values.insert(j: 1, val: Pose3());
273 values.insert(j: 2, val: Point3(0, 0, 1));
274 values.insert(j: 3, val: Cal3_S2());
275
276 // Create old-style factor to create expected value and derivatives
277 GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
278 double expected_error = old.error(c: values);
279 GaussianFactor::shared_ptr expected = old.linearize(x: values);
280
281 // Create leaves
282 Pose3_ x(1);
283 Point3_ p(2);
284 Cal3_S2_ K(3);
285
286 // Create expression tree
287 Point3_ p_cam(x, &Pose3::transformTo, p);
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
292 ExpressionFactor<Point2> f(model, measured, uv_hat);
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(x: values);
296 EXPECT(assert_equal(*expected, *gf, 1e-9))
297
298 // Concise version
299 ExpressionFactor<Point2> f2(model, measured,
300 uncalibrate(K, xy_hat: project(p_cam: 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(x: values);
304 EXPECT(assert_equal(*expected, *gf2, 1e-9))
305
306 // Try ternary version
307 ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
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(x: values);
311 EXPECT(assert_equal(*expected, *gf3, 1e-9))
312}
313
314/* ************************************************************************* */
315TEST(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(dim: 3), Rot3(), R3);
323
324 // Create some values
325 Values values;
326 values.insert(j: 1, val: Rot3());
327 values.insert(j: 2, val: Rot3());
328
329 // Check unwhitenedError
330 std::vector<Matrix> H(2);
331 Vector actual = f.unwhitenedError(x: 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(x: values);
338 std::shared_ptr<JacobianFactor> jf = //
339 std::dynamic_pointer_cast<JacobianFactor>(r: gf);
340 EXPECT(assert_equal(expected, *jf,1e-9))
341}
342
343/* ************************************************************************* */
344// Test compose with arguments referring to the same rotation
345TEST(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(dim: 3), Rot3(), R3);
353
354 // Create some values
355 Values values;
356 values.insert(j: 1, val: Rot3());
357
358 // Check unwhitenedError
359 std::vector<Matrix> H(1);
360 Vector actual = f.unwhitenedError(x: 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(x: values);
367 std::shared_ptr<JacobianFactor> jf = //
368 std::dynamic_pointer_cast<JacobianFactor>(r: gf);
369 EXPECT(assert_equal(expected, *jf,1e-9))
370}
371
372/* ************************************************************************* */
373// Test compose with one arguments referring to a constant same rotation
374TEST(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(dim: 3), Rot3(), R3);
382
383 // Create some values
384 Values values;
385 values.insert(j: 3, val: Rot3());
386
387 // Check unwhitenedError
388 std::vector<Matrix> H(1);
389 Vector actual = f.unwhitenedError(x: 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(x: values);
396 std::shared_ptr<JacobianFactor> jf = //
397 std::dynamic_pointer_cast<JacobianFactor>(r: gf);
398 EXPECT(assert_equal(expected, *jf,1e-9))
399}
400
401/* ************************************************************************* */
402// Test compose with three arguments
403Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
404 OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
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
415TEST(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(dim: 3), Rot3(), ABC);
423
424 // Create some values
425 Values values;
426 values.insert(j: 1, val: Rot3());
427 values.insert(j: 2, val: Rot3());
428 values.insert(j: 3, val: Rot3());
429
430 // Check unwhitenedError
431 std::vector<Matrix> H(3);
432 Vector actual = f.unwhitenedError(x: 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(x: values);
441 std::shared_ptr<JacobianFactor> jf = //
442 std::dynamic_pointer_cast<JacobianFactor>(r: gf);
443 EXPECT(assert_equal(expected, *jf,1e-9))
444}
445
446TEST(ExpressionFactor, tree_finite_differences) {
447
448 // Create some values
449 Values values;
450 values.insert(j: 1, val: Pose3());
451 values.insert(j: 2, val: Point3(0, 0, 1));
452 values.insert(j: 3, val: Cal3_S2());
453
454 // Create leaves
455 Pose3_ x(1);
456 Point3_ p(2);
457 Cal3_S2_ K(3);
458
459 // Create expression tree
460 Point3_ p_cam(x, &Pose3::transformTo, p);
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
469TEST(ExpressionFactor, push_back) {
470 NonlinearFactorGraph graph;
471 graph.addExpressionFactor(R: model, z: Point2(0, 0), h: leaf::p);
472}
473
474/* ************************************************************************* */
475// Test with multiple compositions on duplicate keys
476struct 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,
480 OptionalJacobian<1, 1> H2) {
481 if (H1) (*H1) << a;
482 if (H2) (*H2) << b;
483 return a * x + b * y;
484 }
485};
486
487TEST(Expression, testMultipleCompositions) {
488 const double tolerance = 1e-5;
489 const double fd_step = 1e-5;
490
491 Values values;
492 values.insert(j: 1, val: 10.0);
493 values.insert(j: 2, val: 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
530static double combine3(const double& x, const double& y, const double& z,
531 OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2,
532 OptionalJacobian<1, 1> H3) {
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
539TEST(Expression, testMultipleCompositions2) {
540 const double tolerance = 1e-5;
541 const double fd_step = 1e-5;
542
543 Values values;
544 values.insert(j: 1, val: 10.0);
545 values.insert(j: 2, val: 20.0);
546 values.insert(j: 3, val: 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
571TEST(ExpressionFactor, MultiplyWithInverse) {
572 auto model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1);
573
574 // Create expression
575 Vector3_ f_expr(MultiplyWithInverse<3>(), Expression<Matrix3>(0), Vector3_(1));
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>(j: 0, val: A);
584 values.insert<Vector3>(j: 1, val: 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
591namespace test_operator {
592Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1,
593 OptionalJacobian<3, 3> H2) {
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
604TEST(ExpressionFactor, MultiplyWithInverseFunction) {
605 auto model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1);
606
607 using test_operator::f;
608 Vector3_ f_expr(MultiplyWithInverseFunction<Point2, 3>(f),
609 Expression<Point2>(0), Vector3_(1));
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, H2: A);
617 CHECK(assert_equal(A * b, Ab))
618 CHECK(assert_equal(
619 numericalDerivative11<Vector3, Point2>(
620 [&](const Point2& a) { return f(a, b, {}, {}); }, a),
621 H1))
622
623 Values values;
624 values.insert<Point2>(j: 0, val: a);
625 values.insert<Vector3>(j: 1, val: 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
633class TestNaryFactor
634 : public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
635 gtsam::Rot3, gtsam::Point3,
636 gtsam::Rot3, gtsam::Point3> {
637private:
638 using This = TestNaryFactor;
639 using Base =
640 gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
641 gtsam::Rot3, gtsam::Point3, gtsam::Rot3, gtsam::Point3>;
642
643public:
644 /// default constructor
645 TestNaryFactor() = default;
646
647 TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2,
648 const gtsam::SharedNoiseModel &model, const gtsam::Point3& measured)
649 : Base({kR1, kV1, kR2, kV2}, model, measured) {
650 this->initialize(expression: expression(keys: {kR1, kV1, kR2, kV2}));
651 }
652
653 /// @return a deep copy of this factor
654 gtsam::NonlinearFactor::shared_ptr clone() const override {
655 return std::static_pointer_cast<gtsam::NonlinearFactor>(
656 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));
657 }
658
659 // Return measurement expression
660 gtsam::Expression<gtsam::Point3> expression(
661 const std::array<gtsam::Key, NARY_EXPRESSION_SIZE> &keys) const override {
662 gtsam::Expression<gtsam::Rot3> R1_(keys[0]);
663 gtsam::Expression<gtsam::Point3> V1_(keys[1]);
664 gtsam::Expression<gtsam::Rot3> R2_(keys[2]);
665 gtsam::Expression<gtsam::Point3> V2_(keys[3]);
666 return {gtsam::rotate(x: R1_, p: V1_) - gtsam::rotate(x: R2_, p: V2_)};
667 }
668
669 /** print */
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(m: measured_, str: " measured: ");
679 this->noiseModel_->print(name: " noise model: ");
680 }
681
682 /** equals */
683 bool equals(const gtsam::NonlinearFactor &expected,
684 double tol = 1e-9) const override {
685 const This *e = dynamic_cast<const This *>(&expected);
686 return e != nullptr && Base::equals(f: *e, tol) &&
687 gtsam::traits<gtsam::Point3>::Equals(v1: measured_,v2: e->measured_, tol);
688 }
689
690private:
691#if GTSAM_ENABLE_BOOST_SERIALIZATION
692 /** Serialization function */
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 n: "TestNaryFactor",
698 v&: boost::serialization::base_object<Base>(d&: *this));
699 ar &BOOST_SERIALIZATION_NVP(measured_);
700 }
701#endif
702};
703
704TEST(ExpressionFactor, variadicTemplate) {
705 using gtsam::symbol_shorthand::R;
706 using gtsam::symbol_shorthand::V;
707
708 // Create factor
709 TestNaryFactor f(R(j: 0),V(j: 0), R(j: 1), V(j: 1), noiseModel::Unit::Create(dim: 3), Point3(0,0,0));
710
711 // Create some values
712 Values values;
713 values.insert(j: R(j: 0), val: Rot3::Ypr(y: 0.1, p: 0.2, r: 0.3));
714 values.insert(j: V(j: 0), val: Point3(1, 2, 3));
715 values.insert(j: R(j: 1), val: Rot3::Ypr(y: 0.2, p: 0.5, r: 0.2));
716 values.insert(j: V(j: 1), val: Point3(5, 6, 7));
717
718 // Check unwhitenedError
719 std::vector<Matrix> H(4);
720 Vector actual = f.unwhitenedError(x: 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
727TEST(ExpressionFactor, normalize) {
728 auto model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1);
729
730 // Create expression
731 const auto x = Vector3_(1);
732 Vector3_ f_expr = normalize(a: x);
733
734 // Check derivatives
735 Values values;
736 values.insert(j: 1, val: Vector3(1, 2, 3));
737 ExpressionFactor<Vector3> factor(model, Vector3(1.0/sqrt(x: 14), 2.0/sqrt(x: 14), 3.0/sqrt(x: 14)), f_expr);
738 EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
739}
740
741TEST(ExpressionFactor, crossProduct) {
742 auto model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 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(j: 1, val: Vector3(0.1, 0.2, 0.3));
752 values.insert(j: 2, val: 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
757TEST(ExpressionFactor, dotProduct) {
758 auto model = noiseModel::Isotropic::Sigma(dim: 1, sigma: 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(j: 1, val: Vector3(0.1, 0.2, 0.3));
768 values.insert(j: 2, val: 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/* ************************************************************************* */
775int main() {
776 TestResult tr;
777 return TestRegistry::runAllTests(result&: tr);
778}
779/* ************************************************************************* */
780
781