gtsam
4.0.0
gtsam

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.  
Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization).
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 graphbased simultaneous localization and mapping, RSS, 2011.
graph  nonlinear 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. 
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;
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]