From 61aed0e5adc578a7e7c4e8098c77d8598eff46cd Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Fri, 16 May 2014 15:28:07 -0400
Subject: [PATCH] moving function up

---
 examples/tests/testPlanarSLAMExample_lago.cpp | 28 +++++++++----------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/examples/tests/testPlanarSLAMExample_lago.cpp b/examples/tests/testPlanarSLAMExample_lago.cpp
index d717efe2c..bfe8aeda1 100644
--- a/examples/tests/testPlanarSLAMExample_lago.cpp
+++ b/examples/tests/testPlanarSLAMExample_lago.cpp
@@ -99,6 +99,20 @@ double computeThetaToRoot(const Key nodeKey, PredecessorMap<Key>& tree,
   return nodeTheta;
 }
 
+/*
+ *  This function computes the cumulative orientation (without wrapping)
+ *  from each node to the root (root has zero orientation)
+ */
+map<Key, double> computeThetasToRoot(vector<Key>& keysInBinary, map<Key, double>& deltaThetaMap, PredecessorMap<Key>& tree){
+
+  map<Key, double> thetaToRootMap;
+  BOOST_FOREACH(const Key& nodeKey, keysInBinary){
+    double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap);
+    thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
+  }
+  return thetaToRootMap;
+}
+
 void getSymbolicSubgraph(vector<Key>& keysInBinary, vector<size_t>& spanningTree,
     vector<size_t>& chords, map<Key, double>& deltaThetaMap, PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
   // Get keys for which you want the orientation
@@ -138,20 +152,6 @@ void getSymbolicSubgraph(vector<Key>& keysInBinary, vector<size_t>& spanningTree
   }
 }
 
-/*
- *  This function computes the cumulative orientation (without wrapping)
- *  from each node to the root (root has zero orientation)
- */
-map<Key, double> computeThetasToRoot(vector<Key>& keysInBinary, map<Key, double>& deltaThetaMap, PredecessorMap<Key>& tree){
-
-  map<Key, double> thetaToRootMap;
-  BOOST_FOREACH(const Key& nodeKey, keysInBinary){
-    double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap);
-    thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
-  }
-  return thetaToRootMap;
-}
-
 /*
  *  Linear factor graph with regularized orientation measurements
  */