12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Doxygen documentation

release/4.3a0
dellaert 2014-05-31 13:26:13 -04:00
parent dfb620b5de
commit e5344d3d92
1 changed files with 48 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
gtsam/nonlinear/lago.h
View File

@ -42,53 +42,70 @@
namespace gtsam { namespace gtsam {
namespace lago { namespace lago {
typedef std::map<Key,double> key2doubleMap; typedef std::map<Key, double> key2doubleMap;
/* This function computes 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 * Compute the cumulative orientation (without wrapping) wrt the root of a
* summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap" * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and
* The root is assumed to have orientation zero. * moves towards the root summing up the (directed) rotation measurements.
* Relative measurements are encoded in "deltaThetaMap".
* The root is assumed to have orientation zero.
*/ */
GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree, GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap); const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap);
/* This function computes the cumulative orientations (without wrapping) /**
* for all node wrt the root (root has zero orientation) * Compute the cumulative orientations (without wrapping)
* for all nodes wrt the root (root has zero orientation).
*/ */
GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, GTSAM_EXPORT key2doubleMap computeThetasToRoot(
const PredecessorMap<Key>& tree); const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
/* Given a factor graph "g", and a spanning tree "tree", the function selects 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 * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
* Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: * to the tree and to g, and stores the factor slots corresponding to edges in the
* for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1] * 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]
*/ */
GTSAM_EXPORT void getSymbolicGraph( GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap, /*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g); key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */ /**
* Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
*/
GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
/* Linear factor graph with regularized orientation measurements */ /** Linear factor graph with regularized orientation measurements */
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds, GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree); const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ /** Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(
const NonlinearFactorGraph& graph);
/* Returns the orientations of a graph including only BetweenFactors<Pose2> */ /** Return the orientations of a graph including only BetweenFactors<Pose2> */
GTSAM_EXPORT VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); GTSAM_EXPORT VectorValues computeOrientations(
const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ /** LAGO: Return the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath = true); GTSAM_EXPORT VectorValues initializeOrientations(
const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Returns the values for the Pose2 in a generic factor graph */ /** Return the values for the Pose2 in a generic factor graph */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath = true); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */ /** Only correct the orientation part in initialGuess */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
const Values& initialGuess);
} // end of namespace lago } // end of namespace lago
} // end of namespace gtsam } // end of namespace gtsam