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

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< Key > &tree)

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

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

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

key2doubleMap	computeThetasToRoot (const key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree)

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

static PredecessorMap< Key >	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< Key > &tree, const NonlinearFactorGraph &g)

void	getSymbolicGraph (vector< size_t > &spanningTreeIds, vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &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

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

Definition at line 45 of file lago.h.

Function Documentation

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< Key > &	tree
	)

Linear factor graph with regularized orientation measurements

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

Definition at line 161 of file lago.cpp.

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

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

Definition at line 161 of file lago.cpp.

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

static

Definition at line 226 of file lago.cpp.

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

Definition at line 270 of file lago.cpp.

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

Definition at line 77 of file lago.cpp.

static double gtsam::lago::computeThetaToRoot	(	const Key	nodeKey,
		const PredecessorMap< Key > &	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 51 of file lago.cpp.

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

static

Definition at line 198 of file lago.cpp.

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

static

Definition at line 138 of file lago.cpp.

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]

Definition at line 97 of file lago.cpp.

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]

Definition at line 97 of file lago.cpp.

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 338 of file lago.cpp.

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

Only correct the orientation part in initialGuess

Definition at line 354 of file lago.cpp.

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 258 of file lago.cpp.

Variable Documentation

const Matrix gtsam::lago::I = I_1x1

static

Definition at line 35 of file lago.cpp.

const Matrix gtsam::lago::I3 = I_3x3

static

Definition at line 36 of file lago.cpp.

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

static

Initial value:

=

noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished())

ceres::Vector

Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector

Definition: gtsam/3rdparty/ceres/eigen.h:38

Definition at line 38 of file lago.cpp.

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

static

Initial value:

=

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

gtsam::Vector3

Eigen::Vector3d Vector3

Definition: Vector.h:43

e

Array< double, 1, 3 > e(1./3., 0.5, 2.)

Definition at line 40 of file lago.cpp.

Typedefs

Functions

Variables

Typedef Documentation

Function Documentation

Variable Documentation