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

fixed prior on anchor

release/4.3a0
Luca 2014-05-20 12:41:46 -04:00
parent c8e178b542
commit 1e7e386857
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -77,7 +77,7 @@ static const double PI = boost::math::constants::pi<double>();
* summing up the (directed) rotation measurements. The root is assumed to have orientation zero
*/
typedef map<Key,double> key2doubleMap;
const Key keyAnchor = symbol('A',0);
const Key keyAnchor = symbol('Z',9999999);
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, key2doubleMap& thetaFromRootMap) {
@ -220,7 +220,7 @@ GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTreeIds,
}
// prior on some orientation (anchor)
noiseModel::Diagonal::shared_ptr model_anchor = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
lagoGraph.add(JacobianFactor(tree.begin()->first, I, (Vector(1) << 0.0), model_anchor));
lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), model_anchor));
return lagoGraph;
}