Removes import 'graph.h' inside 'lago.h'
							parent
							
								
									4be4b97b21
								
							
						
					
					
						commit
						e29a0d35ed
					
				|  | @ -12,7 +12,6 @@ | |||
| /**
 | ||||
|  * @file SubgraphBuilder-inl.h | ||||
|  * @date Dec 31, 2009 | ||||
|  * @date Jan 23, 2023 | ||||
|  * @author Frank Dellaert, Yong-Dian Jian | ||||
|  */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -55,7 +55,7 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise = | |||
|  * The root is assumed to have orientation zero. | ||||
|  */ | ||||
| static double computeThetaToRoot(const Key nodeKey, | ||||
|     const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap, | ||||
|     const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, | ||||
|     const key2doubleMap& thetaFromRootMap) { | ||||
| 
 | ||||
|   double nodeTheta = 0; | ||||
|  | @ -81,7 +81,7 @@ static double computeThetaToRoot(const Key nodeKey, | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, | ||||
|     const PredecessorMap<Key>& tree) { | ||||
|     const PredecessorMap& tree) { | ||||
| 
 | ||||
|   key2doubleMap thetaToRootMap; | ||||
| 
 | ||||
|  | @ -102,7 +102,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, | |||
| void getSymbolicGraph( | ||||
| /*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds, | ||||
|     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
 | ||||
|   size_t id = 0; | ||||
|  | @ -166,7 +166,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, | |||
| GaussianFactorGraph buildLinearOrientationGraph( | ||||
|     const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds, | ||||
|     const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, | ||||
|     const PredecessorMap<Key>& tree) { | ||||
|     const PredecessorMap& tree) { | ||||
| 
 | ||||
|   GaussianFactorGraph lagoGraph; | ||||
|   Vector deltaTheta; | ||||
|  | @ -200,10 +200,10 @@ GaussianFactorGraph buildLinearOrientationGraph( | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| static PredecessorMap<Key> findOdometricPath( | ||||
| static PredecessorMap findOdometricPath( | ||||
|     const NonlinearFactorGraph& pose2Graph) { | ||||
| 
 | ||||
|   PredecessorMap<Key> tree; | ||||
|   PredecessorMap tree; | ||||
|   Key minKey = kAnchorKey; // this initialization does not matter
 | ||||
|   bool minUnassigned = true; | ||||
| 
 | ||||
|  | @ -216,18 +216,18 @@ static PredecessorMap<Key> findOdometricPath( | |||
|       minUnassigned = false; | ||||
|     } | ||||
|     if (key2 - key1 == 1) { // consecutive keys
 | ||||
|       tree.insert(key2, key1); | ||||
|       tree.emplace(key2, key1); | ||||
|       if (key1 < minKey) | ||||
|         minKey = key1; | ||||
|     } | ||||
|   } | ||||
|   tree.insert(minKey, kAnchorKey); | ||||
|   tree.insert(kAnchorKey, kAnchorKey); // root
 | ||||
|   tree.emplace(minKey, kAnchorKey); | ||||
|   tree.emplace(kAnchorKey, kAnchorKey); // root
 | ||||
|   return tree; | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| PredecessorMap<Key> findMinimumSpanningTree( | ||||
| PredecessorMap findMinimumSpanningTree( | ||||
|     const NonlinearFactorGraph& pose2Graph) { | ||||
|   // Compute the minimum spanning tree
 | ||||
|   const FastMap<Key, size_t> forwardOrdering = | ||||
|  | @ -239,7 +239,7 @@ PredecessorMap<Key> findMinimumSpanningTree( | |||
|   // Create a PredecessorMap 'predecessorMap' such that:
 | ||||
|   // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
 | ||||
|   // the spanning tree
 | ||||
|   PredecessorMap<Key> predecessorMap; | ||||
|   PredecessorMap predecessorMap; | ||||
|   std::map<Key, bool> visitationMap; | ||||
|   std::stack<std::pair<Key, Key>> stack; | ||||
| 
 | ||||
|  | @ -268,7 +268,7 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, | |||
|                                         bool useOdometricPath) { | ||||
|   gttic(lago_computeOrientations); | ||||
| 
 | ||||
|   PredecessorMap<Key> tree; | ||||
|   PredecessorMap tree; | ||||
|   // Find a minimum spanning tree
 | ||||
|   if (useOdometricPath) | ||||
|     tree = findOdometricPath(pose2Graph); | ||||
|  |  | |||
|  | @ -37,19 +37,19 @@ | |||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/inference/graph.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| namespace lago { | ||||
| 
 | ||||
| typedef std::map<Key, double> key2doubleMap; | ||||
| typedef std::map<Key, Key> PredecessorMap; | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute the cumulative orientations (without wrapping) | ||||
|  * for all nodes wrt the root (root has zero orientation). | ||||
|  */ | ||||
| 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 | ||||
|  | @ -62,19 +62,19 @@ GTSAM_EXPORT key2doubleMap computeThetasToRoot( | |||
| GTSAM_EXPORT void getSymbolicGraph( | ||||
| /*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, | ||||
|     key2doubleMap& deltaThetaMap, | ||||
|     /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g); | ||||
|     /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); | ||||
| 
 | ||||
| /** Linear factor graph with regularized orientation measurements */ | ||||
| GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( | ||||
|     const std::vector<size_t>& spanningTreeIds, | ||||
|     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.
 | ||||
|  * 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 findMinimumSpanningTree( | ||||
|     const NonlinearFactorGraph& pose2Graph); | ||||
| 
 | ||||
| /** LAGO: Return the orientations of the Pose2 in a generic factor graph */ | ||||
|  |  | |||
|  | @ -69,7 +69,7 @@ NonlinearFactorGraph graph() { | |||
| TEST(Lago, findMinimumSpanningTree) { | ||||
|   NonlinearFactorGraph g = simpleLago::graph(); | ||||
|   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:
 | ||||
|   //
 | ||||
|  | @ -94,7 +94,7 @@ TEST(Lago, findMinimumSpanningTree) { | |||
| TEST( Lago, checkSTandChords ) { | ||||
|   NonlinearFactorGraph g = simpleLago::graph(); | ||||
|   auto gPlus = initialize::buildPoseGraph<Pose2>(g); | ||||
|   PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); | ||||
|   lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); | ||||
| 
 | ||||
|   lago::key2doubleMap deltaThetaMap; | ||||
|   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
 | ||||
|  | @ -111,7 +111,7 @@ TEST( Lago, checkSTandChords ) { | |||
| TEST(Lago, orientationsOverSpanningTree) { | ||||
|   NonlinearFactorGraph g = simpleLago::graph(); | ||||
|   auto gPlus = initialize::buildPoseGraph<Pose2>(g); | ||||
|   PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); | ||||
|   lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); | ||||
| 
 | ||||
|   // check the tree structure
 | ||||
|   using initialize::kAnchorKey; | ||||
|  | @ -138,11 +138,6 @@ TEST(Lago, orientationsOverSpanningTree) { | |||
|   lago::key2doubleMap actual; | ||||
|   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[x1], actual[x1], 1e-6); | ||||
|   DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); | ||||
|  | @ -153,7 +148,7 @@ TEST(Lago, orientationsOverSpanningTree) { | |||
| TEST( Lago, regularizedMeasurements ) { | ||||
|   NonlinearFactorGraph g = simpleLago::graph(); | ||||
|   auto gPlus = initialize::buildPoseGraph<Pose2>(g); | ||||
|   PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus); | ||||
|   lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); | ||||
| 
 | ||||
|   lago::key2doubleMap deltaThetaMap; | ||||
|   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue