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

Some cleanup

release/4.3a0
Ankur Roy Chowdhury 2023-02-04 19:37:25 -08:00
parent a0ca68a5b7
commit 4be4b97b21
2 changed files with 35 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
gtsam/slam/lago.cpp
View File

@ -227,90 +227,69 @@ static PredecessorMap<Key> findOdometricPath(
} }
/*****************************************************************************/ /*****************************************************************************/
PredecessorMap<Key> findMinimumSpanningTree(const NonlinearFactorGraph& pose2Graph){ PredecessorMap<Key> findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph) {
// Compute the minimum spanning tree // Compute the minimum spanning tree
const FastMap<Key, size_t> forwardOrdering = Ordering::Natural(pose2Graph).invert(); const FastMap<Key, size_t> forwardOrdering =
Ordering::Natural(pose2Graph).invert();
const auto edgeWeights = std::vector<double>(pose2Graph.size(), 1.0); const auto edgeWeights = std::vector<double>(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"; // Create a PredecessorMap 'predecessorMap' such that:
// for(const auto& i: mstEdgeIndices){ // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
// std::cout << i << ", "; // the spanning tree
// }
// std::cout << "\n";
// Create PredecessorMap
PredecessorMap<Key> predecessorMap; PredecessorMap<Key> predecessorMap;
std::map<Key, bool> visitationMap; std::map<Key, bool> visitationMap;
std::stack<std::pair<Key, Key>> stack; std::stack<std::pair<Key, Key>> stack;
// const auto rootKey = pose2Graph[mstEdgeIndices.front()]->front();
// stack.push({rootKey, rootKey});
stack.push({kAnchorKey, kAnchorKey}); stack.push({kAnchorKey, kAnchorKey});
while (!stack.empty()) { while (!stack.empty()) {
auto [u, parent] = stack.top(); auto [u, parent] = stack.top();
stack.pop(); stack.pop();
if (visitationMap[u]) continue; if (visitationMap[u]) continue;
visitationMap[u] = true; visitationMap[u] = true;
predecessorMap[u] = parent; predecessorMap[u] = parent;
for (const auto& edgeIdx: mstEdgeIndices) { for (const auto& edgeIdx : mstEdgeIndices) {
const auto v = pose2Graph[edgeIdx]->front(); const auto v = pose2Graph[edgeIdx]->front();
const auto w = pose2Graph[edgeIdx]->back(); const auto w = pose2Graph[edgeIdx]->back();
if((v == u || w == u) && !visitationMap[v == u ? w : v]) { if ((v == u || w == u) && !visitationMap[v == u ? w : v]) {
stack.push({v == u ? w : v, u}); stack.push({v == u ? w : v, u});
}
} }
} }
}
return predecessorMap; return predecessorMap;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose2> // Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) { bool useOdometricPath) {
gttic(lago_computeOrientations); gttic(lago_computeOrientations);
// Find a minimum spanning tree
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
// Find a minimum spanning tree
if (useOdometricPath) if (useOdometricPath)
tree = findOdometricPath(pose2Graph); tree = findOdometricPath(pose2Graph);
else else {
{
tree = findMinimumSpanningTree(pose2Graph); tree = findMinimumSpanningTree(pose2Graph);
// tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2>>(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 // Create a linear factor graph (LFG) of scalars
key2doubleMap deltaThetaMap; key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t>
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t>
chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); 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 // temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
// regularize measurements and plug everything in a factor graph // regularize measurements and plug everything in a factor graph
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(
chordsIds, pose2Graph, orientationsToRoot, tree); spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
// Solve the LFG // Solve the LFG
VectorValues orientationsLago = lagoGraph.optimize(); VectorValues orientationsLago = lagoGraph.optimize();
@ -320,21 +299,13 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues initializeOrientations(const NonlinearFactorGraph& graph, VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
bool useOdometricPath) { bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this // We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph // is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph); NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
auto initial = computeOrientations(pose2Graph, useOdometricPath); return 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */

3
gtsam/slam/lago.h
View File

@ -71,7 +71,8 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree); const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/** Given a "pose2" factor graph, find it's minimum spanning 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<Key> findMinimumSpanningTree( GTSAM_EXPORT PredecessorMap<Key> findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph); const NonlinearFactorGraph& pose2Graph);