diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 0732c2a32..90909a43f 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -227,90 +227,69 @@ static PredecessorMap findOdometricPath( } /*****************************************************************************/ -PredecessorMap findMinimumSpanningTree(const NonlinearFactorGraph& pose2Graph){ - +PredecessorMap findMinimumSpanningTree( + const NonlinearFactorGraph& pose2Graph) { // Compute the minimum spanning tree - const FastMap forwardOrdering = Ordering::Natural(pose2Graph).invert(); + const FastMap forwardOrdering = + Ordering::Natural(pose2Graph).invert(); const auto edgeWeights = std::vector(pose2Graph.size(), 1.0); - const auto mstEdgeIndices = utils::kruskal(pose2Graph, forwardOrdering, edgeWeights); + const auto mstEdgeIndices = + utils::kruskal(pose2Graph, forwardOrdering, edgeWeights); - // std::cout << "MST Edge Indices:\n"; - // for(const auto& i: mstEdgeIndices){ - // std::cout << i << ", "; - // } - // std::cout << "\n"; - - // Create PredecessorMap + // Create a PredecessorMap 'predecessorMap' such that: + // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in + // the spanning tree PredecessorMap predecessorMap; std::map visitationMap; std::stack> stack; - // const auto rootKey = pose2Graph[mstEdgeIndices.front()]->front(); - // stack.push({rootKey, rootKey}); stack.push({kAnchorKey, kAnchorKey}); while (!stack.empty()) { - auto [u, parent] = stack.top(); - stack.pop(); - if (visitationMap[u]) continue; - visitationMap[u] = true; - predecessorMap[u] = parent; - for (const auto& edgeIdx: mstEdgeIndices) { - const auto v = pose2Graph[edgeIdx]->front(); - const auto w = pose2Graph[edgeIdx]->back(); - if((v == u || w == u) && !visitationMap[v == u ? w : v]) { - stack.push({v == u ? w : v, u}); - } + auto [u, parent] = stack.top(); + stack.pop(); + if (visitationMap[u]) continue; + visitationMap[u] = true; + predecessorMap[u] = parent; + for (const auto& edgeIdx : mstEdgeIndices) { + const auto v = pose2Graph[edgeIdx]->front(); + const auto w = pose2Graph[edgeIdx]->back(); + if ((v == u || w == u) && !visitationMap[v == u ? w : v]) { + stack.push({v == u ? w : v, u}); } - } - + } + } + return predecessorMap; } /* ************************************************************************* */ // Return the orientations of a graph including only BetweenFactors static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, - bool useOdometricPath) { + bool useOdometricPath) { gttic(lago_computeOrientations); - // Find a minimum spanning tree PredecessorMap tree; + // Find a minimum spanning tree if (useOdometricPath) tree = findOdometricPath(pose2Graph); - else - { + else { tree = findMinimumSpanningTree(pose2Graph); - // tree = findMinimumSpanningTree>(pose2Graph); - - std::cout << "computeOrientations:: Spanning Tree: \n"; - for(const auto&[k, v]: tree){ - std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(v)) << " -> " << gtsam::DefaultKeyFormatter(gtsam::Symbol(k)) << "\n"; - } } // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; - vector spanningTreeIds; // ids of between factors forming the spanning tree T - vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + vector + spanningTreeIds; // ids of between factors forming the spanning tree T + vector + chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); - // std::cout << "Spanning Tree Edge Ids:\n"; - // for(const auto& i: spanningTreeIds){ - // std::cout << i << ", "; - // } - // std::cout << "\n"; - - // std::cout << "Chord Edge Ids:\n"; - // for(const auto& i: chordsIds){ - // std::cout << i << ", "; - // } - // std::cout << "\n"; - // temporary structure to correct wraparounds along loops key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, - chordsIds, pose2Graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph( + spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues orientationsLago = lagoGraph.optimize(); @@ -320,21 +299,13 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, /* ************************************************************************* */ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, - bool useOdometricPath) { - + bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph(graph); // Get orientations from relative orientation measurements - auto initial = computeOrientations(pose2Graph, useOdometricPath); - std::cout << "Lago::initializeOrientations: Values: \n"; - for(const auto& [key, val]: initial){ - std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << " -> " << val << "\n"; - } - std::cout << "\n"; - - return initial; + return computeOrientations(pose2Graph, useOdometricPath); } /* ************************************************************************* */ diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h index ca6a2c89b..39212ed1b 100644 --- a/gtsam/slam/lago.h +++ b/gtsam/slam/lago.h @@ -71,7 +71,8 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); /** Given a "pose2" factor graph, find it's minimum spanning tree. - * Note: all 'Pose2' factors are given equal weightage. + * Note: all 'Pose2' Between factors are given equal weightage. + * Note: assumes all the edges (factors) are Between factors */ GTSAM_EXPORT PredecessorMap findMinimumSpanningTree( const NonlinearFactorGraph& pose2Graph);