116 lines
3.3 KiB
C++
116 lines
3.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file Pose2SLAMExample.cpp
|
|
* @brief A 2D Pose SLAM example that reads input from g2o and uses robust kernels in optimization
|
|
* @date May 15, 2014
|
|
* @author Luca Carlone
|
|
*/
|
|
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/inference/Key.h>
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/dataset.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
|
#include <gtsam/nonlinear/Marginals.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
#define LINESIZE 81920
|
|
|
|
int main(const int argc, const char *argv[]){
|
|
|
|
if (argc < 2)
|
|
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, model));
|
|
graph.add(factor);
|
|
}
|
|
is.ignore(LINESIZE, '\n');
|
|
}
|
|
// graph.print("graph");
|
|
|
|
// 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(graph, 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);
|
|
|
|
return 0;
|
|
}
|