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

typedef std::map< Key, Key >	PredecessorMap

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

GaussianFactorGraph	buildLinearOrientationGraph (const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap &tree)

static VectorValues	computeOrientations (const NonlinearFactorGraph &pose2Graph, bool useOdometricPath)

Values	computePoses (const NonlinearFactorGraph &pose2graph, VectorValues &orientationsLago)

key2doubleMap	computeThetasToRoot (const key2doubleMap &deltaThetaMap, const PredecessorMap &tree)

static double	computeThetaToRoot (const Key nodeKey, const PredecessorMap &tree, const key2doubleMap &deltaThetaMap, const key2doubleMap &thetaFromRootMap)

PredecessorMap	findMinimumSpanningTree (const NonlinearFactorGraph &pose2Graph)

static PredecessorMap	findOdometricPath (const NonlinearFactorGraph &pose2Graph)

static void	getDeltaThetaAndNoise (NonlinearFactor::shared_ptr factor, Vector &deltaTheta, noiseModel::Diagonal::shared_ptr &model_deltaTheta)

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

void	getSymbolicGraph (vector< size_t > &spanningTreeIds, vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap &tree, const NonlinearFactorGraph &g)

Values	initialize (const NonlinearFactorGraph &graph, bool useOdometricPath)

Values	initialize (const NonlinearFactorGraph &graph, const Values &initialGuess)

VectorValues	initializeOrientations (const NonlinearFactorGraph &graph, bool useOdometricPath)

Variables
static const Matrix	I = I_1x1

static const Matrix	I3 = I_3x3

static const noiseModel::Diagonal::shared_ptr	priorOrientationNoise

static const noiseModel::Diagonal::shared_ptr	priorPose2Noise

Typedef Documentation

◆ key2doubleMap

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

Definition at line 44 of file lago.h.

◆ PredecessorMap

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

Definition at line 45 of file lago.h.

Function Documentation

◆ buildLinearOrientationGraph() [1/2]

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

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

Definition at line 165 of file lago.cpp.

◆ buildLinearOrientationGraph() [2/2]

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

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

Definition at line 165 of file lago.cpp.

◆ computeOrientations()

static VectorValues gtsam::lago::computeOrientations	(	const NonlinearFactorGraph &	pose2Graph,
		bool	useOdometricPath
	)

static

Definition at line 264 of file lago.cpp.

◆ computePoses()

Values gtsam::lago::computePoses	(	const NonlinearFactorGraph &	pose2graph,
		VectorValues &	orientationsLago
	)

Definition at line 308 of file lago.cpp.

◆ 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).

Definition at line 82 of file lago.cpp.

◆ computeThetaToRoot()

static double gtsam::lago::computeThetaToRoot	(	const Key	nodeKey,
		const PredecessorMap &	tree,
		const key2doubleMap &	deltaThetaMap,
		const key2doubleMap &	thetaFromRootMap
	)

static

Compute the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) for a node (nodeKey). The function starts at the nodes and moves towards the root summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap". The root is assumed to have orientation zero.

Definition at line 56 of file lago.cpp.

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

Definition at line 229 of file lago.cpp.

◆ findOdometricPath()

static PredecessorMap gtsam::lago::findOdometricPath ( const NonlinearFactorGraph & pose2Graph )

static

Definition at line 202 of file lago.cpp.

◆ getDeltaThetaAndNoise()

static void gtsam::lago::getDeltaThetaAndNoise	(	NonlinearFactor::shared_ptr	factor,
		Vector &	deltaTheta,
		noiseModel::Diagonal::shared_ptr &	model_deltaTheta
	)

static

Definition at line 142 of file lago.cpp.

◆ getSymbolicGraph() [1/2]

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]

Definition at line 101 of file lago.cpp.

◆ getSymbolicGraph() [2/2]

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]

Definition at line 101 of file lago.cpp.

◆ 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

Definition at line 375 of file lago.cpp.

◆ initialize() [2/2]

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

Only correct the orientation part in initialGuess

Definition at line 391 of file lago.cpp.

◆ 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

Definition at line 297 of file lago.cpp.

Variable Documentation

◆ I

const Matrix gtsam::lago::I = I_1x1

static

Definition at line 40 of file lago.cpp.

◆ I3

const Matrix gtsam::lago::I3 = I_3x3

static

Definition at line 41 of file lago.cpp.

◆ priorOrientationNoise

const noiseModel::Diagonal::shared_ptr gtsam::lago::priorOrientationNoise

static

Initial value:

=

noiseModel::Diagonal::Sigmas(Vector1(0))

Definition at line 43 of file lago.cpp.

◆ priorPose2Noise

const noiseModel::Diagonal::shared_ptr gtsam::lago::priorPose2Noise

static

Initial value:

=

noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8))

Definition at line 45 of file lago.cpp.

Typedefs

Functions

Variables

Typedef Documentation

◆ key2doubleMap

◆ PredecessorMap

Function Documentation

◆ buildLinearOrientationGraph() [1/2]

◆ buildLinearOrientationGraph() [2/2]

◆ computeOrientations()

◆ computePoses()

◆ computeThetasToRoot()

◆ computeThetaToRoot()

◆ findMinimumSpanningTree()

◆ findOdometricPath()

◆ getDeltaThetaAndNoise()

◆ getSymbolicGraph() [1/2]

◆ getSymbolicGraph() [2/2]

◆ initialize() [1/2]

◆ initialize() [2/2]

◆ initializeOrientations()

Variable Documentation

◆ I

◆ I3

◆ priorOrientationNoise

◆ priorPose2Noise