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

added functions to read/write g2o files

release/4.3a0
Luca 2014-05-20 16:53:37 -04:00
parent 0453310726
commit 2430b0bbf0
2 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/slam/dataset.cpp
View File

@ -539,7 +539,7 @@ bool readBundler(const string& filename, SfM_data &data)
/* ************************************************************************* */
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial,
const kernelFunctionType kernelFunction = QUADRATIC){
const kernelFunctionType kernelFunction){
ifstream is(g2oFile.c_str());
if (!is){
@ -613,7 +613,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
}
/* ************************************************************************* */
bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
fstream stream(filename.c_str(), fstream::out);
@ -633,16 +633,20 @@ bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const V
if (!factor)
continue;
// Matrix sqrtInfo = factor->get_Noise Model ().
// Matrix info = sqrtInfo.tra
SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!");
Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< info(0, 0) << " " << info(0, 1) << " " << info(1, 1) << " "
<< info(2, 2) << " " << info(0, 2) << " " << info(1, 2) << endl;
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl;
}
stream.close();
return true;
}
/* ************************************************************************* */

11
gtsam/slam/dataset.h
View File

@ -125,7 +125,16 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
* @return initial Values containing the initial guess (VERTEX_SE2)
*/
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
bool readG2o2D(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction);
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
/**
* @brief This function writes a g2o file from
* NonlinearFactorGraph and a Values structure
* @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_SE2)
*/
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a