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

got rid of useless vector keysInBinary

release/4.3a0
Luca 2014-05-20 11:27:05 -04:00
parent f5a664fb47
commit 558bce010d
1 changed files with 36 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -76,8 +76,10 @@ static const double PI = boost::math::constants::pi<double>();
* for a node (without wrapping). The function starts at the nodes and moves towards the root * for a node (without wrapping). The function starts at the nodes and moves towards the root
* summing up the (directed) rotation measurements. The root is assumed to have orientation zero * summing up the (directed) rotation measurements. The root is assumed to have orientation zero
*/ */
typedef map<Key,double> key2doubleMap;
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree, double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const map<Key, double>& deltaThetaMap, map<Key, double>& thetaFromRootMap) { const key2doubleMap& deltaThetaMap, key2doubleMap& thetaFromRootMap) {
double nodeTheta = 0; double nodeTheta = 0;
Key key_child = nodeKey; // the node Key key_child = nodeKey; // the node
@ -104,13 +106,16 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
* This function computes the cumulative orientation (without wrapping) * This function computes the cumulative orientation (without wrapping)
* for all node wrt the root (root has zero orientation) * for all node wrt the root (root has zero orientation)
*/ */
map<Key, double> computeThetasToRoot(const vector<Key>& keysInBinary, key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const map<Key, double>& deltaThetaMap, const PredecessorMap<Key>& tree) { const PredecessorMap<Key>& tree) {
map<Key, double> thetaToRootMap; key2doubleMap thetaToRootMap;
key2doubleMap::const_iterator it;
// for all nodes in the tree // for all nodes in the tree
BOOST_FOREACH(const Key& nodeKey, keysInBinary) { for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it )
{
// compute the orientation wrt root // compute the orientation wrt root
Key nodeKey = it->first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap); thetaToRootMap);
thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta)); thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
@ -124,8 +129,8 @@ map<Key, double> computeThetasToRoot(const vector<Key>& keysInBinary,
* Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: * Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree:
* for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1] * for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1]
*/ */
void getSymbolicSubgraph(vector<Key>& keysInBinary, void getSymbolicSubgraph(
/*OUTPUTS*/ vector<size_t>& spanningTree, vector<size_t>& chords, map<Key, double>& deltaThetaMap, /*OUTPUTS*/ vector<size_t>& spanningTree, vector<size_t>& chords, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){ /*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
// Get keys for which you want the orientation // Get keys for which you want the orientation
@ -140,12 +145,6 @@ void getSymbolicSubgraph(vector<Key>& keysInBinary,
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between = boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor); boost::shared_ptr< BetweenFactor<Pose2> > pose2Between = boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
if (!pose2Between) continue; if (!pose2Between) continue;
// store the keys: these are the orientations we are going to estimate
if(std::find(keysInBinary.begin(), keysInBinary.end(), key1)==keysInBinary.end()) // did not find key1, we add it
keysInBinary.push_back(key1);
if(std::find(keysInBinary.begin(), keysInBinary.end(), key2)==keysInBinary.end()) // did not find key2, we add it
keysInBinary.push_back(key2);
// get the orientation - measured().theta(); // get the orientation - measured().theta();
double deltaTheta = pose2Between->measured().theta(); double deltaTheta = pose2Between->measured().theta();
@ -193,7 +192,7 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
* Linear factor graph with regularized orientation measurements * Linear factor graph with regularized orientation measurements
*/ */
GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, const vector<size_t>& chords, GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, const vector<size_t>& chords,
const NonlinearFactorGraph& g, const map<Key, double>& orientationsToRoot, const PredecessorMap<Key>& tree){ const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){
GaussianFactorGraph lagoGraph; GaussianFactorGraph lagoGraph;
Vector deltaTheta; Vector deltaTheta;
@ -226,19 +225,22 @@ GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, co
/* ************************************************************************* */ /* ************************************************************************* */
// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2> // returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
VectorValues initializeLago(const NonlinearFactorGraph& graph, vector<Key>& keysInBinary) { VectorValues initializeLago(const NonlinearFactorGraph& graph) {
// Find a minimum spanning tree // Find a minimum spanning tree
//buildPose2graph
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(graph); BetweenFactor<Pose2> >(graph);
// Create a linear factor graph (LFG) of scalars // Create a linear factor graph (LFG) of scalars
map<Key, double> deltaThetaMap; key2doubleMap deltaThetaMap;
vector<size_t> spanningTree; // ids of between factors forming the spanning tree T vector<size_t> spanningTree; // ids of between factors forming the spanning tree T
vector<size_t> chords; // ids of between factors corresponding to chords wrt T vector<size_t> chords; // ids of between factors corresponding to chords wrt T
getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, graph); getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, graph);
// temporary structure to correct wraparounds along loops // temporary structure to correct wraparounds along loops
map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, 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 = buildOrientationGraph(spanningTree, chords, graph, orientationsToRoot, tree); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, graph, orientationsToRoot, tree);
@ -249,26 +251,19 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph, vector<Key>& keys
return estimateLago; return estimateLago;
} }
/* ************************************************************************* */
// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
VectorValues initializeLago(const NonlinearFactorGraph& graph) {
vector<Key> keysInBinary;
return initializeLago(graph, keysInBinary);
}
/* ************************************************************************* */ /* ************************************************************************* */
// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2> // returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) {
Values initialGuessLago; Values initialGuessLago;
// get the orientation estimates from LAGO // get the orientation estimates from LAGO
vector<Key> keysInBinary; VectorValues orientations = initializeLago(graph);
VectorValues orientations = initializeLago(graph, keysInBinary);
// plug the orientations in the initialGuess VectorValues::const_iterator it;
for(size_t i=0; i<keysInBinary.size(); i++){ // for all nodes in the tree
Key key = keysInBinary[i]; for(it = orientations.begin(); it != orientations.end(); ++it )
{
Key key = it->first;
Pose2 pose = initialGuess.at<Pose2>(key); Pose2 pose = initialGuess.at<Pose2>(key);
Vector orientation = orientations.at(key); Vector orientation = orientations.at(key);
Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
@ -312,11 +307,10 @@ TEST( Lago, checkSTandChords ) {
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
vector<Key> keysInBinary; key2doubleMap deltaThetaMap;
map<Key, double> deltaThetaMap;
vector<size_t> spanningTree; // ids of between factors forming the spanning tree T vector<size_t> spanningTree; // ids of between factors forming the spanning tree T
vector<size_t> chords; // ids of between factors corresponding to chords wrt T vector<size_t> chords; // ids of between factors corresponding to chords wrt T
getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, g);
DOUBLES_EQUAL(spanningTree[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) DOUBLES_EQUAL(spanningTree[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
DOUBLES_EQUAL(spanningTree[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) DOUBLES_EQUAL(spanningTree[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
@ -336,20 +330,19 @@ TEST( Lago, orientationsOverSpanningTree ) {
EXPECT_LONGS_EQUAL(tree[x2], x0); EXPECT_LONGS_EQUAL(tree[x2], x0);
EXPECT_LONGS_EQUAL(tree[x3], x0); EXPECT_LONGS_EQUAL(tree[x3], x0);
map<Key, double> expected; key2doubleMap expected;
expected[x0]= 0; expected[x0]= 0;
expected[x1]= PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x1]= PI/2; // edge x0->x1 (consistent with edge (x0,x1))
expected[x2]= -PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x2]= -PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
expected[x3]= -PI/2; // edge x0->x3 (consistent with edge (x0,x3)) expected[x3]= -PI/2; // edge x0->x3 (consistent with edge (x0,x3))
vector<Key> keysInBinary; key2doubleMap deltaThetaMap;
map<Key, double> deltaThetaMap;
vector<size_t> spanningTree; // ids of between factors forming the spanning tree T vector<size_t> spanningTree; // ids of between factors forming the spanning tree T
vector<size_t> chords; // ids of between factors corresponding to chords wrt T vector<size_t> chords; // ids of between factors corresponding to chords wrt T
getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, g);
map<Key, double> actual; key2doubleMap actual;
actual = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); actual = computeThetasToRoot(deltaThetaMap, tree);
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);
@ -362,13 +355,12 @@ TEST( Lago, regularizedMeasurements ) {
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
vector<Key> keysInBinary; key2doubleMap deltaThetaMap;
map<Key, double> deltaThetaMap;
vector<size_t> spanningTree; // ids of between factors forming the spanning tree T vector<size_t> spanningTree; // ids of between factors forming the spanning tree T
vector<size_t> chords; // ids of between factors corresponding to chords wrt T vector<size_t> chords; // ids of between factors corresponding to chords wrt T
getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, g);
map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot, tree); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot, tree);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian(); std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();