Typedefs | Functions | Variables
gtsam::lago Namespace Reference

Typedefs

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

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))
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33

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))
Eigen::Vector3d Vector3
Definition: Vector.h:43
Array< double, 1, 3 > e(1./3., 0.5, 2.)

Definition at line 45 of file lago.cpp.



gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:16