diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index f8b089539..c2a7d9454 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -37,8 +37,16 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ -double computeThetaToRoot(const Key nodeKey, const PredecessorMap& 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& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap) { double nodeTheta = 0; 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 +static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { // 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; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -210,7 +220,8 @@ NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { +static PredecessorMap findOdometricPath( + const NonlinearFactorGraph& pose2Graph) { PredecessorMap tree; Key minKey; @@ -236,7 +247,8 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { } /* ************************************************************************* */ -VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, +// Return the orientations of a graph including only BetweenFactors +static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { // Find a minimum spanning tree diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index fa8eef995..0df80ac42 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -44,17 +44,6 @@ namespace lago { typedef std::map 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& tree, const key2doubleMap& deltaThetaMap, - const key2doubleMap& thetaFromRootMap); - /** * Compute the cumulative orientations (without wrapping) * for all nodes wrt the root (root has zero orientation). @@ -75,26 +64,12 @@ GTSAM_EXPORT void getSymbolicGraph( key2doubleMap& deltaThetaMap, /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); -/** - * Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor - */ -GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, - Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); - /** Linear factor graph with regularized orientation measurements */ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( const std::vector& spanningTreeIds, const std::vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& 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 */ -GTSAM_EXPORT VectorValues computeOrientations( - const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); - /** LAGO: Return the orientations of the Pose2 in a generic factor graph */ GTSAM_EXPORT VectorValues initializeOrientations( const NonlinearFactorGraph& graph, bool useOdometricPath = true);