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

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

Include dependency graph for lago.h:

Go to the source code of this file.

Namespaces
	gtsam

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

typedef std::map< Key, Key >	gtsam::lago::PredecessorMap

Functions
GTSAM_EXPORT key2doubleMap	gtsam::lago::computeThetasToRoot (const key2doubleMap &deltaThetaMap, const PredecessorMap &tree)

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

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

GTSAM_EXPORT PredecessorMap	gtsam::lago::findMinimumSpanningTree (const NonlinearFactorGraph &pose2Graph)

GTSAM_EXPORT VectorValues	gtsam::lago::initializeOrientations (const NonlinearFactorGraph &graph, bool useOdometricPath=true)

GTSAM_EXPORT Values	gtsam::lago::initialize (const NonlinearFactorGraph &graph, bool useOdometricPath=true)

GTSAM_EXPORT Values	gtsam::lago::initialize (const NonlinearFactorGraph &graph, const Values &initialGuess)

Detailed Description

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 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 std::vector< size_t > &	spanningTreeIds,
		const std::vector< size_t > &	chordsIds,
		const NonlinearFactorGraph &	g,
		const key2doubleMap &	orientationsToRoot,
		const PredecessorMap &	tree
	)

Linear factor graph with regularized orientation measurements

◆ computeThetasToRoot()

GTSAM_EXPORT key2doubleMap gtsam::lago::computeThetasToRoot	(	const key2doubleMap &	deltaThetaMap,
		const PredecessorMap &	tree
	)

Compute the cumulative orientations (without wrapping) for all nodes wrt the root (root has zero orientation).

◆ findMinimumSpanningTree()

GTSAM_EXPORT PredecessorMap gtsam::lago::findMinimumSpanningTree ( const NonlinearFactorGraph & pose2Graph )

Given a "pose2" factor graph, find its minimum spanning tree. Note: All 'Pose2' Between factors are given equal weightage. Note: Assumes all the edges (factors) are Between factors.

◆ getSymbolicGraph()

GTSAM_EXPORT void gtsam::lago::getSymbolicGraph	(	std::vector< size_t > &	spanningTreeIds,
		std::vector< size_t > &	chordsIds,
		key2doubleMap &	deltaThetaMap,
		const PredecessorMap &	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]

◆ initialize() [1/2]

GTSAM_EXPORT Values gtsam::lago::initialize	(	const NonlinearFactorGraph &	graph,
		bool	useOdometricPath = `true`
	)

Return the values for the Pose2 in a generic factor graph

◆ initialize() [2/2]

GTSAM_EXPORT Values gtsam::lago::initialize	(	const NonlinearFactorGraph &	graph,
		const Values &	initialGuess
	)

Only correct the orientation part in initialGuess

◆ initializeOrientations()

GTSAM_EXPORT VectorValues gtsam::lago::initializeOrientations	(	const NonlinearFactorGraph &	graph,
		bool	useOdometricPath = `true`
	)

LAGO: Return the orientations of the Pose2 in a generic factor graph

Namespaces

Typedefs

Functions