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

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]