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

added 3D version of writeG2o with unit test

release/4.3a0
Luca 2014-08-18 21:14:48 -04:00
parent 358c978f00
commit 881514a45f
2 changed files with 71 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/slam/dataset.cpp
View File

@ -85,7 +85,6 @@ string createRewrittenFileName(const string& name) {
return newpath.string();
}
/* ************************************************************************* */
#endif
/* ************************************************************************* */
@ -379,20 +378,27 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
// save 2D & 3D poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
const Pose2& pose2D = dynamic_cast<const Pose2&>(key_value.value);
if(&pose2D){
stream << "VERTEX_SE2 " << key_value.key << " " << pose2D.x() << " "
<< pose2D.y() << " " << pose2D.theta() << endl;
const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value);
if(pose2D){
stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " "
<< pose2D->y() << " " << pose2D->theta() << endl;
}
const Pose3* pose3D = dynamic_cast<const Pose3*>(&key_value.value);
if(pose3D){
Point3 p = pose3D->translation();
Rot3 R = pose3D->rotation();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z()
<< " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
<< " " << R.toQuaternion().w() << endl;
}
}
// save edges
// save edges (2D or 3D)
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
continue;
if (factor){
SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
@ -407,6 +413,32 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< diagonalModel->precision(1) << " " << 0.0 << " "
<< diagonalModel->precision(2) << endl;
}
boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
if (factor3D){
SharedNoiseModel model = factor3D->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw invalid_argument(
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
Pose3 pose3D = factor3D->measured();
Point3 p = pose3D.translation();
Rot3 R = pose3D.rotation();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << " "
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(1) << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(2) << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(3) << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(4) << " " << 0.0 << " "
<< diagonalModel->precision(5) << endl;
}
}
stream.close();
}

19
gtsam/slam/tests/testDataset.cpp
View File

@ -252,6 +252,25 @@ TEST( dataSet, writeG2o)
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o3D)
{
const string g2oFile = findExampleDataFile("pose3example");
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
bool is3D = true;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
}
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{