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

moving function up

release/4.3a0
Luca 2014-05-16 15:28:07 -04:00
parent 11db29b1d8
commit 61aed0e5ad
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -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
*/