1/**
2 * @file testNonlinearISAM
3 * @author Alex Cunningham
4 */
5
6#include <CppUnitLite/TestHarness.h>
7
8#include <gtsam/slam/BetweenFactor.h>
9#include <gtsam/sam/BearingRangeFactor.h>
10#include <gtsam/slam/dataset.h>
11#include <gtsam/nonlinear/NonlinearEquality.h>
12#include <gtsam/nonlinear/NonlinearISAM.h>
13#include <gtsam/nonlinear/NonlinearFactorGraph.h>
14#include <gtsam/nonlinear/Values.h>
15#include <gtsam/inference/Symbol.h>
16#include <gtsam/linear/Sampler.h>
17#include <gtsam/geometry/Pose2.h>
18
19#include <iostream>
20#include <sstream>
21
22using namespace std;
23using namespace gtsam;
24
25const double tol=1e-5;
26
27/* ************************************************************************* */
28TEST(testNonlinearISAM, markov_chain ) {
29 int reorder_interval = 2;
30 NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
31 NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
32
33 SharedDiagonal model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(3.0, 3.0, 0.5));
34 Sampler sampler(model, 42u);
35
36 // create initial graph
37 Pose2 cur_pose; // start at origin
38 NonlinearFactorGraph start_factors;
39 start_factors.emplace_shared<NonlinearEquality<Pose2>>(args: 0, args&: cur_pose);
40
41 Values init;
42 Values expected;
43 init.insert(j: 0, val: cur_pose);
44 expected.insert(j: 0, val: cur_pose);
45 isamChol.update(newFactors: start_factors, initialValues: init);
46 isamQR.update(newFactors: start_factors, initialValues: init);
47
48 // loop for a period of time to verify memory usage
49 size_t nrPoses = 21;
50 Pose2 z(1.0, 2.0, 0.1);
51 for (size_t i=1; i<=nrPoses; ++i) {
52 NonlinearFactorGraph new_factors;
53 new_factors.emplace_shared<BetweenFactor<Pose2>>(args: i-1, args&: i, args&: z, args&: model);
54 Values new_init;
55
56 cur_pose = cur_pose.compose(g: z);
57 new_init.insert(j: i, val: cur_pose.retract(v: sampler.sample()));
58 expected.insert(j: i, val: cur_pose);
59 isamChol.update(newFactors: new_factors, initialValues: new_init);
60 isamQR.update(newFactors: new_factors, initialValues: new_init);
61 }
62
63 // verify values - all but the last one should be very close
64 Values actualChol = isamChol.estimate();
65 for (size_t i=0; i<nrPoses; ++i) {
66 EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
67 }
68 Values actualQR = isamQR.estimate();
69 for (size_t i=0; i<nrPoses; ++i) {
70 EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
71 }
72}
73
74/* ************************************************************************* */
75TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
76 int reorder_interval = 2;
77 NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
78 NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
79
80 SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(3.0, 3.0, 0.5));
81 SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(sigmas: Vector2(2.0, 2.0));
82 Sampler sampler(model3, 42u);
83
84 // create initial graph
85 Pose2 cur_pose; // start at origin
86 NonlinearFactorGraph start_factors;
87 start_factors.emplace_shared<NonlinearEquality<Pose2>>(args: 0, args&: cur_pose);
88
89 Values init;
90 Values expected;
91 init.insert(j: 0, val: cur_pose);
92 expected.insert(j: 0, val: cur_pose);
93 isamChol.update(newFactors: start_factors, initialValues: init);
94 isamQR.update(newFactors: start_factors, initialValues: init);
95
96 size_t nrPoses = 21;
97
98 // create a constrained constellation of landmarks
99 Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3;
100 Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.);
101 expected.insert(j: lm1, val: landmark1);
102 expected.insert(j: lm2, val: landmark2);
103 expected.insert(j: lm3, val: landmark3);
104
105 // loop for a period of time to verify memory usage
106 Pose2 z(1.0, 2.0, 0.1);
107 for (size_t i=1; i<=nrPoses; ++i) {
108 NonlinearFactorGraph new_factors;
109 new_factors.emplace_shared<BetweenFactor<Pose2>>(args: i-1, args&: i, args&: z, args&: model3);
110 Values new_init;
111
112 cur_pose = cur_pose.compose(g: z);
113 new_init.insert(j: i, val: cur_pose.retract(v: sampler.sample()));
114 expected.insert(j: i, val: cur_pose);
115
116 // Add a floating landmark constellation
117 if (i == 7) {
118 new_factors.addPrior(key: lm1, prior: landmark1, model: model2);
119 new_factors.addPrior(key: lm2, prior: landmark2, model: model2);
120 new_factors.addPrior(key: lm3, prior: landmark3, model: model2);
121
122 // Initialize to origin
123 new_init.insert(j: lm1, val: Point2(0,0));
124 new_init.insert(j: lm2, val: Point2(0,0));
125 new_init.insert(j: lm3, val: Point2(0,0));
126 }
127
128 isamChol.update(newFactors: new_factors, initialValues: new_init);
129 isamQR.update(newFactors: new_factors, initialValues: new_init);
130 }
131
132 // verify values - all but the last one should be very close
133 Values actualChol = isamChol.estimate();
134 for (size_t i=0; i<nrPoses; ++i)
135 EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
136
137 Values actualQR = isamQR.estimate();
138 for (size_t i=0; i<nrPoses; ++i)
139 EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
140
141 // Check landmarks
142 EXPECT(assert_equal(expected.at<Point2>(lm1), actualChol.at<Point2>(lm1), tol));
143 EXPECT(assert_equal(expected.at<Point2>(lm2), actualChol.at<Point2>(lm2), tol));
144 EXPECT(assert_equal(expected.at<Point2>(lm3), actualChol.at<Point2>(lm3), tol));
145
146 EXPECT(assert_equal(expected.at<Point2>(lm1), actualQR.at<Point2>(lm1), tol));
147 EXPECT(assert_equal(expected.at<Point2>(lm2), actualQR.at<Point2>(lm2), tol));
148 EXPECT(assert_equal(expected.at<Point2>(lm3), actualQR.at<Point2>(lm3), tol));
149}
150
151/* ************************************************************************* */
152TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
153 int reorder_interval = 2;
154 NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
155 NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
156
157 SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(3.0, 3.0, 0.5));
158 SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(sigmas: Vector2(2.0, 2.0));
159 Sampler sampler(model3, 42u);
160
161 // create initial graph
162 Pose2 cur_pose; // start at origin
163 NonlinearFactorGraph start_factors;
164 start_factors.emplace_shared<NonlinearEquality<Pose2>>(args: 0, args&: cur_pose);
165
166 Values init;
167 Values expected;
168 init.insert(j: 0, val: cur_pose);
169 expected.insert(j: 0, val: cur_pose);
170 isamChol.update(newFactors: start_factors, initialValues: init);
171 isamQR.update(newFactors: start_factors, initialValues: init);
172
173 size_t nrPoses = 21;
174
175 // create a constrained constellation of landmarks
176 Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3;
177 Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.);
178 expected.insert(j: lm1, val: landmark1);
179 expected.insert(j: lm2, val: landmark2);
180 expected.insert(j: lm3, val: landmark3);
181
182 // loop for a period of time to verify memory usage
183 Pose2 z(1.0, 2.0, 0.1);
184 for (size_t i=1; i<=nrPoses; ++i) {
185 NonlinearFactorGraph new_factors;
186 new_factors.emplace_shared<BetweenFactor<Pose2>>(args: i-1, args&: i, args&: z, args&: model3);
187 Values new_init;
188
189 cur_pose = cur_pose.compose(g: z);
190 new_init.insert(j: i, val: cur_pose.retract(v: sampler.sample()));
191 expected.insert(j: i, val: cur_pose);
192
193 // Add a floating landmark constellation
194 if (i == 7) {
195 new_factors.addPrior(key: lm1, prior: landmark1, model: model2);
196 new_factors.addPrior(key: lm2, prior: landmark2, model: model2);
197 new_factors.addPrior(key: lm3, prior: landmark3, model: model2);
198
199 // Initialize to origin
200 new_init.insert(j: lm1, val: Point2(0,0));
201 new_init.insert(j: lm2, val: Point2(0,0));
202 new_init.insert(j: lm3, val: Point2(0,0));
203 }
204
205 // Reconnect with observation later
206 if (i == 15) {
207 new_factors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
208 args&: i, args&: lm1, args: cur_pose.bearing(point: landmark1), args: cur_pose.range(point: landmark1), args&: model2);
209 }
210
211 isamChol.update(newFactors: new_factors, initialValues: new_init);
212 isamQR.update(newFactors: new_factors, initialValues: new_init);
213 }
214
215 // verify values - all but the last one should be very close
216 Values actualChol = isamChol.estimate();
217 for (size_t i=0; i<nrPoses; ++i)
218 EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), 1e-4));
219
220 Values actualQR = isamQR.estimate();
221 for (size_t i=0; i<nrPoses; ++i)
222 EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), 1e-4));
223
224 // Check landmarks
225 EXPECT(assert_equal(expected.at<Point2>(lm1), actualChol.at<Point2>(lm1), tol));
226 EXPECT(assert_equal(expected.at<Point2>(lm2), actualChol.at<Point2>(lm2), tol));
227 EXPECT(assert_equal(expected.at<Point2>(lm3), actualChol.at<Point2>(lm3), tol));
228
229 EXPECT(assert_equal(expected.at<Point2>(lm1), actualQR.at<Point2>(lm1), tol));
230 EXPECT(assert_equal(expected.at<Point2>(lm2), actualQR.at<Point2>(lm2), tol));
231 EXPECT(assert_equal(expected.at<Point2>(lm3), actualQR.at<Point2>(lm3), tol));
232}
233
234/* ************************************************************************* */
235TEST(testNonlinearISAM, loop_closures ) {
236 int relinearizeInterval = 100;
237 NonlinearISAM isam(relinearizeInterval);
238
239 // Create a Factor Graph and Values to hold the new data
240 NonlinearFactorGraph graph;
241 Values initialEstimate;
242
243 vector<string> lines;
244 lines.emplace_back(args: "VERTEX2 0 0.000000 0.000000 0.000000");
245 lines.emplace_back(args: "EDGE2 1 0 1.030390 0.011350 -0.012958");
246 lines.emplace_back(args: "VERTEX2 1 1.030390 0.011350 -0.012958");
247 lines.emplace_back(args: "EDGE2 2 1 1.013900 -0.058639 -0.013225");
248 lines.emplace_back(args: "VERTEX2 2 2.043445 -0.060422 -0.026183");
249 lines.emplace_back(args: "EDGE2 3 2 1.027650 -0.007456 0.004833");
250 lines.emplace_back(args: "VERTEX2 3 3.070548 -0.094779 -0.021350");
251 lines.emplace_back(args: "EDGE2 4 3 -0.012016 1.004360 1.566790");
252 lines.emplace_back(args: "VERTEX2 4 3.079976 0.909609 1.545440");
253 lines.emplace_back(args: "EDGE2 5 4 1.016030 0.014565 -0.016304");
254 lines.emplace_back(args: "VERTEX2 5 3.091176 1.925681 1.529136");
255 lines.emplace_back(args: "EDGE2 6 5 1.023890 0.006808 0.010981");
256 lines.emplace_back(args: "VERTEX2 6 3.127018 2.948966 1.540117");
257 lines.emplace_back(args: "EDGE2 7 6 0.957734 0.003159 0.010901");
258 lines.emplace_back(args: "VERTEX2 7 3.153237 3.906347 1.551018");
259 lines.emplace_back(args: "EDGE2 8 7 -1.023820 -0.013668 -3.093240");
260 lines.emplace_back(args: "VERTEX2 8 3.146655 2.882457 -1.542222");
261 lines.emplace_back(args: "EDGE2 9 8 1.023440 0.013984 -0.007802");
262 lines.emplace_back(args: "EDGE2 9 5 0.033943 0.032439 -3.127400");
263 lines.emplace_back(args: "VERTEX2 9 3.189873 1.859834 -1.550024");
264 lines.emplace_back(args: "EDGE2 10 9 1.003350 0.022250 0.023491");
265 lines.emplace_back(args: "EDGE2 10 3 0.044020 0.988477 -1.563530");
266 lines.emplace_back(args: "VERTEX2 10 3.232959 0.857162 -1.526533");
267 lines.emplace_back(args: "EDGE2 11 10 0.977245 0.019042 -0.028623");
268 lines.emplace_back(args: "VERTEX2 11 3.295225 -0.118283 -1.555156");
269 lines.emplace_back(args: "EDGE2 12 11 -0.996880 -0.025512 -3.126915");
270 lines.emplace_back(args: "VERTEX2 12 3.254125 0.878076 1.601114");
271 lines.emplace_back(args: "EDGE2 13 12 0.990646 0.018396 -0.016519");
272 lines.emplace_back(args: "VERTEX2 13 3.205708 1.867709 1.584594");
273 lines.emplace_back(args: "EDGE2 14 13 0.945873 0.008893 -0.002602");
274 lines.emplace_back(args: "EDGE2 14 8 0.015808 0.021059 3.128310");
275 lines.emplace_back(args: "VERTEX2 14 3.183765 2.813370 1.581993");
276 lines.emplace_back(args: "EDGE2 15 14 1.000010 0.006428 0.028234");
277 lines.emplace_back(args: "EDGE2 15 7 -0.014728 -0.001595 -0.019579");
278 lines.emplace_back(args: "VERTEX2 15 3.166141 3.813245 1.610227");
279
280 auto model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(3.0, 3.0, 0.5));
281
282 // Loop over the different poses, adding the observations to iSAM incrementally
283 for (const string& str : lines) {
284 // scan the tag
285 string tag;
286 istringstream is(str);
287 if (!(is >> tag))
288 break;
289
290 // Check if vertex
291 const auto indexedPose = parseVertexPose(is, tag);
292 if (indexedPose) {
293 Key id = indexedPose->first;
294 initialEstimate.insert(j: Symbol('x', id), val: indexedPose->second);
295 if (id == 0) {
296 noiseModel::Diagonal::shared_ptr priorNoise =
297 noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.001, 0.001, 0.001));
298 graph.addPrior(key: Symbol('x', id), prior: Pose2(0, 0, 0), model: priorNoise);
299 } else {
300 isam.update(newFactors: graph, initialValues: initialEstimate);
301
302 // Clear the factor graph and values for the next iteration
303 graph.resize(size: 0);
304 initialEstimate.clear();
305 }
306 }
307
308 // check if edge
309 const auto betweenPose = parseEdge(is, tag);
310 if (betweenPose) {
311 size_t id1, id2;
312 tie(args&: id1, args&: id2) = betweenPose->first;
313 graph.emplace_shared<BetweenFactor<Pose2> >(args: Symbol('x', id2),
314 args: Symbol('x', id1), args: betweenPose->second, args&: model);
315 }
316 }
317 EXPECT_LONGS_EQUAL(16, isam.estimate().size())
318}
319
320/* ************************************************************************* */
321int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
322/* ************************************************************************* */
323