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 testNonlinearFactorGraph.cpp
14 * @brief Unit tests for Non-Linear Factor NonlinearFactorGraph
15 * @brief testNonlinearFactorGraph
16 * @author Carlos Nieto
17 * @author Christian Potthast
18 * @author Frank Dellaert
19 */
20
21#include <gtsam/base/Testable.h>
22#include <gtsam/base/Matrix.h>
23#include <tests/smallExample.h>
24#include <gtsam/inference/FactorGraph.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/symbolic/SymbolicFactorGraph.h>
27#include <gtsam/nonlinear/NonlinearFactorGraph.h>
28#include <gtsam/geometry/Pose2.h>
29#include <gtsam/geometry/Pose3.h>
30#include <gtsam/sam/RangeFactor.h>
31#include <gtsam/slam/BetweenFactor.h>
32
33#include <CppUnitLite/TestHarness.h>
34
35/*STL/C++*/
36#include <iostream>
37
38using namespace std;
39using namespace gtsam;
40using namespace example;
41
42using symbol_shorthand::X;
43using symbol_shorthand::L;
44
45/* ************************************************************************* */
46TEST( NonlinearFactorGraph, equals )
47{
48 NonlinearFactorGraph fg = createNonlinearFactorGraph();
49 NonlinearFactorGraph fg2 = createNonlinearFactorGraph();
50 CHECK( fg.equals(fg2) );
51}
52
53/* ************************************************************************* */
54TEST( NonlinearFactorGraph, error )
55{
56 NonlinearFactorGraph fg = createNonlinearFactorGraph();
57 Values c1 = createValues();
58 double actual1 = fg.error(values: c1);
59 DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
60
61 Values c2 = createNoisyValues();
62 double actual2 = fg.error(values: c2);
63 DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
64}
65
66/* ************************************************************************* */
67TEST( NonlinearFactorGraph, keys )
68{
69 NonlinearFactorGraph fg = createNonlinearFactorGraph();
70 KeySet actual = fg.keys();
71 LONGS_EQUAL(3, (long)actual.size());
72 KeySet::const_iterator it = actual.begin();
73 LONGS_EQUAL((long)L(1), (long)*(it++));
74 LONGS_EQUAL((long)X(1), (long)*(it++));
75 LONGS_EQUAL((long)X(2), (long)*(it++));
76}
77
78/* ************************************************************************* */
79TEST( NonlinearFactorGraph, GET_ORDERING)
80{
81 const Ordering expected{L(j: 1), X(j: 2), X(j: 1)}; // For starting with l1,x1,x2
82 NonlinearFactorGraph nlfg = createNonlinearFactorGraph();
83 Ordering actual = Ordering::Colamd(graph: nlfg);
84 EXPECT(assert_equal(expected,actual));
85
86 // Constrained ordering - put x2 at the end
87 const Ordering expectedConstrained{L(j: 1), X(j: 1), X(j: 2)};
88 FastMap<Key, int> constraints;
89 constraints[X(j: 2)] = 1;
90 Ordering actualConstrained = Ordering::ColamdConstrained(graph: nlfg, groups: constraints);
91 EXPECT(assert_equal(expectedConstrained, actualConstrained));
92}
93
94/* ************************************************************************* */
95TEST( NonlinearFactorGraph, probPrime )
96{
97 NonlinearFactorGraph fg = createNonlinearFactorGraph();
98 Values cfg = createValues();
99
100 // evaluate the probability of the factor graph
101 double actual = fg.probPrime(values: cfg);
102 double expected = 1.0;
103 DOUBLES_EQUAL(expected,actual,0);
104}
105
106/* ************************************************************************* */
107TEST(NonlinearFactorGraph, ProbPrime2) {
108 NonlinearFactorGraph fg;
109 fg.emplace_shared<PriorFactor<double>>(args: 1, args: 0.0,
110 args: noiseModel::Isotropic::Sigma(dim: 1, sigma: 1.0));
111
112 Values values;
113 values.insert(j: 1, val: 1.0);
114
115 // The prior factor squared error is: 0.5.
116 EXPECT_DOUBLES_EQUAL(0.5, fg.error(values), 1e-12);
117
118 // The probability value is: exp^(-factor_error) / sqrt(2 * PI)
119 // Ignore the denominator and we get: exp^(-factor_error) = exp^(-0.5)
120 double expected = exp(x: -0.5);
121 EXPECT_DOUBLES_EQUAL(expected, fg.probPrime(values), 1e-12);
122}
123
124/* ************************************************************************* */
125TEST( NonlinearFactorGraph, linearize )
126{
127 NonlinearFactorGraph fg = createNonlinearFactorGraph();
128 Values initial = createNoisyValues();
129 GaussianFactorGraph linearFG = *fg.linearize(linearizationPoint: initial);
130 GaussianFactorGraph expected = createGaussianFactorGraph();
131 CHECK(assert_equal(expected,linearFG)); // Needs correct linearizations
132}
133
134/* ************************************************************************* */
135TEST( NonlinearFactorGraph, clone )
136{
137 NonlinearFactorGraph fg = createNonlinearFactorGraph();
138 NonlinearFactorGraph actClone = fg.clone();
139 EXPECT(assert_equal(fg, actClone));
140 for (size_t i=0; i<fg.size(); ++i)
141 EXPECT(fg[i] != actClone[i]);
142}
143
144/* ************************************************************************* */
145TEST( NonlinearFactorGraph, rekey )
146{
147 NonlinearFactorGraph init = createNonlinearFactorGraph();
148 map<Key,Key> rekey_mapping;
149 rekey_mapping.insert(x: make_pair(x: L(j: 1), y: L(j: 4)));
150 NonlinearFactorGraph actRekey = init.rekey(rekey_mapping);
151
152 // ensure deep clone
153 LONGS_EQUAL((long)init.size(), (long)actRekey.size());
154 for (size_t i=0; i<init.size(); ++i)
155 EXPECT(init[i] != actRekey[i]);
156
157 NonlinearFactorGraph expRekey;
158 // original measurements
159 expRekey.push_back(factor: init[0]);
160 expRekey.push_back(factor: init[1]);
161
162 // updated measurements
163 Point2 z3(0, -1), z4(-1.5, -1.);
164 SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(dim: 2,sigma: 0.2);
165 expRekey.emplace_shared<simulated2D::Measurement>(args&: z3, args&: sigma0_2, args: X(j: 1), args: L(j: 4));
166 expRekey.emplace_shared<simulated2D::Measurement>(args&: z4, args&: sigma0_2, args: X(j: 2), args: L(j: 4));
167
168 EXPECT(assert_equal(expRekey, actRekey));
169}
170
171/* ************************************************************************* */
172TEST( NonlinearFactorGraph, symbolic )
173{
174 NonlinearFactorGraph graph = createNonlinearFactorGraph();
175
176 SymbolicFactorGraph expected;
177 expected.push_factor(key: X(j: 1));
178 expected.push_factor(key1: X(j: 1), key2: X(j: 2));
179 expected.push_factor(key1: X(j: 1), key2: L(j: 1));
180 expected.push_factor(key1: X(j: 2), key2: L(j: 1));
181
182 SymbolicFactorGraph actual = *graph.symbolic();
183
184 EXPECT(assert_equal(expected, actual));
185}
186
187/* ************************************************************************* */
188TEST(NonlinearFactorGraph, UpdateCholesky) {
189 NonlinearFactorGraph fg = createNonlinearFactorGraph();
190 Values initial = createNoisyValues();
191
192 // solve conventionally
193 GaussianFactorGraph linearFG = *fg.linearize(linearizationPoint: initial);
194 auto delta = linearFG.optimizeDensely();
195 auto expected = initial.retract(delta);
196
197 // solve with new method
198 EXPECT(assert_equal(expected, fg.updateCholesky(initial)));
199
200 // solve with Ordering
201 const Ordering ordering{L(j: 1), X(j: 2), X(j: 1)};
202 EXPECT(assert_equal(expected, fg.updateCholesky(initial, ordering)));
203
204 // solve with new method, heavily damped
205 auto dampen = [](const HessianFactor::shared_ptr& hessianFactor) {
206 auto iterator = hessianFactor->begin();
207 for (; iterator != hessianFactor->end(); iterator++) {
208 const auto index = std::distance(first: hessianFactor->begin(), last: iterator);
209 auto block = hessianFactor->info().diagonalBlock(J: index);
210 for (int j = 0; j < block.rows(); j++) {
211 block(j, j) += 1e9;
212 }
213 }
214 };
215 EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6));
216}
217
218/* ************************************************************************* */
219// Example from issue #452 which threw an ILS error. The reason was a very
220// weak prior on heading, which was tightened, and the ILS disappeared.
221TEST(testNonlinearFactorGraph, eliminate) {
222 // Linearization point
223 Pose2 T11(0, 0, 0);
224 Pose2 T12(1, 0, 0);
225 Pose2 T21(0, 1, 0);
226 Pose2 T22(1, 1, 0);
227
228 // Factor graph
229 auto graph = NonlinearFactorGraph();
230
231 // Priors
232 auto prior = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1);
233 graph.addPrior(key: 11, prior: T11, model: prior);
234 graph.addPrior(key: 21, prior: T21, model: prior);
235
236 // Odometry
237 auto model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.01, 0.01, 0.3));
238 graph.add(factorOrContainer: BetweenFactor<Pose2>(11, 12, T11.between(g: T12), model));
239 graph.add(factorOrContainer: BetweenFactor<Pose2>(21, 22, T21.between(g: T22), model));
240
241 // Range factor
242 auto model_rho = noiseModel::Isotropic::Sigma(dim: 1, sigma: 0.01);
243 graph.add(factorOrContainer: RangeFactor<Pose2>(12, 22, 1.0, model_rho));
244
245 Values values;
246 values.insert(j: 11, val: T11.retract(v: Vector3(0.1,0.2,0.3)));
247 values.insert(j: 12, val: T12);
248 values.insert(j: 21, val: T21);
249 values.insert(j: 22, val: T22);
250 auto linearized = graph.linearize(linearizationPoint: values);
251
252 // Eliminate
253 const Ordering ordering{11, 21, 12, 22};
254 auto bn = linearized->eliminateSequential(ordering);
255 EXPECT_LONGS_EQUAL(4, bn->size());
256}
257
258/* ************************************************************************* */
259TEST(testNonlinearFactorGraph, addPrior) {
260 Key k(0);
261
262 // Factor graph.
263 auto graph = NonlinearFactorGraph();
264
265 // Add a prior factor for key k.
266 auto model_double = noiseModel::Isotropic::Sigma(dim: 1, sigma: 1);
267 graph.addPrior<double>(key: k, prior: 10, model: model_double);
268
269 // Assert the graph has 0 error with the correct values.
270 Values values;
271 values.insert(j: k, val: 10.0);
272 EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
273
274 // Assert the graph has some error with incorrect values.
275 values.clear();
276 values.insert(j: k, val: 11.0);
277 EXPECT(0 != graph.error(values));
278
279 // Clear the factor graph and values.
280 values.clear();
281 graph.erase(first: graph.begin(), last: graph.end());
282
283 // Add a Pose3 prior to the factor graph. Use a gaussian noise model by
284 // providing the covariance matrix.
285 Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
286 covariance_pose3.setIdentity();
287 Pose3 pose{Rot3(), Point3(0, 0, 0)};
288 graph.addPrior(key: k, prior: pose, covariance: covariance_pose3);
289
290 // Assert the graph has 0 error with the correct values.
291 values.insert(j: k, val: pose);
292 EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
293
294 // Assert the graph has some error with incorrect values.
295 values.clear();
296 Pose3 pose_incorrect{Rot3::RzRyRx(x: -M_PI, M_PI, z: -M_PI / 8), Point3(1, 2, 3)};
297 values.insert(j: k, val: pose_incorrect);
298 EXPECT(0 != graph.error(values));
299}
300
301/* ************************************************************************* */
302TEST(NonlinearFactorGraph, printErrors)
303{
304 const NonlinearFactorGraph fg = createNonlinearFactorGraph();
305 const Values c = createValues();
306
307 // Test that it builds with default parameters.
308 // We cannot check the output since (at present) output is fixed to std::cout.
309 fg.printErrors(values: c);
310
311 // Second round: using callback filter to check that we actually visit all factors:
312 std::vector<bool> visited;
313 visited.assign(n: fg.size(), x: false);
314 const auto testFilter =
315 [&](const gtsam::Factor *f, double error, size_t index) {
316 EXPECT(f!=nullptr);
317 EXPECT(error>=.0);
318 visited.at(n: index)=true;
319 return false; // do not print
320 };
321 fg.printErrors(values: c,str: "Test graph: ", keyFormatter: gtsam::DefaultKeyFormatter,printCondition: testFilter);
322
323 for (bool visit : visited) EXPECT(visit==true);
324}
325
326/* ************************************************************************* */
327TEST(NonlinearFactorGraph, dot) {
328 string expected =
329 "graph {\n"
330 " size=\"5,5\";\n"
331 "\n"
332 " var7782220156096217089[label=\"l1\"];\n"
333 " var8646911284551352321[label=\"x1\"];\n"
334 " var8646911284551352322[label=\"x2\"];\n"
335 "\n"
336 " factor0[label=\"\", shape=point];\n"
337 " var8646911284551352321--factor0;\n"
338 " factor1[label=\"\", shape=point];\n"
339 " var8646911284551352321--factor1;\n"
340 " var8646911284551352322--factor1;\n"
341 " factor2[label=\"\", shape=point];\n"
342 " var8646911284551352321--factor2;\n"
343 " var7782220156096217089--factor2;\n"
344 " factor3[label=\"\", shape=point];\n"
345 " var8646911284551352322--factor3;\n"
346 " var7782220156096217089--factor3;\n"
347 "}\n";
348
349 const NonlinearFactorGraph fg = createNonlinearFactorGraph();
350 string actual = fg.dot();
351 EXPECT(actual == expected);
352}
353
354/* ************************************************************************* */
355TEST(NonlinearFactorGraph, dot_extra) {
356 string expected =
357 "graph {\n"
358 " size=\"5,5\";\n"
359 "\n"
360 " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
361 " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
362 " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
363 "\n"
364 " factor0[label=\"\", shape=point];\n"
365 " var8646911284551352321--factor0;\n"
366 " factor1[label=\"\", shape=point];\n"
367 " var8646911284551352321--factor1;\n"
368 " var8646911284551352322--factor1;\n"
369 " factor2[label=\"\", shape=point];\n"
370 " var8646911284551352321--factor2;\n"
371 " var7782220156096217089--factor2;\n"
372 " factor3[label=\"\", shape=point];\n"
373 " var8646911284551352322--factor3;\n"
374 " var7782220156096217089--factor3;\n"
375 "}\n";
376
377 const NonlinearFactorGraph fg = createNonlinearFactorGraph();
378 const Values c = createValues();
379
380 stringstream ss;
381 fg.dot(os&: ss, values: c);
382 EXPECT(ss.str() == expected);
383}
384
385/* ************************************************************************* */
386int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
387/* ************************************************************************* */
388