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

adding functions to read/write g2o files

release/4.3a0
Luca 2014-05-20 16:35:45 -04:00
parent c167430389
commit 0453310726
3 changed files with 122 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
examples/Pose2SLAMExampleRobust_g2o.cpp
View File

@ -39,89 +39,28 @@ int main(const int argc, const char *argv[]){
std::cout << "Please specify input file (in g2o format) and output file" << std::endl;
const string g2oFile = argv[1];
ifstream is(g2oFile.c_str());
if (!is)
throw std::invalid_argument("File not found!");
std::cout << "Reading g2o file: " << g2oFile << std::endl;
// READ INITIAL GUESS FROM G2O FILE
Values initial;
string tag;
while (is) {
if(! (is >> tag))
break;
if (tag == "VERTEX_SE2") {
int id;
double x, y, yaw;
is >> id >> x >> y >> yaw;
initial.insert(id, Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
// initial.print("initial guess");
// READ MEASUREMENTS FROM G2O FILE
NonlinearFactorGraph graph;
while (is) {
if(! (is >> tag))
break;
if (tag == "EDGE_SE2") {
int id1, id2;
double x, y, yaw;
double I11, I12, I13, I22, I23, I33;
is >> id1 >> id2 >> x >> y >> yaw;
is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
// Try to guess covariance matrix layout
Matrix m(3,3);
m << I11, I12, I13, I12, I22, I23, I13, I23, I33;
Pose2 l1Xl2(x, y, yaw);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33));
// NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
// noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
graph.add(factor);
}
is.ignore(LINESIZE, '\n');
}
//std::cout << "Robust kernel: Huber" << std::endl;
std::cout << "Robust kernel: Tukey" << std::endl;
Values initial;
readG2o(g2oFile, graph, initial);
// otherwise GTSAM cannot solve the problem
NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
// GaussNewtonParams parameters;
// Stop iterating once the change in error between steps is less than this value
// parameters.relativeErrorTol = 1e-5;
// Do not perform more than N iteration steps
// parameters.maxIterations = 100;
// Create the optimizer ...
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
// ... and optimize
Values result = optimizer.optimize();
// result.print("results");
std::cout << "Optimization complete" << std::endl;
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0, 0.0, 0.0));
save2D(graph, result, model, outputFile);
writeG2o(graph, result, model, outputFile);
std::cout << "done! " << std::endl;
return 0;
}

108
gtsam/slam/dataset.cpp
View File

@ -537,6 +537,114 @@ bool readBundler(const string& filename, SfM_data &data)
return true;
}
/* ************************************************************************* */
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial,
const kernelFunctionType kernelFunction = QUADRATIC){
ifstream is(g2oFile.c_str());
if (!is){
throw std::invalid_argument("File not found!");
return false;
}
std::cout << "Reading g2o file: " << g2oFile << std::endl;
// READ INITIAL GUESS FROM G2O FILE
string tag;
while (is) {
if(! (is >> tag))
break;
if (tag == "VERTEX_SE2") {
int id;
double x, y, yaw;
is >> id >> x >> y >> yaw;
initial.insert(id, Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
// initial.print("initial guess");
// READ MEASUREMENTS FROM G2O FILE
while (is) {
if(! (is >> tag))
break;
if (tag == "EDGE_SE2") {
int id1, id2;
double x, y, yaw;
double I11, I12, I13, I22, I23, I33;
is >> id1 >> id2 >> x >> y >> yaw;
is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
Pose2 l1Xl2(x, y, yaw);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33));
switch (kernelFunction) {
{case QUADRATIC:
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph.add(factor);
break;}
{case HUBER:
NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
graph.add(huberFactor);
break;}
{case TUKEY:
NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
graph.add(tukeyFactor);
break;}
}
}
is.ignore(LINESIZE, '\n');
}
// Output which kernel is used
switch (kernelFunction) {
case QUADRATIC:
std::cout << "Robust kernel: None" << std::endl; break;
case HUBER:
std::cout << "Robust kernel: Huber" << std::endl; break;
case TUKEY:
std::cout << "Robust kernel: Tukey" << std::endl; break;
}
return true;
}
/* ************************************************************************* */
bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
fstream stream(filename.c_str(), fstream::out);
// save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate)
{
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl;
}
// save edges
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
{
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
continue;
// Matrix sqrtInfo = factor->get_Noise Model ().
// Matrix info = sqrtInfo.tra
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;
}
stream.close();
}
/* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data)
{

10
gtsam/slam/dataset.h
View File

@ -117,6 +117,16 @@ struct SfM_data
*/
GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
/**
* @brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure
* @param filename The name of the g2o file
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @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);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfM_data structure