Loading...
Searching...
No Matches
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

namespace | 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.
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.
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

  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.

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]