1/**
2 * @file testGaussianISAM2.cpp
3 * @brief Unit tests for GaussianISAM2
4 * @author Michael Kaess
5 */
6
7#include <gtsam/nonlinear/ISAM2.h>
8
9#include <tests/smallExample.h>
10#include <gtsam/slam/BetweenFactor.h>
11#include <gtsam/sam/BearingRangeFactor.h>
12#include <gtsam/geometry/Point2.h>
13#include <gtsam/geometry/Pose2.h>
14#include <gtsam/geometry/Pose3.h>
15#include <gtsam/nonlinear/Values.h>
16#include <gtsam/nonlinear/NonlinearFactorGraph.h>
17#include <gtsam/nonlinear/Marginals.h>
18#include <gtsam/linear/GaussianBayesNet.h>
19#include <gtsam/linear/GaussianBayesTree.h>
20#include <gtsam/linear/GaussianFactorGraph.h>
21#include <gtsam/inference/Ordering.h>
22#include <gtsam/base/debug.h>
23#include <gtsam/base/TestableAssertions.h>
24#include <gtsam/base/treeTraversal-inst.h>
25
26#include <CppUnitLite/TestHarness.h>
27
28#include <cassert>
29
30using namespace std;
31using namespace gtsam;
32using std::shared_ptr;
33
34static const SharedNoiseModel model;
35
36// SETDEBUG("ISAM2 update", true);
37// SETDEBUG("ISAM2 update verbose", true);
38// SETDEBUG("ISAM2 recalculate", true);
39
40// Set up parameters
41SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(sigmas: (Vector(3) << 0.1, 0.1, M_PI/100.0).finished());
42SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(sigmas: (Vector(2) << M_PI/100.0, 0.1).finished());
43
44ISAM2 createSlamlikeISAM2(
45 Values* init_values = nullptr,
46 NonlinearFactorGraph* full_graph = nullptr,
47 const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0,
48 0, false, true,
49 ISAM2Params::CHOLESKY, true,
50 DefaultKeyFormatter, true),
51 size_t maxPoses = 10) {
52 // These variables will be reused and accumulate factors and values
53 ISAM2 isam(params);
54 Values fullinit;
55 NonlinearFactorGraph fullgraph;
56
57 // i keeps track of the time step
58 size_t i = 0;
59
60 // Add a prior at time 0 and update isam
61 {
62 NonlinearFactorGraph newfactors;
63 newfactors.addPrior(key: 0, prior: Pose2(0.0, 0.0, 0.0), model: odoNoise);
64 fullgraph.push_back(container: newfactors);
65
66 Values init;
67 init.insert(j: (0), val: Pose2(0.01, 0.01, 0.01));
68 fullinit.insert(j: (0), val: Pose2(0.01, 0.01, 0.01));
69
70 isam.update(newFactors: newfactors, newTheta: init);
71 }
72
73 if(i > maxPoses)
74 goto done;
75
76 // Add odometry from time 0 to time 5
77 for( ; i<5; ++i) {
78 NonlinearFactorGraph newfactors;
79 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
80 fullgraph.push_back(container: newfactors);
81
82 Values init;
83 init.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
84 fullinit.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
85
86 isam.update(newFactors: newfactors, newTheta: init);
87
88 if(i > maxPoses)
89 goto done;
90 }
91
92 if(i > maxPoses)
93 goto done;
94
95 // Add odometry from time 5 to 6 and landmark measurement at time 5
96 {
97 NonlinearFactorGraph newfactors;
98 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
99 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 100, args: Rot2::fromAngle(M_PI/4.0), args: 5.0, args&: brNoise);
100 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 101, args: Rot2::fromAngle(theta: -M_PI/4.0), args: 5.0, args&: brNoise);
101 fullgraph.push_back(container: newfactors);
102
103 Values init;
104 init.insert(j: (i+1), val: Pose2(1.01, 0.01, 0.01));
105 init.insert(j: 100, val: Point2(5.0/sqrt(x: 2.0), 5.0/sqrt(x: 2.0)));
106 init.insert(j: 101, val: Point2(5.0/sqrt(x: 2.0), -5.0/sqrt(x: 2.0)));
107 fullinit.insert(j: (i+1), val: Pose2(1.01, 0.01, 0.01));
108 fullinit.insert(j: 100, val: Point2(5.0/sqrt(x: 2.0), 5.0/sqrt(x: 2.0)));
109 fullinit.insert(j: 101, val: Point2(5.0/sqrt(x: 2.0), -5.0/sqrt(x: 2.0)));
110
111 isam.update(newFactors: newfactors, newTheta: init);
112 ++ i;
113 }
114
115 if(i > maxPoses)
116 goto done;
117
118 // Add odometry from time 6 to time 10
119 for( ; i<10; ++i) {
120 NonlinearFactorGraph newfactors;
121 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
122 fullgraph.push_back(container: newfactors);
123
124 Values init;
125 init.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
126 fullinit.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
127
128 isam.update(newFactors: newfactors, newTheta: init);
129
130 if(i > maxPoses)
131 goto done;
132 }
133
134 if(i > maxPoses)
135 goto done;
136
137 // Add odometry from time 10 to 11 and landmark measurement at time 10
138 {
139 NonlinearFactorGraph newfactors;
140 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
141 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 100, args: Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), args: 4.5, args&: brNoise);
142 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 101, args: Rot2::fromAngle(theta: -M_PI/4.0 + M_PI/16.0), args: 4.5, args&: brNoise);
143 fullgraph.push_back(container: newfactors);
144
145 Values init;
146 init.insert(j: (i+1), val: Pose2(6.9, 0.1, 0.01));
147 fullinit.insert(j: (i+1), val: Pose2(6.9, 0.1, 0.01));
148
149 isam.update(newFactors: newfactors, newTheta: init);
150 ++ i;
151 }
152
153done:
154
155 if (full_graph)
156 *full_graph = fullgraph;
157
158 if (init_values)
159 *init_values = fullinit;
160
161 return isam;
162}
163
164/* ************************************************************************* */
165//TEST(ISAM2, CheckRelinearization) {
166//
167// typedef GaussianISAM2<Values>::Impl Impl;
168//
169// // Create values where indices 1 and 3 are above the threshold of 0.1
170// VectorValues values;
171// values.reserve(4, 10);
172// values.push_back_preallocated(Vector2(0.09, 0.09));
173// values.push_back_preallocated(Vector3(0.11, 0.11, 0.09));
174// values.push_back_preallocated(Vector3(0.09, 0.09, 0.09));
175// values.push_back_preallocated(Vector2(0.11, 0.11));
176//
177// // Create a permutation
178// Permutation permutation(4);
179// permutation[0] = 2;
180// permutation[1] = 0;
181// permutation[2] = 1;
182// permutation[3] = 3;
183//
184// Permuted<VectorValues> permuted(permutation, values);
185//
186// // After permutation, the indices above the threshold are 2 and 2
187// KeySet expected;
188// expected.insert(2);
189// expected.insert(3);
190//
191// // Indices checked by CheckRelinearization
192// KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
193//
194// EXPECT(assert_equal(expected, actual));
195//}
196
197/* ************************************************************************* */
198struct ConsistencyVisitor
199{
200 bool consistent;
201 const ISAM2& isam;
202 ConsistencyVisitor(const ISAM2& isam) :
203 consistent(true), isam(isam) {}
204 int operator()(const ISAM2::sharedClique& node, int& parentData)
205 {
206 if(find(first: isam.roots().begin(), last: isam.roots().end(), val: node) == isam.roots().end())
207 {
208 if(node->parent_.expired())
209 consistent = false;
210 if(find(first: node->parent()->children.begin(), last: node->parent()->children.end(), val: node) == node->parent()->children.end())
211 consistent = false;
212 }
213 for(Key j: node->conditional()->frontals())
214 {
215 if(isam.nodes().at(k: j).get() != node.get())
216 consistent = false;
217 }
218 return 0;
219 }
220};
221
222/* ************************************************************************* */
223bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
224
225 TestResult& result_ = result;
226 const string name_ = test.getName();
227
228 Values actual = isam.calculateEstimate();
229 Values expected = fullinit.retract(delta: fullgraph.linearize(linearizationPoint: fullinit)->optimize());
230
231 bool isamEqual = assert_equal(expected, actual);
232
233 // Check information
234 GaussianFactorGraph isamGraph(isam);
235 isamGraph.push_back(factor: isam.roots().front()->cachedFactor_);
236 Matrix expectedHessian = fullgraph.linearize(linearizationPoint: isam.getLinearizationPoint())->augmentedHessian();
237 Matrix actualHessian = isamGraph.augmentedHessian();
238 expectedHessian.bottomRightCorner(cRows: 1,cCols: 1) = actualHessian.bottomRightCorner(cRows: 1,cCols: 1);
239 bool isamTreeEqual = assert_equal(A: expectedHessian, B: actualHessian);
240
241 // Check consistency
242 ConsistencyVisitor visitor(isam);
243 int data; // Unused
244 treeTraversal::DepthFirstForest(forest: isam, rootData&: data, visitorPre&: visitor);
245 bool consistent = visitor.consistent;
246
247 // The following two checks make sure that the cached gradients are maintained and used correctly
248
249 // Check gradient at each node
250 bool nodeGradientsOk = true;
251 for (const auto& [key, clique] : isam.nodes()) {
252 // Compute expected gradient
253 GaussianFactorGraph jfg;
254 jfg.push_back(factor: clique->conditional());
255 VectorValues expectedGradient = jfg.gradientAtZero();
256 // Compare with actual gradients
257 DenseIndex variablePosition = 0;
258 for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
259 const DenseIndex dim = clique->conditional()->getDim(variable: jit);
260 Vector actual = clique->gradientContribution().segment(start: variablePosition, n: dim);
261 bool gradOk = assert_equal(vec1: expectedGradient[*jit], vec2: actual);
262 EXPECT(gradOk);
263 nodeGradientsOk = nodeGradientsOk && gradOk;
264 variablePosition += dim;
265 }
266 bool dimOk = clique->gradientContribution().rows() == variablePosition;
267 EXPECT(dimOk);
268 nodeGradientsOk = nodeGradientsOk && dimOk;
269 }
270
271 // Check gradient
272 VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
273 VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(x0: VectorValues::Zero(other: expectedGradient));
274 VectorValues actualGradient = isam.gradientAtZero();
275 bool expectedGradOk = assert_equal(expected: expectedGradient2, actual: expectedGradient);
276 EXPECT(expectedGradOk);
277 bool totalGradOk = assert_equal(expected: expectedGradient, actual: actualGradient);
278 EXPECT(totalGradOk);
279
280 return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
281}
282
283/* ************************************************************************* */
284TEST(ISAM2, simple)
285{
286 for(size_t i = 0; i < 10; ++i) {
287 // These variables will be reused and accumulate factors and values
288 Values fullinit;
289 NonlinearFactorGraph fullgraph;
290 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), maxPoses: i);
291
292 // Compare solutions
293 EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
294 }
295}
296
297/* ************************************************************************* */
298TEST(ISAM2, slamlike_solution_gaussnewton)
299{
300 // These variables will be reused and accumulate factors and values
301 Values fullinit;
302 NonlinearFactorGraph fullgraph;
303 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
304
305 // Compare solutions
306 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
307}
308
309/* ************************************************************************* */
310TEST(ISAM2, slamlike_solution_dogleg)
311{
312 // These variables will be reused and accumulate factors and values
313 Values fullinit;
314 NonlinearFactorGraph fullgraph;
315 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
316
317 // Compare solutions
318 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
319}
320
321/* ************************************************************************* */
322TEST(ISAM2, slamlike_solution_gaussnewton_qr)
323{
324 // These variables will be reused and accumulate factors and values
325 Values fullinit;
326 NonlinearFactorGraph fullgraph;
327 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
328
329 // Compare solutions
330 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
331}
332
333/* ************************************************************************* */
334TEST(ISAM2, slamlike_solution_dogleg_qr)
335{
336 // These variables will be reused and accumulate factors and values
337 Values fullinit;
338 NonlinearFactorGraph fullgraph;
339 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
340
341 // Compare solutions
342 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
343}
344
345/* ************************************************************************* */
346TEST(ISAM2, clone) {
347
348 ISAM2 clone1;
349
350 {
351 ISAM2 isam = createSlamlikeISAM2();
352 clone1 = isam;
353
354 ISAM2 clone2(isam);
355
356 // Modify original isam
357 NonlinearFactorGraph factors;
358 factors.emplace_shared<BetweenFactor<Pose2>>(args: 0, args: 10,
359 args: isam.calculateEstimate<Pose2>(key: 0).between(g: isam.calculateEstimate<Pose2>(key: 10)), args: noiseModel::Unit::Create(dim: 3));
360 isam.update(newFactors: factors);
361
362 CHECK(assert_equal(createSlamlikeISAM2(), clone2));
363 }
364
365 // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced
366 // if the references in the iSAM2 copy point to the old instance which deleted at
367 // the end of the {...} section above.
368 ISAM2 temp = createSlamlikeISAM2();
369
370 CHECK(assert_equal(createSlamlikeISAM2(), clone1));
371 CHECK(assert_equal(clone1, temp));
372
373 // Check clone empty
374 ISAM2 isam;
375 clone1 = isam;
376 CHECK(assert_equal(ISAM2(), clone1));
377}
378
379/* ************************************************************************* */
380TEST(ISAM2, removeFactors)
381{
382 // This test builds a graph in the same way as the "slamlike" test above, but
383 // then removes the 2nd-to-last landmark measurement
384
385 // These variables will be reused and accumulate factors and values
386 Values fullinit;
387 NonlinearFactorGraph fullgraph;
388 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
389
390 // Remove the 2nd measurement on landmark 0 (Key 100)
391 FactorIndices toRemove;
392 toRemove.push_back(x: 12);
393 isam.update(newFactors: NonlinearFactorGraph(), newTheta: Values(), removeFactorIndices: toRemove);
394
395 // Remove the factor from the full system
396 fullgraph.remove(i: 12);
397
398 // Compare solutions
399 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
400}
401
402/* ************************************************************************* */
403TEST(ISAM2, removeVariables)
404{
405 // These variables will be reused and accumulate factors and values
406 Values fullinit;
407 NonlinearFactorGraph fullgraph;
408 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params: ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
409
410 // Remove the measurement on landmark 0 (Key 100)
411 FactorIndices toRemove;
412 toRemove.push_back(x: 7);
413 toRemove.push_back(x: 14);
414 isam.update(newFactors: NonlinearFactorGraph(), newTheta: Values(), removeFactorIndices: toRemove);
415
416 // Remove the factors and variable from the full system
417 fullgraph.remove(i: 7);
418 fullgraph.remove(i: 14);
419 fullinit.erase(j: 100);
420
421 // Compare solutions
422 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
423}
424
425/* ************************************************************************* */
426TEST(ISAM2, swapFactors)
427{
428 // This test builds a graph in the same way as the "slamlike" test above, but
429 // then swaps the 2nd-to-last landmark measurement with a different one
430
431 Values fullinit;
432 NonlinearFactorGraph fullgraph;
433 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph);
434
435 // Remove the measurement on landmark 0 and replace with a different one
436 {
437 size_t swap_idx = isam.getFactorsUnsafe().size()-2;
438 FactorIndices toRemove;
439 toRemove.push_back(x: swap_idx);
440 fullgraph.remove(i: swap_idx);
441
442 NonlinearFactorGraph swapfactors;
443// swapfactors += BearingRange<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor
444 swapfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args: 10, args: 100, args: Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), args: 5.0, args&: brNoise);
445 fullgraph.push_back(container: swapfactors);
446 isam.update(newFactors: swapfactors, newTheta: Values(), removeFactorIndices: toRemove);
447 }
448
449 // Compare solutions
450 EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe())));
451 EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
452
453 // Check gradient at each node
454 for (const auto& [key, clique]: isam.nodes()) {
455 // Compute expected gradient
456 GaussianFactorGraph jfg;
457 jfg.push_back(factor: clique->conditional());
458 VectorValues expectedGradient = jfg.gradientAtZero();
459 // Compare with actual gradients
460 DenseIndex variablePosition = 0;
461 for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
462 const DenseIndex dim = clique->conditional()->getDim(variable: jit);
463 Vector actual = clique->gradientContribution().segment(start: variablePosition, n: dim);
464 EXPECT(assert_equal(expectedGradient[*jit], actual));
465 variablePosition += dim;
466 }
467 EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition);
468 }
469
470 // Check gradient
471 VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
472 VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(x0: VectorValues::Zero(other: expectedGradient));
473 VectorValues actualGradient = isam.gradientAtZero();
474 EXPECT(assert_equal(expectedGradient2, expectedGradient));
475 EXPECT(assert_equal(expectedGradient, actualGradient));
476}
477
478/* ************************************************************************* */
479TEST(ISAM2, constrained_ordering)
480{
481 // These variables will be reused and accumulate factors and values
482 ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
483 Values fullinit;
484 NonlinearFactorGraph fullgraph;
485
486 // We will constrain x3 and x4 to the end
487 FastMap<Key, int> constrained;
488 constrained.insert(x: make_pair(x: (3), y: 1));
489 constrained.insert(x: make_pair(x: (4), y: 2));
490
491 // i keeps track of the time step
492 size_t i = 0;
493
494 // Add a prior at time 0 and update isam
495 {
496 NonlinearFactorGraph newfactors;
497 newfactors.addPrior(key: 0, prior: Pose2(0.0, 0.0, 0.0), model: odoNoise);
498 fullgraph.push_back(container: newfactors);
499
500 Values init;
501 init.insert(j: (0), val: Pose2(0.01, 0.01, 0.01));
502 fullinit.insert(j: (0), val: Pose2(0.01, 0.01, 0.01));
503
504 isam.update(newFactors: newfactors, newTheta: init);
505 }
506
507 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
508
509 // Add odometry from time 0 to time 5
510 for( ; i<5; ++i) {
511 NonlinearFactorGraph newfactors;
512 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
513 fullgraph.push_back(container: newfactors);
514
515 Values init;
516 init.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
517 fullinit.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
518
519 if(i >= 3)
520 isam.update(newFactors: newfactors, newTheta: init, removeFactorIndices: FactorIndices(), constrainedKeys: constrained);
521 else
522 isam.update(newFactors: newfactors, newTheta: init);
523 }
524
525 // Add odometry from time 5 to 6 and landmark measurement at time 5
526 {
527 NonlinearFactorGraph newfactors;
528 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
529 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 100, args: Rot2::fromAngle(M_PI/4.0), args: 5.0, args&: brNoise);
530 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 101, args: Rot2::fromAngle(theta: -M_PI/4.0), args: 5.0, args&: brNoise);
531 fullgraph.push_back(container: newfactors);
532
533 Values init;
534 init.insert(j: (i+1), val: Pose2(1.01, 0.01, 0.01));
535 init.insert(j: 100, val: Point2(5.0/sqrt(x: 2.0), 5.0/sqrt(x: 2.0)));
536 init.insert(j: 101, val: Point2(5.0/sqrt(x: 2.0), -5.0/sqrt(x: 2.0)));
537 fullinit.insert(j: (i+1), val: Pose2(1.01, 0.01, 0.01));
538 fullinit.insert(j: 100, val: Point2(5.0/sqrt(x: 2.0), 5.0/sqrt(x: 2.0)));
539 fullinit.insert(j: 101, val: Point2(5.0/sqrt(x: 2.0), -5.0/sqrt(x: 2.0)));
540
541 isam.update(newFactors: newfactors, newTheta: init, removeFactorIndices: FactorIndices(), constrainedKeys: constrained);
542 ++ i;
543 }
544
545 // Add odometry from time 6 to time 10
546 for( ; i<10; ++i) {
547 NonlinearFactorGraph newfactors;
548 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
549 fullgraph.push_back(container: newfactors);
550
551 Values init;
552 init.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
553 fullinit.insert(j: (i+1), val: Pose2(double(i+1)+0.1, -0.1, 0.01));
554
555 isam.update(newFactors: newfactors, newTheta: init, removeFactorIndices: FactorIndices(), constrainedKeys: constrained);
556 }
557
558 // Add odometry from time 10 to 11 and landmark measurement at time 10
559 {
560 NonlinearFactorGraph newfactors;
561 newfactors.emplace_shared<BetweenFactor<Pose2>>(args&: i, args: i+1, args: Pose2(1.0, 0.0, 0.0), args&: odoNoise);
562 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 100, args: Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), args: 4.5, args&: brNoise);
563 newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: i, args: 101, args: Rot2::fromAngle(theta: -M_PI/4.0 + M_PI/16.0), args: 4.5, args&: brNoise);
564 fullgraph.push_back(container: newfactors);
565
566 Values init;
567 init.insert(j: (i+1), val: Pose2(6.9, 0.1, 0.01));
568 fullinit.insert(j: (i+1), val: Pose2(6.9, 0.1, 0.01));
569
570 isam.update(newFactors: newfactors, newTheta: init, removeFactorIndices: FactorIndices(), constrainedKeys: constrained);
571 ++ i;
572 }
573
574 // Compare solutions
575 EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
576
577 // Check gradient at each node
578 for (const auto& [key, clique]: isam.nodes()) {
579 // Compute expected gradient
580 GaussianFactorGraph jfg;
581 jfg.push_back(factor: clique->conditional());
582 VectorValues expectedGradient = jfg.gradientAtZero();
583 // Compare with actual gradients
584 DenseIndex variablePosition = 0;
585 for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
586 const DenseIndex dim = clique->conditional()->getDim(variable: jit);
587 Vector actual = clique->gradientContribution().segment(start: variablePosition, n: dim);
588 EXPECT(assert_equal(expectedGradient[*jit], actual));
589 variablePosition += dim;
590 }
591 LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition);
592 }
593
594 // Check gradient
595 VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
596 VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(x0: VectorValues::Zero(other: expectedGradient));
597 VectorValues actualGradient = isam.gradientAtZero();
598 EXPECT(assert_equal(expectedGradient2, expectedGradient));
599 EXPECT(assert_equal(expectedGradient, actualGradient));
600}
601
602/* ************************************************************************* */
603TEST(ISAM2, slamlike_solution_partial_relinearization_check)
604{
605 // These variables will be reused and accumulate factors and values
606 Values fullinit;
607 NonlinearFactorGraph fullgraph;
608 ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
609 params.enablePartialRelinearizationCheck = true;
610 ISAM2 isam = createSlamlikeISAM2(init_values: &fullinit, full_graph: &fullgraph, params);
611
612 // Compare solutions
613 CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
614}
615
616namespace {
617 bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
618 Matrix expectedAugmentedHessian, expected3AugmentedHessian;
619 KeyVector toKeep;
620 for (const auto& [key, clique]: isam.getDelta()) {
621 if(find(first: leafKeys.begin(), last: leafKeys.end(), val: key) == leafKeys.end()) {
622 toKeep.push_back(x: key);
623 }
624 }
625
626 // Calculate expected marginal from iSAM2 tree
627 expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(variables: toKeep, function: EliminateQR)->augmentedHessian();
628
629 // Calculate expected marginal from cached linear factors
630 //assert(isam.params().cacheLinearizedFactors);
631 //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian();
632
633 // Calculate expected marginal from original nonlinear factors
634 expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(linearizationPoint: isam.getLinearizationPoint())
635 ->marginal(variables: toKeep, function: EliminateQR)->augmentedHessian();
636
637 // Do marginalization
638 isam.marginalizeLeaves(leafKeys);
639
640 // Check
641 GaussianFactorGraph actualMarginalGraph(isam);
642 Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
643 //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian();
644 Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize(
645 linearizationPoint: isam.getLinearizationPoint())->augmentedHessian();
646 assert(actualAugmentedHessian.allFinite());
647
648 // Check full marginalization
649 //cout << "treeEqual" << endl;
650 bool treeEqual = assert_equal(A: expectedAugmentedHessian, B: actualAugmentedHessian, tol: 1e-6);
651 //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
652 //cout << "nonlinEqual" << endl;
653 actualAugmentedHessian.bottomRightCorner(cRows: 1,cCols: 1) = expected3AugmentedHessian.bottomRightCorner(cRows: 1,cCols: 1); bool nonlinEqual = assert_equal(A: expected3AugmentedHessian, B: actualAugmentedHessian, tol: 1e-6);
654 //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6);
655 //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6);
656 //cout << "nonlinCorrect" << endl;
657 bool afterNonlinCorrect = assert_equal(A: expected3AugmentedHessian, B: actual3AugmentedHessian, tol: 1e-6);
658
659 bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
660 return ok;
661 }
662
663 std::optional<FastMap<Key, int>> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys)
664 {
665 if (marginalizableKeys.empty()) {
666 return {};
667 } else {
668 FastMap<Key, int> constrainedKeys = FastMap<Key, int>();
669 // Generate ordering constraints so that the marginalizable variables will be eliminated first
670 // Set all existing and new variables to Group1
671 for (const auto& key_val : isam.getDelta()) {
672 constrainedKeys.emplace(args: key_val.first, args: 1);
673 }
674 for (const auto& key : newKeys) {
675 constrainedKeys.emplace(args: key, args: 1);
676 }
677 // And then re-assign the marginalizable variables to Group0 so that they'll all be leaf nodes
678 for (const auto& key : marginalizableKeys) {
679 constrainedKeys.at(k: key) = 0;
680 }
681 return constrainedKeys;
682 }
683 }
684
685 void markAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& rootClique, KeyList& additionalKeys)
686 {
687 std::stack<ISAM2Clique::shared_ptr> frontier;
688 frontier.push(x: rootClique);
689 // Basic DFS to find additional keys
690 while (!frontier.empty()) {
691 // Get the top of the stack
692 const ISAM2Clique::shared_ptr clique = frontier.top();
693 frontier.pop();
694 // Check if we have more keys and children to add
695 if (std::find(first: clique->conditional()->beginParents(), last: clique->conditional()->endParents(), val: key) !=
696 clique->conditional()->endParents()) {
697 for (Key i : clique->conditional()->frontals()) {
698 additionalKeys.push_back(x: i);
699 }
700 for (const ISAM2Clique::shared_ptr& child : clique->children) {
701 frontier.push(x: child);
702 }
703 }
704 }
705 }
706
707 bool updateAndMarginalize(const NonlinearFactorGraph& newFactors, const Values& newValues, const KeySet& marginalizableKeys, ISAM2& isam)
708 {
709 // Force ISAM2 to put marginalizable variables at the beginning
710 auto orderingConstraints = createOrderingConstraints(isam, newKeys: newValues.keys(), marginalizableKeys);
711
712 // Mark additional keys between the marginalized keys and the leaves
713 KeyList markedKeys;
714 for (Key key : marginalizableKeys) {
715 markedKeys.push_back(x: key);
716 ISAM2Clique::shared_ptr clique = isam[key];
717 for (const ISAM2Clique::shared_ptr& child : clique->children) {
718 markAffectedKeys(key, rootClique: child, additionalKeys&: markedKeys);
719 }
720 }
721
722 // Update
723 isam.update(newFactors, newTheta: newValues, removeFactorIndices: FactorIndices{}, constrainedKeys: orderingConstraints, noRelinKeys: {}, extraReelimKeys: markedKeys);
724
725 if (!marginalizableKeys.empty()) {
726 FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
727 return checkMarginalizeLeaves(isam, leafKeys);
728 }
729 else {
730 return true;
731 }
732 }
733}
734
735/* ************************************************************************* */
736TEST(ISAM2, marginalizeLeaves1) {
737 ISAM2 isam;
738 NonlinearFactorGraph factors;
739 factors.addPrior(key: 0, prior: 0.0, model);
740
741 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 1, args: 0.0, args: model);
742 factors.emplace_shared<BetweenFactor<double>>(args: 1, args: 2, args: 0.0, args: model);
743 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 2, args: 0.0, args: model);
744
745 Values values;
746 values.insert(j: 0, val: 0.0);
747 values.insert(j: 1, val: 0.0);
748 values.insert(j: 2, val: 0.0);
749
750 FastMap<Key, int> constrainedKeys;
751 constrainedKeys.insert(x: make_pair(x: 0, y: 0));
752 constrainedKeys.insert(x: make_pair(x: 1, y: 1));
753 constrainedKeys.insert(x: make_pair(x: 2, y: 2));
754
755 isam.update(newFactors: factors, newTheta: values, removeFactorIndices: FactorIndices(), constrainedKeys);
756
757 FastList<Key> leafKeys {0};
758 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
759}
760
761/* ************************************************************************* */
762TEST(ISAM2, marginalizeLeaves2) {
763 ISAM2 isam;
764
765 NonlinearFactorGraph factors;
766 factors.addPrior(key: 0, prior: 0.0, model);
767
768 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 1, args: 0.0, args: model);
769 factors.emplace_shared<BetweenFactor<double>>(args: 1, args: 2, args: 0.0, args: model);
770 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 2, args: 0.0, args: model);
771 factors.emplace_shared<BetweenFactor<double>>(args: 2, args: 3, args: 0.0, args: model);
772
773 Values values;
774 values.insert(j: 0, val: 0.0);
775 values.insert(j: 1, val: 0.0);
776 values.insert(j: 2, val: 0.0);
777 values.insert(j: 3, val: 0.0);
778
779 FastMap<Key, int> constrainedKeys;
780 constrainedKeys.insert(x: make_pair(x: 0, y: 0));
781 constrainedKeys.insert(x: make_pair(x: 1, y: 1));
782 constrainedKeys.insert(x: make_pair(x: 2, y: 2));
783 constrainedKeys.insert(x: make_pair(x: 3, y: 3));
784
785 isam.update(newFactors: factors, newTheta: values, removeFactorIndices: FactorIndices(), constrainedKeys);
786
787 FastList<Key> leafKeys {0};
788 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
789}
790
791/* ************************************************************************* */
792TEST(ISAM2, marginalizeLeaves3) {
793 ISAM2 isam;
794
795 NonlinearFactorGraph factors;
796 factors.addPrior(key: 0, prior: 0.0, model);
797
798 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 1, args: 0.0, args: model);
799 factors.emplace_shared<BetweenFactor<double>>(args: 1, args: 2, args: 0.0, args: model);
800 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 2, args: 0.0, args: model);
801
802 factors.emplace_shared<BetweenFactor<double>>(args: 2, args: 3, args: 0.0, args: model);
803
804 factors.emplace_shared<BetweenFactor<double>>(args: 3, args: 4, args: 0.0, args: model);
805 factors.emplace_shared<BetweenFactor<double>>(args: 4, args: 5, args: 0.0, args: model);
806 factors.emplace_shared<BetweenFactor<double>>(args: 3, args: 5, args: 0.0, args: model);
807
808 Values values;
809 values.insert(j: 0, val: 0.0);
810 values.insert(j: 1, val: 0.0);
811 values.insert(j: 2, val: 0.0);
812 values.insert(j: 3, val: 0.0);
813 values.insert(j: 4, val: 0.0);
814 values.insert(j: 5, val: 0.0);
815
816 FastMap<Key, int> constrainedKeys;
817 constrainedKeys.insert(x: make_pair(x: 0, y: 0));
818 constrainedKeys.insert(x: make_pair(x: 1, y: 1));
819 constrainedKeys.insert(x: make_pair(x: 2, y: 2));
820 constrainedKeys.insert(x: make_pair(x: 3, y: 3));
821 constrainedKeys.insert(x: make_pair(x: 4, y: 4));
822 constrainedKeys.insert(x: make_pair(x: 5, y: 5));
823
824 isam.update(newFactors: factors, newTheta: values, removeFactorIndices: FactorIndices(), constrainedKeys);
825
826 FastList<Key> leafKeys {0};
827 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
828}
829
830/* ************************************************************************* */
831TEST(ISAM2, marginalizeLeaves4) {
832 ISAM2 isam;
833
834 NonlinearFactorGraph factors;
835 factors.addPrior(key: 0, prior: 0.0, model);
836 factors.emplace_shared<BetweenFactor<double>>(args: 0, args: 2, args: 0.0, args: model);
837 factors.emplace_shared<BetweenFactor<double>>(args: 1, args: 2, args: 0.0, args: model);
838
839 Values values;
840 values.insert(j: 0, val: 0.0);
841 values.insert(j: 1, val: 0.0);
842 values.insert(j: 2, val: 0.0);
843
844 FastMap<Key, int> constrainedKeys;
845 constrainedKeys.insert(x: make_pair(x: 0, y: 0));
846 constrainedKeys.insert(x: make_pair(x: 1, y: 1));
847 constrainedKeys.insert(x: make_pair(x: 2, y: 2));
848
849 isam.update(newFactors: factors, newTheta: values, removeFactorIndices: FactorIndices(), constrainedKeys);
850
851 FastList<Key> leafKeys {1};
852 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
853}
854
855/* ************************************************************************* */
856TEST(ISAM2, marginalizeLeaves5)
857{
858 // Create isam2
859 ISAM2 isam = createSlamlikeISAM2();
860
861 // Marginalize
862 FastList<Key> marginalizeKeys {0};
863 EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
864}
865
866/* ************************************************************************* */
867TEST(ISAM2, marginalizeLeaves6)
868{
869 auto nm = noiseModel::Isotropic::Sigma(dim: 6, sigma: 1.0);
870
871 int gridDim = 10;
872
873 auto idxToKey = [gridDim](int i, int j){return i * gridDim + j;};
874
875 NonlinearFactorGraph factors;
876 Values values;
877 ISAM2 isam;
878
879 // Create a grid of pose variables
880 for (int i = 0; i < gridDim; ++i) {
881 for (int j = 0; j < gridDim; ++j) {
882 Pose3 pose = Pose3(Rot3::Identity(), Point3(i, j, 0));
883 Key key = idxToKey(i, j);
884 // Place a prior on the first pose
885 factors.addPrior(key, prior: Pose3(Rot3::Identity(), Point3(i, j, 0)), model: nm);
886 values.insert(j: key, val: pose);
887 if (i > 0) {
888 factors.emplace_shared<BetweenFactor<Pose3>>(args: idxToKey(i - 1, j), args&: key, args: Pose3(Rot3::Identity(), Point3(1, 0, 0)),args&: nm);
889 }
890 if (j > 0) {
891 factors.emplace_shared<BetweenFactor<Pose3>>(args: idxToKey(i, j - 1), args&: key, args: Pose3(Rot3::Identity(), Point3(0, 1, 0)),args&: nm);
892 }
893 }
894 }
895
896 // Optimize the graph
897 EXPECT(updateAndMarginalize(factors, values, {}, isam));
898 auto estimate = isam.calculateBestEstimate();
899
900 // Get the list of keys
901 std::vector<Key> key_list(gridDim * gridDim);
902 std::iota(first: key_list.begin(), last: key_list.end(), value: 0);
903
904 // Shuffle the keys -> we will marginalize the keys one by one in this order
905 std::shuffle(first: key_list.begin(), last: key_list.end(), g: std::default_random_engine(1234));
906
907 for (const auto& key : key_list) {
908 KeySet marginalKeys;
909 marginalKeys.insert(x: key);
910 EXPECT(updateAndMarginalize({}, {}, marginalKeys, isam));
911 estimate = isam.calculateBestEstimate();
912 }
913}
914
915/* ************************************************************************* */
916TEST(ISAM2, MarginalizeRoot)
917{
918 auto nm = noiseModel::Isotropic::Sigma(dim: 6, sigma: 1.0);
919
920 NonlinearFactorGraph factors;
921 Values values;
922 ISAM2 isam;
923
924 // Create a factor graph with one variable and a prior
925 Pose3 root = Pose3::Identity();
926 Key rootKey(0);
927 values.insert(j: rootKey, val: root);
928 factors.addPrior(key: rootKey, prior: Pose3::Identity(), model: nm);
929
930 // Optimize the graph
931 EXPECT(updateAndMarginalize(factors, values, {}, isam));
932 auto estimate = isam.calculateBestEstimate();
933 EXPECT(estimate.size() == 1);
934
935 // And remove the node from the graph
936 KeySet marginalizableKeys;
937 marginalizableKeys.insert(x: rootKey);
938
939 EXPECT(updateAndMarginalize({}, {}, marginalizableKeys, isam));
940
941 estimate = isam.calculateBestEstimate();
942 EXPECT(estimate.empty());
943}
944
945/* ************************************************************************* */
946TEST(ISAM2, marginalizationSize)
947{
948 auto nm = noiseModel::Isotropic::Sigma(dim: 6, sigma: 1.0);
949
950 NonlinearFactorGraph factors;
951 Values values;
952 ISAM2Params params;
953 params.findUnusedFactorSlots = true;
954 ISAM2 isam{params};
955
956 // Create a pose variable
957 Key aKey(0);
958 values.insert(j: aKey, val: Pose3::Identity());
959 factors.addPrior(key: aKey, prior: Pose3::Identity(), model: nm);
960 // Create another pose variable linked to the first
961 Pose3 b = Pose3::Identity();
962 Key bKey(1);
963 values.insert(j: bKey, val: Pose3::Identity());
964 factors.emplace_shared<BetweenFactor<Pose3>>(args&: aKey, args&: bKey, args: Pose3::Identity(), args&: nm);
965 // Optimize the graph
966 EXPECT(updateAndMarginalize(factors, values, {}, isam));
967
968 // Now remove a variable -> we should not see the number of factors increase
969 gtsam::KeySet to_remove;
970 to_remove.insert(x: aKey);
971 const auto numFactorsBefore = isam.getFactorsUnsafe().size();
972 updateAndMarginalize(newFactors: {}, newValues: {}, marginalizableKeys: to_remove, isam);
973 EXPECT(numFactorsBefore == isam.getFactorsUnsafe().size());
974}
975
976/* ************************************************************************* */
977TEST(ISAM2, marginalCovariance)
978{
979 // Create isam2
980 ISAM2 isam = createSlamlikeISAM2();
981
982 // Check marginal
983 Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(variable: 5);
984 Matrix actual = isam.marginalCovariance(key: 5);
985 EXPECT(assert_equal(expected, actual));
986}
987
988/* ************************************************************************* */
989TEST(ISAM2, calculate_nnz)
990{
991 ISAM2 isam = createSlamlikeISAM2();
992 int expected = 241;
993 int actual = isam.roots().front()->calculate_nnz();
994
995 EXPECT_LONGS_EQUAL(expected, actual);
996}
997
998class FixActiveFactor : public NoiseModelFactorN<Vector2> {
999 using Base = NoiseModelFactorN<Vector2>;
1000 bool is_active_;
1001
1002public:
1003 FixActiveFactor(const gtsam::Key& key, const bool active)
1004 : Base(nullptr, key), is_active_(active) {}
1005
1006 virtual bool active(const gtsam::Values &values) const override {
1007 return is_active_;
1008 }
1009
1010 virtual Vector
1011 evaluateError(const Vector2& x,
1012 Base::OptionalMatrixTypeT<Vector2> H = nullptr) const override {
1013 if (H) {
1014 *H = Vector2::Identity();
1015 }
1016 return Vector2::Zero();
1017 }
1018};
1019
1020TEST(ActiveFactorTesting, Issue1596) {
1021 // Issue1596: When a derived Nonlinear Factor is not active, the linearization returns a nullptr (NonlinearFactor.cpp:156), which
1022 // causes an error when `EliminateSymbolic` is called (SymbolicFactor-inst.h:45) due to not checking if the factor is nullptr.
1023 const gtsam::Key key{Symbol('x', 0)};
1024
1025 ISAM2 isam;
1026 Values values;
1027 NonlinearFactorGraph graph;
1028
1029 // Insert an active factor
1030 values.insert<Vector2>(j: key, val: Vector2::Zero());
1031 graph.emplace_shared<FixActiveFactor>(args: key, args: true);
1032
1033 // No problem here
1034 isam.update(newFactors: graph, newTheta: values);
1035
1036 graph = NonlinearFactorGraph();
1037
1038 // Inserting a factor that is never active
1039 graph.emplace_shared<FixActiveFactor>(args: key, args: false);
1040
1041 // This call throws the error if the pointer is not validated on (SymbolicFactor-inst.h:45)
1042 isam.update(newFactors: graph);
1043
1044 // If the bug is fixed, this line is reached.
1045 EXPECT(isam.getFactorsUnsafe().size() == 2);
1046}
1047
1048/* ************************************************************************* */
1049int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
1050/* ************************************************************************* */
1051