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

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:35