testGraph.cpp
Go to the documentation of this file.
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 
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 <boost/shared_ptr.hpp>
30 #include <boost/assign/std/list.hpp> // for operator +=
31 using namespace boost::assign;
32 
33 #include <iostream>
34 
35 using namespace std;
36 using namespace gtsam;
37 
38 /* ************************************************************************* */
39 // x1 -> x2
40 // -> x3 -> x4
41 // -> x5
42 TEST ( Ordering, predecessorMap2Keys ) {
43  PredecessorMap<Key> p_map;
44  p_map.insert(1,1);
45  p_map.insert(2,1);
46  p_map.insert(3,1);
47  p_map.insert(4,3);
48  p_map.insert(5,1);
49 
50  list<Key> expected;
51  expected += 4,5,3,2,1;
52 
53  list<Key> actual = predecessorMap2Keys<Key>(p_map);
54  LONGS_EQUAL((long)expected.size(), (long)actual.size());
55 
56  list<Key>::const_iterator it1 = expected.begin();
57  list<Key>::const_iterator it2 = actual.begin();
58  for(; it1!=expected.end(); it1++, it2++)
59  CHECK(*it1 == *it2)
60 }
61 
62 /* ************************************************************************* */
63 TEST( Graph, predecessorMap2Graph )
64 {
65  typedef SGraph<string>::Vertex SVertex;
66  SGraph<Key> graph;
67  SVertex root;
68  map<Key, SVertex> key2vertex;
69 
70  PredecessorMap<Key> p_map;
71  p_map.insert(1, 2);
72  p_map.insert(2, 2);
73  p_map.insert(3, 2);
74  boost::tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
75 
76  LONGS_EQUAL(3, (long)boost::num_vertices(graph));
77  CHECK(root == key2vertex[2]);
78 }
79 
80 /* ************************************************************************* */
81 TEST( Graph, composePoses )
82 {
83  NonlinearFactorGraph graph;
84  SharedNoiseModel cov = noiseModel::Unit::Create(3);
85  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);
86  Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
87  graph += BetweenFactor<Pose2>(1,2, p12, cov);
88  graph += BetweenFactor<Pose2>(2,3, p23, cov);
89  graph += BetweenFactor<Pose2>(4,3, p43, cov);
90 
91  PredecessorMap<Key> tree;
92  tree.insert(1,2);
93  tree.insert(2,2);
94  tree.insert(3,2);
95  tree.insert(4,3);
96 
97  Pose2 rootPose = p2;
98 
99  boost::shared_ptr<Values> actual = composePoses<NonlinearFactorGraph, BetweenFactor<Pose2>, Pose2, Key> (graph, tree, rootPose);
100 
101  Values expected;
102  expected.insert(1, p1);
103  expected.insert(2, p2);
104  expected.insert(3, p3);
105  expected.insert(4, p4);
106 
107  LONGS_EQUAL(4, (long)actual->size());
108  CHECK(assert_equal(expected, *actual));
109 }
110 
111 /* ************************************************************************* */
112 TEST( GaussianFactorGraph, findMinimumSpanningTree )
113 {
114  GaussianFactorGraph g;
115  Matrix I = I_2x2;
116  Vector2 b(0, 0);
117  const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
118  using namespace symbol_shorthand;
119  g += JacobianFactor(X(1), I, X(2), I, b, model);
120  g += JacobianFactor(X(1), I, X(3), I, b, model);
121  g += JacobianFactor(X(1), I, X(4), I, b, model);
122  g += JacobianFactor(X(2), I, X(3), I, b, model);
123  g += JacobianFactor(X(2), I, X(4), I, b, model);
124  g += JacobianFactor(X(3), I, X(4), I, b, model);
125 
126  PredecessorMap<Key> tree = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
127  EXPECT_LONGS_EQUAL(X(1),tree[X(1)]);
128  EXPECT_LONGS_EQUAL(X(1),tree[X(2)]);
129  EXPECT_LONGS_EQUAL(X(1),tree[X(3)]);
130  EXPECT_LONGS_EQUAL(X(1),tree[X(4)]);
131 
132  // we add a disconnected component - does not work yet
133  // g += JacobianFactor(X(5), I, X(6), I, b, model);
134  //
135  // PredecessorMap<Key> forest = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
136  // EXPECT_LONGS_EQUAL(X(1),forest[X(1)]);
137  // EXPECT_LONGS_EQUAL(X(1),forest[X(2)]);
138  // EXPECT_LONGS_EQUAL(X(1),forest[X(3)]);
139  // EXPECT_LONGS_EQUAL(X(1),forest[X(4)]);
140  // EXPECT_LONGS_EQUAL(X(5),forest[X(5)]);
141  // EXPECT_LONGS_EQUAL(X(5),forest[X(6)]);
142 }
143 
145 // SL-FIX TEST( GaussianFactorGraph, split )
146 //{
147 // GaussianFactorGraph g;
148 // Matrix I = eye(2);
149 // Vector b = Vector_(0, 0, 0);
150 // g += X(1), I, X(2), I, b, model;
151 // g += X(1), I, X(3), I, b, model;
152 // g += X(1), I, X(4), I, b, model;
153 // g += X(2), I, X(3), I, b, model;
154 // g += X(2), I, X(4), I, b, model;
155 //
156 // PredecessorMap<string> tree;
157 // tree[X(1)] = X(1);
158 // tree[X(2)] = X(1);
159 // tree[X(3)] = X(1);
160 // tree[X(4)] = X(1);
161 //
162 // GaussianFactorGraph Ab1, Ab2;
163 // g.split<string, GaussianFactor>(tree, Ab1, Ab2);
164 // LONGS_EQUAL(3, Ab1.size());
165 // LONGS_EQUAL(2, Ab2.size());
166 //}
167 
169 // SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )
170 //{
171 // SymbolicFactorGraph G;
172 // G.push_factor("x1", "x2");
173 // G.push_factor("x1", "x3");
174 // G.push_factor("x1", "x4");
175 // G.push_factor("x2", "x3");
176 // G.push_factor("x2", "x4");
177 // G.push_factor("x3", "x4");
178 //
179 // SymbolicFactorGraph T, C;
180 // boost::tie(T, C) = G.splitMinimumSpanningTree();
181 //
182 // SymbolicFactorGraph expectedT, expectedC;
183 // expectedT.push_factor("x1", "x2");
184 // expectedT.push_factor("x1", "x3");
185 // expectedT.push_factor("x1", "x4");
186 // expectedC.push_factor("x2", "x3");
187 // expectedC.push_factor("x2", "x4");
188 // expectedC.push_factor("x3", "x4");
189 // CHECK(assert_equal(expectedT,T));
190 // CHECK(assert_equal(expectedC,C));
191 //}
192 
195 // * x1 - x2 - x3 - x4 - x5
196 // * | | / |
197 // * l1 l2 l3
198 // */
199 // SL-FIX TEST( FactorGraph, removeSingletons )
200 //{
201 // SymbolicFactorGraph G;
202 // G.push_factor("x1", "x2");
203 // G.push_factor("x2", "x3");
204 // G.push_factor("x3", "x4");
205 // G.push_factor("x4", "x5");
206 // G.push_factor("x2", "l1");
207 // G.push_factor("x3", "l2");
208 // G.push_factor("x4", "l2");
209 // G.push_factor("x4", "l3");
210 //
211 // SymbolicFactorGraph singletonGraph;
212 // set<Symbol> singletons;
213 // boost::tie(singletonGraph, singletons) = G.removeSingletons();
214 //
215 // set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
216 // CHECK(singletons_excepted == singletons);
217 //
218 // SymbolicFactorGraph singletonGraph_excepted;
219 // singletonGraph_excepted.push_factor("x2", "l1");
220 // singletonGraph_excepted.push_factor("x4", "l3");
221 // singletonGraph_excepted.push_factor("x1", "x2");
222 // singletonGraph_excepted.push_factor("x4", "x5");
223 // singletonGraph_excepted.push_factor("x2", "x3");
224 // CHECK(singletonGraph_excepted.equals(singletonGraph));
225 //}
226 
227 /* ************************************************************************* */
228 int main() {
229  TestResult tr;
230  return TestRegistry::runAllTests(tr);
231 }
232 /* ************************************************************************* */
CHECK
#define CHECK(condition)
Definition: Test.h:109
gtsam::PredecessorMap::insert
void insert(const KEY &key, const KEY &parent)
Definition: graph.h:61
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:54
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
p4
KeyInt p4(x4, 4)
gtsam::composePoses
boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Definition: graph-inl.h:174
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
std
Definition: Half.h:150
mpfr::root
const mpreal root(const mpreal &x, unsigned long int k, mp_rnd_t r=mpreal::get_default_rnd())
Definition: mpreal.h:2194
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:43
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::lago::I
static const Matrix I
Definition: lago.cpp:35
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
graph.h
Graph algorithm using boost library.
gtsam::predecessorMap2Graph
boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Definition: graph-inl.h:118
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
main
****int main()
Definition: testGraph.cpp:228
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::SGraph::Vertex
boost::graph_traits< SGraph< KEY > >::vertex_descriptor Vertex
Definition: graph.h:49
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:51
gtsam::findMinimumSpanningTree
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
Definition: graph-inl.h:232
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:52
gtsam::predecessorMap2Keys
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Definition: graph-inl.h:50
Pose2.h
2D Pose
X
#define X
Definition: icosphere.cpp:20
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
TEST
TEST(Ordering, predecessorMap2Keys)
Definition: testGraph.cpp:42

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:47:15