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 testGraph.cpp
14 * @date Jan 12, 2010
15 * @author nikai
16 * @brief unit test for graph-inl.h
17 */
18
19#include <gtsam/slam/BetweenFactor.h>
20#include <gtsam/nonlinear/NonlinearFactorGraph.h>
21#include <gtsam/linear/GaussianFactorGraph.h>
22#include <gtsam/linear/JacobianFactor.h>
23#include <gtsam/inference/graph.h>
24#include <gtsam/inference/Symbol.h>
25#include <gtsam/geometry/Pose2.h>
26
27#include <CppUnitLite/TestHarness.h>
28
29#include <memory>
30
31#include <iostream>
32
33using namespace std;
34using namespace gtsam;
35
36/* ************************************************************************* */
37// x1 -> x2
38// -> x3 -> x4
39// -> x5
40TEST ( Ordering, predecessorMap2Keys ) {
41 PredecessorMap<Key> p_map;
42 p_map.insert(key: 1,parent: 1);
43 p_map.insert(key: 2,parent: 1);
44 p_map.insert(key: 3,parent: 1);
45 p_map.insert(key: 4,parent: 3);
46 p_map.insert(key: 5,parent: 1);
47
48 list<Key> expected{4, 5, 3, 2, 1};
49
50 list<Key> actual = predecessorMap2Keys<Key>(p_map);
51 LONGS_EQUAL((long)expected.size(), (long)actual.size());
52
53 list<Key>::const_iterator it1 = expected.begin();
54 list<Key>::const_iterator it2 = actual.begin();
55 for(; it1!=expected.end(); it1++, it2++)
56 CHECK(*it1 == *it2)
57}
58
59/* ************************************************************************* */
60TEST( Graph, predecessorMap2Graph )
61{
62 typedef SGraph<string>::Vertex SVertex;
63 SGraph<Key> graph;
64 SVertex root;
65 map<Key, SVertex> key2vertex;
66
67 PredecessorMap<Key> p_map;
68 p_map.insert(key: 1, parent: 2);
69 p_map.insert(key: 2, parent: 2);
70 p_map.insert(key: 3, parent: 2);
71 std::tie(args&: graph, args&: root, args&: key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
72
73 LONGS_EQUAL(3, (long)boost::num_vertices(graph));
74 CHECK(root == key2vertex[2]);
75}
76
77/* ************************************************************************* */
78TEST( Graph, composePoses )
79{
80 NonlinearFactorGraph graph;
81 SharedNoiseModel cov = noiseModel::Unit::Create(dim: 3);
82 Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
83 Pose2 p12=p1.between(g: p2), p23=p2.between(g: p3), p43=p4.between(g: p3);
84 graph += BetweenFactor<Pose2>(1,2, p12, cov);
85 graph += BetweenFactor<Pose2>(2,3, p23, cov);
86 graph += BetweenFactor<Pose2>(4,3, p43, cov);
87
88 PredecessorMap<Key> tree;
89 tree.insert(key: 1,parent: 2);
90 tree.insert(key: 2,parent: 2);
91 tree.insert(key: 3,parent: 2);
92 tree.insert(key: 4,parent: 3);
93
94 Pose2 rootPose = p2;
95
96 std::shared_ptr<Values> actual = composePoses<NonlinearFactorGraph, BetweenFactor<Pose2>, Pose2, Key> (graph, tree, rootPose);
97
98 Values expected;
99 expected.insert(j: 1, val: p1);
100 expected.insert(j: 2, val: p2);
101 expected.insert(j: 3, val: p3);
102 expected.insert(j: 4, val: p4);
103
104 LONGS_EQUAL(4, (long)actual->size());
105 CHECK(assert_equal(expected, *actual));
106}
107
108/* ************************************************************************* */
109TEST( GaussianFactorGraph, findMinimumSpanningTree )
110{
111 GaussianFactorGraph g;
112 Matrix I = I_2x2;
113 Vector2 b(0, 0);
114 const SharedDiagonal model = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.5, 0.5));
115 using namespace symbol_shorthand;
116 g += JacobianFactor(X(j: 1), I, X(j: 2), I, b, model);
117 g += JacobianFactor(X(j: 1), I, X(j: 3), I, b, model);
118 g += JacobianFactor(X(j: 1), I, X(j: 4), I, b, model);
119 g += JacobianFactor(X(j: 2), I, X(j: 3), I, b, model);
120 g += JacobianFactor(X(j: 2), I, X(j: 4), I, b, model);
121 g += JacobianFactor(X(j: 3), I, X(j: 4), I, b, model);
122
123 PredecessorMap<Key> tree = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(fg: g);
124 EXPECT_LONGS_EQUAL(X(1),tree[X(1)]);
125 EXPECT_LONGS_EQUAL(X(1),tree[X(2)]);
126 EXPECT_LONGS_EQUAL(X(1),tree[X(3)]);
127 EXPECT_LONGS_EQUAL(X(1),tree[X(4)]);
128
129 // we add a disconnected component - does not work yet
130 // g += JacobianFactor(X(5), I, X(6), I, b, model);
131 //
132 // PredecessorMap<Key> forest = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
133 // EXPECT_LONGS_EQUAL(X(1),forest[X(1)]);
134 // EXPECT_LONGS_EQUAL(X(1),forest[X(2)]);
135 // EXPECT_LONGS_EQUAL(X(1),forest[X(3)]);
136 // EXPECT_LONGS_EQUAL(X(1),forest[X(4)]);
137 // EXPECT_LONGS_EQUAL(X(5),forest[X(5)]);
138 // EXPECT_LONGS_EQUAL(X(5),forest[X(6)]);
139}
140
141///* ************************************************************************* */
142// SL-FIX TEST( GaussianFactorGraph, split )
143//{
144// GaussianFactorGraph g;
145// Matrix I = eye(2);
146// Vector b = Vector_(0, 0, 0);
147// g += X(1), I, X(2), I, b, model;
148// g += X(1), I, X(3), I, b, model;
149// g += X(1), I, X(4), I, b, model;
150// g += X(2), I, X(3), I, b, model;
151// g += X(2), I, X(4), I, b, model;
152//
153// PredecessorMap<string> tree;
154// tree[X(1)] = X(1);
155// tree[X(2)] = X(1);
156// tree[X(3)] = X(1);
157// tree[X(4)] = X(1);
158//
159// GaussianFactorGraph Ab1, Ab2;
160// g.split<string, GaussianFactor>(tree, Ab1, Ab2);
161// LONGS_EQUAL(3, Ab1.size());
162// LONGS_EQUAL(2, Ab2.size());
163//}
164
165///* ************************************************************************* */
166// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )
167//{
168// SymbolicFactorGraph G;
169// G.push_factor("x1", "x2");
170// G.push_factor("x1", "x3");
171// G.push_factor("x1", "x4");
172// G.push_factor("x2", "x3");
173// G.push_factor("x2", "x4");
174// G.push_factor("x3", "x4");
175//
176// SymbolicFactorGraph T, C;
177// std::tie(T, C) = G.splitMinimumSpanningTree();
178//
179// SymbolicFactorGraph expectedT, expectedC;
180// expectedT.push_factor("x1", "x2");
181// expectedT.push_factor("x1", "x3");
182// expectedT.push_factor("x1", "x4");
183// expectedC.push_factor("x2", "x3");
184// expectedC.push_factor("x2", "x4");
185// expectedC.push_factor("x3", "x4");
186// CHECK(assert_equal(expectedT,T));
187// CHECK(assert_equal(expectedC,C));
188//}
189
190///* ************************************************************************* */
191///**
192// * x1 - x2 - x3 - x4 - x5
193// * | | / |
194// * l1 l2 l3
195// */
196// SL-FIX TEST( FactorGraph, removeSingletons )
197//{
198// SymbolicFactorGraph G;
199// G.push_factor("x1", "x2");
200// G.push_factor("x2", "x3");
201// G.push_factor("x3", "x4");
202// G.push_factor("x4", "x5");
203// G.push_factor("x2", "l1");
204// G.push_factor("x3", "l2");
205// G.push_factor("x4", "l2");
206// G.push_factor("x4", "l3");
207//
208// SymbolicFactorGraph singletonGraph;
209// set<Symbol> singletons;
210// std::tie(singletonGraph, singletons) = G.removeSingletons();
211//
212// set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
213// CHECK(singletons_excepted == singletons);
214//
215// SymbolicFactorGraph singletonGraph_excepted;
216// singletonGraph_excepted.push_factor("x2", "l1");
217// singletonGraph_excepted.push_factor("x4", "l3");
218// singletonGraph_excepted.push_factor("x1", "x2");
219// singletonGraph_excepted.push_factor("x4", "x5");
220// singletonGraph_excepted.push_factor("x2", "x3");
221// CHECK(singletonGraph_excepted.equals(singletonGraph));
222//}
223
224/* ************************************************************************* */
225int main() {
226 TestResult tr;
227 return TestRegistry::runAllTests(result&: tr);
228}
229/* ************************************************************************* */
230