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

fixed anchor key: only remains to improve computation of noise model and test on some larger dataset

release/4.3a0
Luca 2014-05-16 16:35:24 -04:00
parent f9172ceb34
commit a14b88f607
1 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -168,7 +168,7 @@ void getSymbolicSubgraph(vector<Key>& keysInBinary,
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta){ Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta){
td::cout << "TODO: improve computation of noise model" << std::endl; std::cout << "TODO: improve computation of noise model" << std::endl;
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) throw std::invalid_argument("buildOrientationGraph: invalid between factor!"); if (!pose2Between) throw std::invalid_argument("buildOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta()); deltaTheta = (Vector(1) << pose2Between->measured().theta());
@ -186,24 +186,25 @@ 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, map<Key, double> orientationsToRoot){ const NonlinearFactorGraph& g, map<Key, double> orientationsToRoot, PredecessorMap<Key>& tree){
GaussianFactorGraph lagoGraph; GaussianFactorGraph lagoGraph;
Vector deltaTheta; Vector deltaTheta;
noiseModel::Diagonal::shared_ptr model_deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta;
Key key1, key2;
Matrix I = eye(1); Matrix I = eye(1);
// put original measurements in the spanning tree // put original measurements in the spanning tree
BOOST_FOREACH(const size_t& factorId, spanningTree){ BOOST_FOREACH(const size_t& factorId, spanningTree){
Key key1 = g[factorId]->keys()[0]; key1 = g[factorId]->keys()[0];
Key key2 = g[factorId]->keys()[1]; key2 = g[factorId]->keys()[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta));
} }
// put regularized measurements in the chords // put regularized measurements in the chords
BOOST_FOREACH(const size_t& factorId, chords){ BOOST_FOREACH(const size_t& factorId, chords){
Key key1 = g[factorId]->keys()[0]; key1 = g[factorId]->keys()[0];
Key key2 = g[factorId]->keys()[1]; key2 = g[factorId]->keys()[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
double key1_DeltaTheta_key2 = deltaTheta(0); double key1_DeltaTheta_key2 = deltaTheta(0);
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot[key1] - orientationsToRoot[key2]; // this coincides to summing up measurements along the cycle induced by the chord double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot[key1] - orientationsToRoot[key2]; // this coincides to summing up measurements along the cycle induced by the chord
@ -211,10 +212,17 @@ GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, co
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*PI); Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*PI);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));
} }
// prior on first orientation (anchor) // prior on first orientation (anchor), corresponding to the root of the tree
noiseModel::Diagonal::shared_ptr model_anchor = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); noiseModel::Diagonal::shared_ptr model_anchor = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
std::cout << "TODO: fix the right root orientation and key" << std::endl; // find the root
lagoGraph.add(JacobianFactor(x0, I, (Vector(1) << 0.0), model_anchor)); Key key_root = key1; // one random node
while(1){
// We check if we reached the root
if(tree[key_root]==key_root) // if we reached the root
break;
key_root = tree[key_root]; // we move upwards in the tree
}
lagoGraph.add(JacobianFactor(key_root, I, (Vector(1) << 0.0), model_anchor));
return lagoGraph; return lagoGraph;
} }
@ -235,7 +243,7 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph, vector<Key>& keys
map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, 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); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, graph, orientationsToRoot, tree);
// Solve the LFG // Solve the LFG
VectorValues estimateLago = lagoGraph.optimize(); VectorValues estimateLago = lagoGraph.optimize();
@ -364,7 +372,7 @@ TEST( Lago, regularizedMeasurements ) {
map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree);
GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot, tree);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian(); std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
// jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4));