testLago.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 
22 #include <gtsam/slam/lago.h>
23 #include <gtsam/slam/dataset.h>
24 #include <gtsam/slam/BetweenFactor.h>
25 #include <gtsam/geometry/Pose2.h>
26 #include <gtsam/inference/Symbol.h>
27 
28 #include <CppUnitLite/TestHarness.h>
29 
30 #include <cmath>
31 
32 using namespace std;
33 using namespace gtsam;
34 using namespace boost::assign;
35 
36 static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
37 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
38 
39 namespace simpleLago {
40 // We consider a small graph:
41 // symbolic FG
42 // x2 0 1
43 // / | \ 1 2
44 // / | \ 2 3
45 // x3 | x1 2 0
46 // \ | / 0 3
47 // \ | /
48 // x0
49 //
50 
51 static Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
52 static Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
53 static Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
54 static Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
55 
56 NonlinearFactorGraph graph() {
57  NonlinearFactorGraph g;
58  g.add(BetweenFactor<Pose2>(x0, x1, pose0.between(pose1), model));
59  g.add(BetweenFactor<Pose2>(x1, x2, pose1.between(pose2), model));
60  g.add(BetweenFactor<Pose2>(x2, x3, pose2.between(pose3), model));
61  g.add(BetweenFactor<Pose2>(x2, x0, pose2.between(pose0), model));
62  g.add(BetweenFactor<Pose2>(x0, x3, pose0.between(pose3), model));
63  g.addPrior(x0, pose0, model);
64  return g;
65 }
66 }
67 
68 /* *************************************************************************** */
69 TEST( Lago, checkSTandChords ) {
70  NonlinearFactorGraph g = simpleLago::graph();
71  PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
72  BetweenFactor<Pose2> >(g);
73 
74  lago::key2doubleMap deltaThetaMap;
75  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
76  vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
77  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
78 
79  DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
80  DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
81  DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the ST(0->3)
82 
83 }
84 
85 /* *************************************************************************** */
86 TEST( Lago, orientationsOverSpanningTree ) {
87  NonlinearFactorGraph g = simpleLago::graph();
88  PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
89  BetweenFactor<Pose2> >(g);
90 
91  // check the tree structure
92  EXPECT_LONGS_EQUAL(tree[x0], x0);
93  EXPECT_LONGS_EQUAL(tree[x1], x0);
94  EXPECT_LONGS_EQUAL(tree[x2], x0);
95  EXPECT_LONGS_EQUAL(tree[x3], x0);
96 
97  lago::key2doubleMap expected;
98  expected[x0]= 0;
99  expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1))
100  expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
101  expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3))
102 
103  lago::key2doubleMap deltaThetaMap;
104  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
105  vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
106  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
107 
108  lago::key2doubleMap actual;
109  actual = lago::computeThetasToRoot(deltaThetaMap, tree);
110  DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
111  DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
112  DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
113  DOUBLES_EQUAL(expected[x3], actual[x3], 1e-6);
114 }
115 
116 /* *************************************************************************** */
117 TEST( Lago, regularizedMeasurements ) {
118  NonlinearFactorGraph g = simpleLago::graph();
119  PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
120  BetweenFactor<Pose2> >(g);
121 
122  lago::key2doubleMap deltaThetaMap;
123  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
124  vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
125  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
126 
127  lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree);
128 
129  GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
130  std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
131  // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
132  Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)).finished();
133  // this is the whitened error, so we multiply by the std to unwhiten
134  actual = 0.1 * actual;
135  // Expected regularized measurements (same for the spanning tree, corrected for the chordsIds)
136  Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2).finished();
137 
138  EXPECT(assert_equal(expected, actual, 1e-6));
139 }
140 
141 /* *************************************************************************** */
142 TEST( Lago, smallGraphVectorValues ) {
143  bool useOdometricPath = false;
144  VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath);
145 
146  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
147  EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
148  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
149  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
150  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
151 }
152 
153 /* *************************************************************************** */
154 TEST( Lago, smallGraphVectorValuesSP ) {
155 
156  VectorValues initial = lago::initializeOrientations(simpleLago::graph());
157 
158  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
159  EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
160  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
161  EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6));
162  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6));
163 }
164 
165 /* *************************************************************************** */
166 TEST( Lago, multiplePosePriors ) {
167  bool useOdometricPath = false;
168  NonlinearFactorGraph g = simpleLago::graph();
169  g.addPrior(x1, simpleLago::pose1, model);
170  VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
171 
172  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
173  EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
174  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
175  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
176  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
177 }
178 
179 /* *************************************************************************** */
180 TEST( Lago, multiplePosePriorsSP ) {
181  NonlinearFactorGraph g = simpleLago::graph();
182  g.addPrior(x1, simpleLago::pose1, model);
183  VectorValues initial = lago::initializeOrientations(g);
184 
185  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
186  EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
187  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
188  EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6));
189  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6));
190 }
191 
192 /* *************************************************************************** */
193 TEST( Lago, multiplePoseAndRotPriors ) {
194  bool useOdometricPath = false;
195  NonlinearFactorGraph g = simpleLago::graph();
196  g.addPrior(x1, simpleLago::pose1.theta(), model);
197  VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
198 
199  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
200  EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
201  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
202  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
203  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
204 }
205 
206 /* *************************************************************************** */
207 TEST( Lago, multiplePoseAndRotPriorsSP ) {
208  NonlinearFactorGraph g = simpleLago::graph();
209  g.addPrior(x1, simpleLago::pose1.theta(), model);
210  VectorValues initial = lago::initializeOrientations(g);
211 
212  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
213  EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
214  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
215  EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6));
216  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6));
217 }
218 
219 /* *************************************************************************** */
220 TEST( Lago, smallGraphValues ) {
221 
222  // we set the orientations in the initial guess to zero
223  Values initialGuess;
224  initialGuess.insert(x0,Pose2(simpleLago::pose0.x(),simpleLago::pose0.y(),0.0));
225  initialGuess.insert(x1,Pose2(simpleLago::pose1.x(),simpleLago::pose1.y(),0.0));
226  initialGuess.insert(x2,Pose2(simpleLago::pose2.x(),simpleLago::pose2.y(),0.0));
227  initialGuess.insert(x3,Pose2(simpleLago::pose3.x(),simpleLago::pose3.y(),0.0));
228 
229  // lago does not touch the Cartesian part and only fixed the orientations
230  Values actual = lago::initialize(simpleLago::graph(), initialGuess);
231 
232  // we are in a noiseless case
233  Values expected;
234  expected.insert(x0,simpleLago::pose0);
235  expected.insert(x1,simpleLago::pose1);
236  expected.insert(x2,simpleLago::pose2);
237  expected.insert(x3,simpleLago::pose3);
238 
239  EXPECT(assert_equal(expected, actual, 1e-6));
240 }
241 
242 /* *************************************************************************** */
243 TEST( Lago, smallGraph2 ) {
244 
245  // lago does not touch the Cartesian part and only fixed the orientations
246  Values actual = lago::initialize(simpleLago::graph());
247 
248  // we are in a noiseless case
249  Values expected;
250  expected.insert(x0,simpleLago::pose0);
251  expected.insert(x1,simpleLago::pose1);
252  expected.insert(x2,simpleLago::pose2);
253  expected.insert(x3,simpleLago::pose3);
254 
255  EXPECT(assert_equal(expected, actual, 1e-6));
256 }
257 
258 /* *************************************************************************** */
259 TEST( Lago, largeGraphNoisy_orientations ) {
260 
261  string inputFile = findExampleDataFile("noisyToyGraph");
262  NonlinearFactorGraph::shared_ptr g;
263  Values::shared_ptr initial;
264  boost::tie(g, initial) = readG2o(inputFile);
265 
266  // Add prior on the pose having index (key) = 0
267  NonlinearFactorGraph graphWithPrior = *g;
268  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
269  graphWithPrior.addPrior(0, Pose2(), priorModel);
270 
271  VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
272  Values actual;
273  Key keyAnc = symbol('Z',9999999);
274  for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
275  Key key = it->first;
276  if (key != keyAnc){
277  Vector orientation = actualVV.at(key);
278  Pose2 poseLago = Pose2(0.0,0.0,orientation(0));
279  actual.insert(key, poseLago);
280  }
281  }
282  string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
283  NonlinearFactorGraph::shared_ptr gmatlab;
284  Values::shared_ptr expected;
285  boost::tie(gmatlab, expected) = readG2o(matlabFile);
286 
287  for(const auto key_val: *expected){
288  Key k = key_val.key;
289  EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
290  }
291 }
292 
293 /* *************************************************************************** */
294 TEST( Lago, largeGraphNoisy ) {
295 
296  string inputFile = findExampleDataFile("noisyToyGraph");
297  NonlinearFactorGraph::shared_ptr g;
298  Values::shared_ptr initial;
299  boost::tie(g, initial) = readG2o(inputFile);
300 
301  // Add prior on the pose having index (key) = 0
302  NonlinearFactorGraph graphWithPrior = *g;
303  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
304  graphWithPrior.addPrior(0, Pose2(), priorModel);
305 
306  Values actual = lago::initialize(graphWithPrior);
307 
308  string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
309  NonlinearFactorGraph::shared_ptr gmatlab;
310  Values::shared_ptr expected;
311  boost::tie(gmatlab, expected) = readG2o(matlabFile);
312 
313  for(const auto key_val: *expected){
314  Key k = key_val.key;
315  EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
316  }
317 }
318 
319 
320 /* ************************************************************************* */
321 int main() {
322  TestResult tr;
323  return TestRegistry::runAllTests(tr);
324 }
325 /* ************************************************************************* */
326 
simpleLago::pose3
static Pose2 pose3
Definition: testLago.cpp:54
y
Scalar * y
Definition: level1_cplx_impl.h:102
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
M_PI
#define M_PI
Definition: main.h:78
main
int main()
Definition: testLago.cpp:321
simpleLago::pose0
static Pose2 pose0
Definition: testLago.cpp:51
initial
Values initial
Definition: OdometryOptimize.cpp:2
std
Definition: Half.h:150
gtsam::VectorValues::end
iterator end()
Iterator over variables.
Definition: VectorValues.h:228
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:190
x3
static Symbol x3('x', 3)
gtsam::VectorValues::at
Vector & at(Key j)
Definition: VectorValues.h:137
theta
Rot2 theta
Definition: testMagFactor.cpp:44
g
void g(const string &key, int i)
Definition: testBTree.cpp:43
gtsam::lago::buildLinearOrientationGraph
GaussianFactorGraph buildLinearOrientationGraph(const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap< Key > &tree)
Definition: lago.cpp:161
TEST
TEST(Lago, checkSTandChords)
Definition: testLago.cpp:69
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:229
gtsam::findExampleDataFile
string findExampleDataFile(const string &name)
Definition: dataset.cpp:65
lago.h
Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization). see papers:
inputFile
string inputFile
Definition: SolverComparer.cpp:91
gtsam::NonlinearFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
Definition: NonlinearFactorGraph.h:84
gtsam.examples.Pose2SLAMExample_g2o.priorModel
priorModel
Definition: Pose2SLAMExample_g2o.py:56
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:342
simpleLago::pose2
static Pose2 pose2
Definition: testLago.cpp:53
simpleLago::graph
NonlinearFactorGraph graph()
Definition: testLago.cpp:56
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:134
gtsam::lago::initializeOrientations
VectorValues initializeOrientations(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:258
gtsam::noiseModel::Diagonal::shared_ptr
boost::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:301
gtsam::GaussianFactorGraph::jacobian
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:217
gtsam::lago::getSymbolicGraph
void getSymbolicGraph(vector< size_t > &spanningTreeIds, vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree, const NonlinearFactorGraph &g)
Definition: lago.cpp:97
x0
static Symbol x0('x', 0)
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
simpleLago::pose1
static Pose2 pose1
Definition: testLago.cpp:52
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
model
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1))
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
x2
static Symbol x2('x', 2)
gtsam::lago::key2doubleMap
std::map< Key, double > key2doubleMap
Definition: lago.h:45
Pose2.h
2D Pose
gtsam::readG2o
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:615
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::lago::computeThetasToRoot
key2doubleMap computeThetasToRoot(const key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree)
Definition: lago.cpp:77
gtsam::VectorValues::begin
iterator begin()
Iterator over variables.
Definition: VectorValues.h:226
gtsam::lago::initialize
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:338
x1
static Symbol x1('x', 1)
dataset.h
utility functions for loading datasets
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

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