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

Made three helper functions static (not directly called in testLago)

release/4.3a0
dellaert 2014-05-31 13:44:23 -04:00
parent 970d49f60b
commit 8d54460d2c
2 changed files with 18 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/nonlinear/lago.cpp
View File

@ -37,8 +37,16 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* ************************************************************************* */ /* ************************************************************************* */
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree, /**
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) { * 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.
*/
static double computeThetaToRoot(const Key nodeKey,
const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap) {
double nodeTheta = 0; double nodeTheta = 0;
Key key_child = nodeKey; // the node Key key_child = nodeKey; // the node
@ -122,7 +130,8 @@ void getSymbolicGraph(
} }
/* ************************************************************************* */ /* ************************************************************************* */
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, // Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) {
// Get the relative rotation measurement from the between factor // Get the relative rotation measurement from the between factor
@ -187,7 +196,8 @@ GaussianFactorGraph buildLinearOrientationGraph(
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph pose2Graph; NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) { BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
@ -210,7 +220,8 @@ NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) { static PredecessorMap<Key> findOdometricPath(
const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
Key minKey; Key minKey;
@ -236,7 +247,8 @@ PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, // Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) { bool useOdometricPath) {
// Find a minimum spanning tree // Find a minimum spanning tree

25
gtsam/nonlinear/lago.h
View File

@ -44,17 +44,6 @@ namespace lago {
typedef std::map<Key, double> key2doubleMap; typedef std::map<Key, double> key2doubleMap;
/**
* 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.
*/
GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey,
const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap);
/** /**
* Compute the cumulative orientations (without wrapping) * Compute the cumulative orientations (without wrapping)
* for all nodes wrt the root (root has zero orientation). * for all nodes wrt the root (root has zero orientation).
@ -75,26 +64,12 @@ GTSAM_EXPORT void getSymbolicGraph(
key2doubleMap& deltaThetaMap, key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g); /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/**
* Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
*/
GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
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( GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g, const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree); const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/** Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(
const NonlinearFactorGraph& graph);
/** Return the orientations of a graph including only BetweenFactors<Pose2> */
GTSAM_EXPORT VectorValues computeOrientations(
const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/** LAGO: Return 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( GTSAM_EXPORT VectorValues initializeOrientations(
const NonlinearFactorGraph& graph, bool useOdometricPath = true); const NonlinearFactorGraph& graph, bool useOdometricPath = true);