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 testTSAMFactors.cpp
14 * @brief Unit tests for TSAM 1 Factors
15 * @author Frank Dellaert
16 * @date May 2014
17 */
18
19#include <gtsam_unstable/slam/TSAMFactors.h>
20#include <gtsam/base/numericalDerivative.h>
21
22#include <CppUnitLite/TestHarness.h>
23#include "gtsam/geometry/Point2.h"
24
25using namespace std::placeholders;
26using namespace std;
27using namespace gtsam;
28
29Key i(1), j(2); // Key for pose and point
30
31//*************************************************************************
32TEST( DeltaFactor, all ) {
33 // Create a factor
34 Point2 measurement(1, 1);
35 static SharedNoiseModel model(noiseModel::Unit::Create(dim: 2));
36 DeltaFactor factor(i, j, measurement, model);
37
38 // Set the linearization point
39 Pose2 pose(1, 2, 0);
40 Point2 point(4, 11);
41 Vector2 expected(4 - 1 - 1, 11 - 2 - 1);
42
43 // Use the factor to calculate the Jacobians
44 Matrix H1Actual, H2Actual;
45 Vector actual = factor.evaluateError(x: pose, x: point, H&: H1Actual, H&: H2Actual);
46 EXPECT(assert_equal(expected, actual, 1e-9));
47
48 // Use numerical derivatives to calculate the Jacobians
49 Matrix H1Expected, H2Expected;
50
51 H1Expected = numericalDerivative11<Vector2, Pose2>(
52 h: [&factor, &point](const Pose2& pose) { return factor.evaluateError(x: pose, x: point); }, x: pose);
53 H2Expected = numericalDerivative11<Vector2, Point2>(
54 h: [&factor, &pose](const Point2& point) { return factor.evaluateError(x: pose, x: point); }, x: point);
55
56 // Verify the Jacobians are correct
57 EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
58 EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
59}
60
61//*************************************************************************
62TEST( DeltaFactorBase, all ) {
63 // Create a factor
64 Key b1(10), b2(20);
65 Point2 measurement(1, 1);
66 static SharedNoiseModel model(noiseModel::Unit::Create(dim: 2));
67 DeltaFactorBase factor(b1, i, b2, j, measurement, model);
68
69 // Set the linearization point
70 Pose2 base1, base2(1, 0, 0);
71 Pose2 pose(1, 2, 0);
72 Point2 point(4, 11);
73 Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1);
74
75 // Use the factor to calculate the Jacobians
76 Matrix H1Actual, H2Actual, H3Actual, H4Actual;
77 Vector actual = factor.evaluateError(x: base1, x: pose, x: base2, x: point, H&: H1Actual,
78 H&: H2Actual, H&: H3Actual, H&: H4Actual);
79 EXPECT(assert_equal(expected, actual, 1e-9));
80
81 // Use numerical derivatives to calculate the Jacobians
82 Matrix H1Expected, H2Expected, H3Expected, H4Expected;
83 H1Expected = numericalDerivative11<Vector2, Pose2>(
84 h: [&factor, &pose, &base2, &point](const Pose2& pose_arg) {
85 return factor.evaluateError(x: pose_arg, x: pose, x: base2, x: point);
86 },
87 x: base1);
88 H2Expected = numericalDerivative11<Vector2, Pose2>(
89 h: [&factor, &point, &base1, &base2](const Pose2& pose_arg) {
90 return factor.evaluateError(x: base1, x: pose_arg, x: base2, x: point);
91 },
92 x: pose);
93 H3Expected = numericalDerivative11<Vector2, Pose2>(
94 h: [&factor, &pose, &base1, &point](const Pose2& pose_arg) {
95 return factor.evaluateError(x: base1, x: pose, x: pose_arg, x: point);
96 },
97 x: base2);
98 H4Expected = numericalDerivative11<Vector2, Point2>(
99 h: [&factor, &pose, &base1, &base2](const Point2& point_arg) {
100 return factor.evaluateError(x: base1, x: pose, x: base2, x: point_arg);
101 },
102 x: point);
103
104 // Verify the Jacobians are correct
105 EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
106 EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
107 EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
108 EXPECT(assert_equal(H4Expected, H4Actual, 1e-9));
109}
110
111//*************************************************************************
112TEST( OdometryFactorBase, all ) {
113 // Create a factor
114 Key b1(10), b2(20);
115 Pose2 measurement(1, 1, 0);
116 static SharedNoiseModel model(noiseModel::Unit::Create(dim: 2));
117 OdometryFactorBase factor(b1, i, b2, j, measurement, model);
118
119 // Set the linearization pose2
120 Pose2 base1, base2(1, 0, 0);
121 Pose2 pose1(1, 2, 0), pose2(4, 11, 0);
122 Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0);
123
124 // Use the factor to calculate the Jacobians
125 Matrix H1Actual, H2Actual, H3Actual, H4Actual;
126 Vector actual = factor.evaluateError(x: base1, x: pose1, x: base2, x: pose2, H&: H1Actual,
127 H&: H2Actual, H&: H3Actual, H&: H4Actual);
128 EXPECT(assert_equal(expected, actual, 1e-9));
129
130 // Use numerical derivatives to calculate the Jacobians
131 Matrix H1Expected, H2Expected, H3Expected, H4Expected;
132 // using lambdas to replace bind
133 H1Expected = numericalDerivative11<Vector3, Pose2>(
134 h: [&factor, &pose1, &pose2, &base2](const Pose2& pose_arg) {
135 return factor.evaluateError(x: pose_arg, x: pose1, x: base2, x: pose2);
136 },
137 x: base1);
138 H2Expected = numericalDerivative11<Vector3, Pose2>(
139 h: [&factor, &pose2, &base1, &base2](const Pose2& pose_arg) {
140 return factor.evaluateError(x: base1, x: pose_arg, x: base2, x: pose2);
141 },
142 x: pose1);
143 H3Expected = numericalDerivative11<Vector3, Pose2>(
144 h: [&factor, &pose1, &base1, &pose2](const Pose2& pose_arg) {
145 return factor.evaluateError(x: base1, x: pose1, x: pose_arg, x: pose2);
146 },
147 x: base2);
148 H4Expected = numericalDerivative11<Vector3, Pose2>(
149 h: [&factor, &pose1, &base1, &base2](const Pose2& pose_arg) {
150 return factor.evaluateError(x: base1, x: pose1, x: base2, x: pose_arg);
151 },
152 x: pose2);
153
154 // Verify the Jacobians are correct
155 EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
156 EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
157 EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
158 EXPECT(assert_equal(H4Expected, H4Actual, 1e-9));
159}
160
161//*************************************************************************
162int main() {
163 TestResult tr;
164 return TestRegistry::runAllTests(result&: tr);
165}
166//*************************************************************************
167
168