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

Removes import 'graph.h' inside 'lago.h'

release/4.3a0
Ankur Roy Chowdhury 2023-02-04 19:49:05 -08:00
parent 4be4b97b21
commit e29a0d35ed
4 changed files with 21 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/base/kruskal.h
View File

@ -12,7 +12,6 @@
/** /**
* @file SubgraphBuilder-inl.h * @file SubgraphBuilder-inl.h
* @date Dec 31, 2009 * @date Dec 31, 2009
* @date Jan 23, 2023
* @author Frank Dellaert, Yong-Dian Jian * @author Frank Dellaert, Yong-Dian Jian
*/ */

24
gtsam/slam/lago.cpp
View File

@ -55,7 +55,7 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
* The root is assumed to have orientation zero. * The root is assumed to have orientation zero.
*/ */
static double computeThetaToRoot(const Key nodeKey, static double computeThetaToRoot(const Key nodeKey,
const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap, const PredecessorMap& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap) { const key2doubleMap& thetaFromRootMap) {
double nodeTheta = 0; double nodeTheta = 0;
@ -81,7 +81,7 @@ static double computeThetaToRoot(const Key nodeKey,
/* ************************************************************************* */ /* ************************************************************************* */
key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree) { const PredecessorMap& tree) {
key2doubleMap thetaToRootMap; key2doubleMap thetaToRootMap;
@ -102,7 +102,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
void getSymbolicGraph( void getSymbolicGraph(
/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds, /*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap, key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) { /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) {
// Get keys for which you want the orientation // Get keys for which you want the orientation
size_t id = 0; size_t id = 0;
@ -166,7 +166,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
GaussianFactorGraph buildLinearOrientationGraph( GaussianFactorGraph buildLinearOrientationGraph(
const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds, const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
const PredecessorMap<Key>& tree) { const PredecessorMap& tree) {
GaussianFactorGraph lagoGraph; GaussianFactorGraph lagoGraph;
Vector deltaTheta; Vector deltaTheta;
@ -200,10 +200,10 @@ GaussianFactorGraph buildLinearOrientationGraph(
} }
/* ************************************************************************* */ /* ************************************************************************* */
static PredecessorMap<Key> findOdometricPath( static PredecessorMap findOdometricPath(
const NonlinearFactorGraph& pose2Graph) { const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree; PredecessorMap tree;
Key minKey = kAnchorKey; // this initialization does not matter Key minKey = kAnchorKey; // this initialization does not matter
bool minUnassigned = true; bool minUnassigned = true;
@ -216,18 +216,18 @@ static PredecessorMap<Key> findOdometricPath(
minUnassigned = false; minUnassigned = false;
} }
if (key2 - key1 == 1) { // consecutive keys if (key2 - key1 == 1) { // consecutive keys
tree.insert(key2, key1); tree.emplace(key2, key1);
if (key1 < minKey) if (key1 < minKey)
minKey = key1; minKey = key1;
} }
} }
tree.insert(minKey, kAnchorKey); tree.emplace(minKey, kAnchorKey);
tree.insert(kAnchorKey, kAnchorKey); // root tree.emplace(kAnchorKey, kAnchorKey); // root
return tree; return tree;
} }
/*****************************************************************************/ /*****************************************************************************/
PredecessorMap<Key> findMinimumSpanningTree( PredecessorMap findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph) { const NonlinearFactorGraph& pose2Graph) {
// Compute the minimum spanning tree // Compute the minimum spanning tree
const FastMap<Key, size_t> forwardOrdering = const FastMap<Key, size_t> forwardOrdering =
@ -239,7 +239,7 @@ PredecessorMap<Key> findMinimumSpanningTree(
// Create a PredecessorMap 'predecessorMap' such that: // Create a PredecessorMap 'predecessorMap' such that:
// predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
// the spanning tree // the spanning tree
PredecessorMap<Key> predecessorMap; PredecessorMap 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;
@ -268,7 +268,7 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) { bool useOdometricPath) {
gttic(lago_computeOrientations); gttic(lago_computeOrientations);
PredecessorMap<Key> tree; PredecessorMap tree;
// Find a minimum spanning tree // Find a minimum spanning tree
if (useOdometricPath) if (useOdometricPath)
tree = findOdometricPath(pose2Graph); tree = findOdometricPath(pose2Graph);

10
gtsam/slam/lago.h
View File

@ -37,19 +37,19 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
namespace gtsam { namespace gtsam {
namespace lago { namespace lago {
typedef std::map<Key, double> key2doubleMap; typedef std::map<Key, double> key2doubleMap;
typedef std::map<Key, Key> PredecessorMap;
/** /**
* 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).
*/ */
GTSAM_EXPORT key2doubleMap computeThetasToRoot( GTSAM_EXPORT key2doubleMap computeThetasToRoot(
const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree); const key2doubleMap& deltaThetaMap, const PredecessorMap& tree);
/** /**
* Given a factor graph "g", and a spanning tree "tree", select the nodes belonging * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
@ -62,19 +62,19 @@ GTSAM_EXPORT key2doubleMap computeThetasToRoot(
GTSAM_EXPORT void getSymbolicGraph( GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, /*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap, key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g); /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g);
/** 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& 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' Between factors are given equal weightage. * Note: all 'Pose2' Between factors are given equal weightage.
* Note: assumes all the edges (factors) are Between factors * Note: assumes all the edges (factors) are Between factors
*/ */
GTSAM_EXPORT PredecessorMap<Key> findMinimumSpanningTree( GTSAM_EXPORT PredecessorMap findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph); const NonlinearFactorGraph& pose2Graph);
/** LAGO: Return the orientations of the Pose2 in a generic factor graph */ /** LAGO: Return the orientations of the Pose2 in a generic factor graph */

13
gtsam/slam/tests/testLago.cpp
View File

@ -69,7 +69,7 @@ NonlinearFactorGraph graph() {
TEST(Lago, findMinimumSpanningTree) { TEST(Lago, findMinimumSpanningTree) {
NonlinearFactorGraph g = simpleLago::graph(); NonlinearFactorGraph g = simpleLago::graph();
auto gPlus = initialize::buildPoseGraph<Pose2>(g); auto gPlus = initialize::buildPoseGraph<Pose2>(g);
PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
// We should recover the following spanning tree: // We should recover the following spanning tree:
// //
@ -94,7 +94,7 @@ TEST(Lago, findMinimumSpanningTree) {
TEST( Lago, checkSTandChords ) { TEST( Lago, checkSTandChords ) {
NonlinearFactorGraph g = simpleLago::graph(); NonlinearFactorGraph g = simpleLago::graph();
auto gPlus = initialize::buildPoseGraph<Pose2>(g); auto gPlus = initialize::buildPoseGraph<Pose2>(g);
PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
lago::key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
@ -111,7 +111,7 @@ TEST( Lago, checkSTandChords ) {
TEST(Lago, orientationsOverSpanningTree) { TEST(Lago, orientationsOverSpanningTree) {
NonlinearFactorGraph g = simpleLago::graph(); NonlinearFactorGraph g = simpleLago::graph();
auto gPlus = initialize::buildPoseGraph<Pose2>(g); auto gPlus = initialize::buildPoseGraph<Pose2>(g);
PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
// check the tree structure // check the tree structure
using initialize::kAnchorKey; using initialize::kAnchorKey;
@ -138,11 +138,6 @@ TEST(Lago, orientationsOverSpanningTree) {
lago::key2doubleMap actual; lago::key2doubleMap actual;
actual = lago::computeThetasToRoot(deltaThetaMap, tree); actual = lago::computeThetasToRoot(deltaThetaMap, tree);
std::cout << "Thetas to root Map\n";
for (const auto& [k, v] : actual) {
std::cout << k << ": " << v << "\n";
}
DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@ -153,7 +148,7 @@ TEST(Lago, orientationsOverSpanningTree) {
TEST( Lago, regularizedMeasurements ) { TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simpleLago::graph(); NonlinearFactorGraph g = simpleLago::graph();
auto gPlus = initialize::buildPoseGraph<Pose2>(g); auto gPlus = initialize::buildPoseGraph<Pose2>(g);
PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
lago::key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T