lago.cpp File Reference

#include <gtsam/slam/lago.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/kruskal.h>
#include <iostream>
#include <stack>
#include <cmath>

Include dependency graph for lago.cpp:

Go to the source code of this file.

Namespaces
	gtsam
	traits
	gtsam::lago

Functions
GaussianFactorGraph	gtsam::lago::buildLinearOrientationGraph (const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap &tree)
static VectorValues	gtsam::lago::computeOrientations (const NonlinearFactorGraph &pose2Graph, bool useOdometricPath)
Values	gtsam::lago::computePoses (const NonlinearFactorGraph &pose2graph, VectorValues &orientationsLago)
key2doubleMap	gtsam::lago::computeThetasToRoot (const key2doubleMap &deltaThetaMap, const PredecessorMap &tree)
static double	gtsam::lago::computeThetaToRoot (const Key nodeKey, const PredecessorMap &tree, const key2doubleMap &deltaThetaMap, const key2doubleMap &thetaFromRootMap)
PredecessorMap	gtsam::lago::findMinimumSpanningTree (const NonlinearFactorGraph &pose2Graph)
static PredecessorMap	gtsam::lago::findOdometricPath (const NonlinearFactorGraph &pose2Graph)
static void	gtsam::lago::getDeltaThetaAndNoise (NonlinearFactor::shared_ptr factor, Vector &deltaTheta, noiseModel::Diagonal::shared_ptr &model_deltaTheta)
void	gtsam::lago::getSymbolicGraph (vector< size_t > &spanningTreeIds, vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap &tree, const NonlinearFactorGraph &g)
Values	gtsam::lago::initialize (const NonlinearFactorGraph &graph, bool useOdometricPath)
Values	gtsam::lago::initialize (const NonlinearFactorGraph &graph, const Values &initialGuess)
VectorValues	gtsam::lago::initializeOrientations (const NonlinearFactorGraph &graph, bool useOdometricPath)

Variables
static const Matrix	gtsam::lago::I = I_1x1
static const Matrix	gtsam::lago::I3 = I_3x3
static const noiseModel::Diagonal::shared_ptr	gtsam::lago::priorOrientationNoise
static const noiseModel::Diagonal::shared_ptr	gtsam::lago::priorPose2Noise