lago.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/lago.h>
20 
24 #include <gtsam/geometry/Pose2.h>
25 #include <gtsam/inference/Symbol.h>
26 #include <gtsam/base/timing.h>
27 #include <gtsam/base/kruskal.h>
28 
29 #include <iostream>
30 #include <stack>
31 #include <cmath>
32 
33 using namespace std;
34 
35 namespace gtsam {
36 namespace lago {
37 
39 
40 static const Matrix I = I_1x1;
41 static const Matrix I3 = I_3x3;
42 
44  noiseModel::Diagonal::Sigmas(Vector1(0));
46  noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
47 
48 /* ************************************************************************* */
56 static double computeThetaToRoot(const Key nodeKey,
57  const PredecessorMap& tree, const key2doubleMap& deltaThetaMap,
58  const key2doubleMap& thetaFromRootMap) {
59 
60  double nodeTheta = 0;
61  Key key_child = nodeKey; // the node
62  Key key_parent = 0; // the initialization does not matter
63  while (1) {
64  // We check if we reached the root
65  if (tree.at(key_child) == key_child) // if we reached the root
66  break;
67  // we sum the delta theta corresponding to the edge parent->child
68  nodeTheta += deltaThetaMap.at(key_child);
69  // we get the parent
70  key_parent = tree.at(key_child); // the parent
71  // we check if we connected to some part of the tree we know
72  if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) {
73  nodeTheta += thetaFromRootMap.at(key_parent);
74  break;
75  }
76  key_child = key_parent; // we move upwards in the tree
77  }
78  return nodeTheta;
79 }
80 
81 /* ************************************************************************* */
83  const PredecessorMap& tree) {
84 
85  key2doubleMap thetaToRootMap;
86 
87  // Orientation of the root
88  thetaToRootMap.emplace(kAnchorKey, 0.0);
89 
90  // for all nodes in the tree
91  for(const auto& [nodeKey, _]: deltaThetaMap) {
92  // compute the orientation wrt root
93  const double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
94  thetaToRootMap);
95  thetaToRootMap.emplace(nodeKey, nodeTheta);
96  }
97  return thetaToRootMap;
98 }
99 
100 /* ************************************************************************* */
102 /*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
103  key2doubleMap& deltaThetaMap,
104  /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) {
105 
106  // Get keys for which you want the orientation
107  size_t id = 0;
108  // Loop over the factors
109  for(const std::shared_ptr<NonlinearFactor>& factor: g) {
110  if (factor->keys().size() == 2) {
111  Key key1 = factor->keys()[0];
112  Key key2 = factor->keys()[1];
113  // recast to a between
114  std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
115  std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
116  if (!pose2Between)
117  continue;
118  // get the orientation - measured().theta();
119  double deltaTheta = pose2Between->measured().theta();
120  // insert (directed) orientations in the map "deltaThetaMap"
121  bool inTree = false;
122  if (tree.at(key1) == key2) { // key2 -> key1
123  deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta));
124  inTree = true;
125  } else if (tree.at(key2) == key1) { // key1 -> key2
126  deltaThetaMap.insert(pair<Key, double>(key2, deltaTheta));
127  inTree = true;
128  }
129  // store factor slot, distinguishing spanning tree edges from chordsIds
130  if (inTree == true)
131  spanningTreeIds.push_back(id);
132  else
133  // it's a chord!
134  chordsIds.push_back(id);
135  }
136  id++;
137  }
138 }
139 
140 /* ************************************************************************* */
141 // Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
143  Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) {
144 
145  // Get the relative rotation measurement from the between factor
146  std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
147  std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
148  if (!pose2Between)
149  throw invalid_argument(
150  "buildLinearOrientationGraph: invalid between factor!");
151  deltaTheta = (Vector(1) << pose2Between->measured().theta()).finished();
152 
153  // Retrieve the noise model for the relative rotation
154  SharedNoiseModel model = pose2Between->noiseModel();
155  std::shared_ptr<noiseModel::Diagonal> diagonalModel =
156  std::dynamic_pointer_cast<noiseModel::Diagonal>(model);
157  if (!diagonalModel)
158  throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
159  "(current version assumes diagonal noise model)!");
160  Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)).finished(); // std on the angular measurement
161  model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
162 }
163 
164 /* ************************************************************************* */
166  const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
167  const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
168  const PredecessorMap& tree) {
169 
170  GaussianFactorGraph lagoGraph;
171  Vector deltaTheta;
172  noiseModel::Diagonal::shared_ptr model_deltaTheta;
173 
174  // put original measurements in the spanning tree
175  for(const size_t& factorId: spanningTreeIds) {
176  const KeyVector& keys = g[factorId]->keys();
177  Key key1 = keys[0], key2 = keys[1];
178  getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
179  lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
180  }
181  // put regularized measurements in the chords
182  for(const size_t& factorId: chordsIds) {
183  const KeyVector& keys = g[factorId]->keys();
184  Key key1 = keys[0], key2 = keys[1];
185  getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
186  double key1_DeltaTheta_key2 = deltaTheta(0);
188  double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1)
189  - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
190  double k = std::round(k2pi_noise / (2 * M_PI));
191  //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
192  Vector deltaThetaRegularized = (Vector(1)
193  << key1_DeltaTheta_key2 - 2 * k * M_PI).finished();
194  lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
195  }
196  // prior on the anchor orientation
197  lagoGraph.add(kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
198  return lagoGraph;
199 }
200 
201 /* ************************************************************************* */
203  const NonlinearFactorGraph& pose2Graph) {
204 
206  Key minKey = kAnchorKey; // this initialization does not matter
207  bool minUnassigned = true;
208 
209  for(const std::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
210 
211  Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
212  Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
213  if (minUnassigned) {
214  minKey = key1;
215  minUnassigned = false;
216  }
217  if (key2 - key1 == 1) { // consecutive keys
218  tree.emplace(key2, key1);
219  if (key1 < minKey)
220  minKey = key1;
221  }
222  }
223  tree.emplace(minKey, kAnchorKey);
224  tree.emplace(kAnchorKey, kAnchorKey); // root
225  return tree;
226 }
227 
228 /*****************************************************************************/
230  const NonlinearFactorGraph& pose2Graph) {
231  // Compute the minimum spanning tree
232  const auto edgeWeights = std::vector<double>(pose2Graph.size(), 1.0);
233  const auto mstEdgeIndices =
234  utils::kruskal(pose2Graph, edgeWeights);
235 
236  // Create a PredecessorMap 'predecessorMap' such that:
237  // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
238  // the spanning tree
239  PredecessorMap predecessorMap;
240  std::map<Key, bool> visitationMap;
241  std::stack<std::pair<Key, Key>> stack;
242 
243  stack.push({kAnchorKey, kAnchorKey});
244  while (!stack.empty()) {
245  auto [u, parent] = stack.top();
246  stack.pop();
247  if (visitationMap[u]) continue;
248  visitationMap[u] = true;
249  predecessorMap[u] = parent;
250  for (const auto& edgeIdx : mstEdgeIndices) {
251  const auto v = pose2Graph[edgeIdx]->front();
252  const auto w = pose2Graph[edgeIdx]->back();
253  if ((v == u || w == u) && !visitationMap[v == u ? w : v]) {
254  stack.push({v == u ? w : v, u});
255  }
256  }
257  }
258 
259  return predecessorMap;
260 }
261 
262 /* ************************************************************************* */
263 // Return the orientations of a graph including only BetweenFactors<Pose2>
265  bool useOdometricPath) {
266  gttic(lago_computeOrientations);
267 
269  // Find a minimum spanning tree
270  if (useOdometricPath)
271  tree = findOdometricPath(pose2Graph);
272  else
273  tree = findMinimumSpanningTree(pose2Graph);
274 
275  // Create a linear factor graph (LFG) of scalars
276  key2doubleMap deltaThetaMap;
277  vector<size_t>
278  spanningTreeIds; // ids of between factors forming the spanning tree T
279  vector<size_t>
280  chordsIds; // ids of between factors corresponding to chordsIds wrt T
281  getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
282 
283  // temporary structure to correct wraparounds along loops
284  key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
285 
286  // regularize measurements and plug everything in a factor graph
288  spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
289 
290  // Solve the LFG
291  VectorValues orientationsLago = lagoGraph.optimize();
292 
293  return orientationsLago;
294 }
295 
296 /* ************************************************************************* */
298  bool useOdometricPath) {
299  // We "extract" the Pose2 subgraph of the original graph: this
300  // is done to properly model priors and avoiding operating on a larger graph
301  NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
302 
303  // Get orientations from relative orientation measurements
304  return computeOrientations(pose2Graph, useOdometricPath);
305 }
306 
307 /* ************************************************************************* */
309  VectorValues& orientationsLago) {
310  gttic(lago_computePoses);
311 
312  // Linearized graph on full poses
313  GaussianFactorGraph linearPose2graph;
314 
315  // We include the linear version of each between factor
316  for(const std::shared_ptr<NonlinearFactor>& factor: pose2graph) {
317 
318  std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
319  std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
320 
321  if (pose2Between) {
322  Key key1 = pose2Between->keys()[0];
323  double theta1 = orientationsLago.at(key1)(0);
324  double s1 = sin(theta1);
325  double c1 = cos(theta1);
326 
327  Key key2 = pose2Between->keys()[1];
328  double theta2 = orientationsLago.at(key2)(0);
329 
330  double linearDeltaRot = theta2 - theta1
331  - pose2Between->measured().theta();
332  linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize
333 
334  double dx = pose2Between->measured().x();
335  double dy = pose2Between->measured().y();
336 
337  Vector globalDeltaCart = //
338  (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy).finished();
339  Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot).finished(); // rhs
340  Matrix J1 = -I3;
341  J1(0, 2) = s1 * dx + c1 * dy;
342  J1(1, 2) = -c1 * dx + s1 * dy;
343  // Retrieve the noise model for the relative rotation
344  std::shared_ptr<noiseModel::Diagonal> diagonalModel =
345  std::dynamic_pointer_cast<noiseModel::Diagonal>(
346  pose2Between->noiseModel());
347 
348  linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
349  } else {
350  throw invalid_argument(
351  "computeLagoPoses: cannot manage non between factor here!");
352  }
353  }
354  // add prior
355  linearPose2graph.add(kAnchorKey, I3, Vector3(0.0, 0.0, 0.0), priorPose2Noise);
356 
357  // optimize
358  VectorValues posesLago = linearPose2graph.optimize();
359 
360  // put into Values structure
361  Values initialGuessLago;
362  for(const VectorValues::value_type& it: posesLago) {
363  Key key = it.first;
364  if (key != kAnchorKey) {
365  const Vector& poseVector = it.second;
366  Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
367  orientationsLago.at(key)(0) + poseVector(2));
368  initialGuessLago.insert(key, poseLago);
369  }
370  }
371  return initialGuessLago;
372 }
373 
374 /* ************************************************************************* */
375 Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
376  gttic(lago_initialize);
377 
378  // We "extract" the Pose2 subgraph of the original graph: this
379  // is done to properly model priors and avoiding operating on a larger graph
380  NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
381 
382  // Get orientations from relative orientation measurements
383  VectorValues orientationsLago = computeOrientations(pose2Graph,
384  useOdometricPath);
385 
386  // Compute the full poses
387  return computePoses(pose2Graph, orientationsLago);
388 }
389 
390 /* ************************************************************************* */
392  const Values& initialGuess) {
393  Values initialGuessLago;
394 
395  // get the orientation estimates from LAGO
396  VectorValues orientations = initializeOrientations(graph);
397 
398  // for all nodes in the tree
399  for(const VectorValues::value_type& it: orientations) {
400  Key key = it.first;
401  if (key != kAnchorKey) {
402  const Pose2& pose = initialGuess.at<Pose2>(key);
403  const Vector& orientation = it.second;
404  Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
405  initialGuessLago.insert(key, poseLago);
406  }
407  }
408  return initialGuessLago;
409 }
410 
411 } // end of namespace lago
412 } // end of namespace gtsam
key1
const Symbol key1('v', 1)
gtsam::lago::initialize
Values initialize(const NonlinearFactorGraph &graph, const Values &initialGuess)
Definition: lago.cpp:391
InitializePose.h
common code between lago.* (2D) and InitializePose3.* (3D)
timing.h
Timing utilities.
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::lago::priorPose2Noise
static const noiseModel::Diagonal::shared_ptr priorPose2Noise
Definition: lago.cpp:45
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Pose2.h
2D Pose
kruskal.h
gtsam::lago::I3
static const Matrix I3
Definition: lago.cpp:41
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::utils::kruskal
std::vector< size_t > kruskal(const FactorGraph< FACTOR > &fg, const std::vector< double > &weights)
Definition: kruskal-inl.h:55
gtsam::lago::getDeltaThetaAndNoise
static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector &deltaTheta, noiseModel::Diagonal::shared_ptr &model_deltaTheta)
Definition: lago.cpp:142
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
tree
Definition: testExpression.cpp:212
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::VectorValues::at
Vector & at(Key j)
Definition: VectorValues.h:142
gtsam::Rot2::theta
double theta() const
Definition: Rot2.h:186
gtsam::lago::computeThetasToRoot
key2doubleMap computeThetasToRoot(const key2doubleMap &deltaThetaMap, const PredecessorMap &tree)
Definition: lago.cpp:82
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
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
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
gtsam::lago::PredecessorMap
std::map< Key, Key > PredecessorMap
Definition: lago.h:45
c1
static double c1
Definition: airy.c:54
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::lago::priorOrientationNoise
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise
Definition: lago.cpp:43
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
PriorFactor.h
gtsam::FactorGraph::front
sharedFactor front() const
Definition: FactorGraph.h:345
key2
const Symbol key2('v', 2)
gtsam::VectorValues
Definition: VectorValues.h:74
I
#define I
Definition: main.h:112
BetweenFactor.h
gtsam::stack
Matrix stack(size_t nrMatrices,...)
Definition: Matrix.cpp:395
gtsam::lago::computeThetaToRoot
static double computeThetaToRoot(const Key nodeKey, const PredecessorMap &tree, const key2doubleMap &deltaThetaMap, const key2doubleMap &thetaFromRootMap)
Definition: lago.cpp:56
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
round
double round(double x)
Definition: round.c:38
gtsam::lago::findOdometricPath
static PredecessorMap findOdometricPath(const NonlinearFactorGraph &pose2Graph)
Definition: lago.cpp:202
gtsam::lago::computeOrientations
static VectorValues computeOrientations(const NonlinearFactorGraph &pose2Graph, bool useOdometricPath)
Definition: lago.cpp:264
Symbol.h
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::lago::initializeOrientations
VectorValues initializeOrientations(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:297
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
key
const gtsam::Symbol key('X', 0)
gtsam::FactorGraph::back
sharedFactor back() const
Definition: FactorGraph.h:348
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::Rot2
Definition: Rot2.h:35
gtsam::lago::findMinimumSpanningTree
PredecessorMap findMinimumSpanningTree(const NonlinearFactorGraph &pose2Graph)
Definition: lago.cpp:229
gtsam::lago::computePoses
Values computePoses(const NonlinearFactorGraph &pose2graph, VectorValues &orientationsLago)
Definition: lago.cpp:308
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
lago.h
Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization)....
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
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
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::initialize::kAnchorKey
static constexpr Key kAnchorKey
Definition: InitializePose.h:30
gtsam::lago::key2doubleMap
std::map< Key, double > key2doubleMap
Definition: lago.h:44
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: GaussianFactorGraph.cpp:309
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
_
constexpr descr< N - 1 > _(char const (&text)[N])
Definition: descr.h:109
max
#define max(a, b)
Definition: datatypes.h:20
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
gtsam::VectorValues::value_type
Values::value_type value_type
Typedef to pair<Key, Vector>
Definition: VectorValues.h:87
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:03