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

added comment

release/4.3a0
Luca 2014-05-16 16:18:42 -04:00
parent eca776c8c0
commit f9172ceb34
1 changed files with 1 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -168,6 +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;
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());