gtsam  4.1.0
gtsam
lago.h File Reference

Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization). More...

Go to the source code of this file.

Namespaces

 gtsam
 Global functions in a separate testing namespace.
 

Typedefs

typedef std::map< Key, double > gtsam::lago::key2doubleMap
 

Functions

key2doubleMap gtsam::lago::computeThetasToRoot (const key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree)
 Compute the cumulative orientations (without wrapping) for all nodes wrt the root (root has zero orientation).
 
void gtsam::lago::getSymbolicGraph (std::vector< size_t > &spanningTreeIds, std::vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree, const NonlinearFactorGraph &g)
 Given a factor graph "g", and a spanning tree "tree", select the nodes belonging to the tree and to g, and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree. More...
 
GaussianFactorGraph gtsam::lago::buildLinearOrientationGraph (const std::vector< size_t > &spanningTreeIds, const std::vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap< Key > &tree)
 Linear factor graph with regularized orientation measurements. More...
 
VectorValues gtsam::lago::initializeOrientations (const NonlinearFactorGraph &graph, bool useOdometricPath=true)
 LAGO: Return the orientations of the Pose2 in a generic factor graph.
 
Values gtsam::lago::initialize (const NonlinearFactorGraph &graph, bool useOdometricPath=true)
 Return the values for the Pose2 in a generic factor graph.
 
Values gtsam::lago::initialize (const NonlinearFactorGraph &graph, const Values &initialGuess)
 Only correct the orientation part in initialGuess.
 

Detailed Description

Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization).

Author
Luca Carlone
Frank Dellaert
Date
May 14, 2014

see papers:

L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate approximation for planar pose graph optimization, IJRR, 2014.

L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation for graph-based simultaneous localization and mapping, RSS, 2011.

Parameters
graphnonlinear factor graph (can include arbitrary factors but we assume that there is a subgraph involving Pose2 and betweenFactors). Also in the current version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) and a prior on x0. This assumption can be relaxed by using the extra argument useOdometricPath = false, although this part of code is not stable yet.
Returns
Values: initial guess from LAGO (only pose2 are initialized)
Author
Luca Carlone
Frank Dellaert
Date
May 14, 2014

Function Documentation

◆ buildLinearOrientationGraph()

GTSAM_EXPORT GaussianFactorGraph gtsam::lago::buildLinearOrientationGraph ( const vector< size_t > &  spanningTreeIds,
const vector< size_t > &  chordsIds,
const NonlinearFactorGraph g,
const key2doubleMap &  orientationsToRoot,
const PredecessorMap< Key > &  tree 
)

Linear factor graph with regularized orientation measurements.

cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl;

◆ getSymbolicGraph()

GTSAM_EXPORT void gtsam::lago::getSymbolicGraph ( std::vector< size_t > &  spanningTreeIds,
std::vector< size_t > &  chordsIds,
key2doubleMap &  deltaThetaMap,
const PredecessorMap< Key > &  tree,
const NonlinearFactorGraph g 
)

Given a factor graph "g", and a spanning tree "tree", select the nodes belonging to the tree and to g, and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree.

Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: for a node key2, s.t. tree[key2]=key1, the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1]