void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->setFixed(fixed);
}
}
bool G2oSlamInterface::fixNode(const std::vector<int>& nodes)
{
for (size_t i = 0; i < nodes.size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->vertex(nodes[i]);
if (v)
v->setFixed(true);
}
return true;
}
bool OptimizableGraph::load(istream& is, bool createEdges)
{
// scna for the paramers in the whole file
if (!_parameters.read(is,&_renamedTypesLookup))
return false;
#ifndef NDEBUG
cerr << "Loaded " << _parameters.size() << " parameters" << endl;
#endif
is.clear();
is.seekg(ios_base::beg);
set<string> warnedUnknownTypes;
stringstream currentLine;
string token;
Factory* factory = Factory::instance();
HyperGraph::GraphElemBitset elemBitset;
elemBitset[HyperGraph::HGET_PARAMETER] = 1;
elemBitset.flip();
Vertex* previousVertex = 0;
Data* previousData = 0;
while (1) {
int bytesRead = readLine(is, currentLine);
if (bytesRead == -1)
break;
currentLine >> token;
//cerr << "Token=" << token << endl;
if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
continue;
// handle commands encoded in the file
bool handledCommand = false;
if (token == "FIX") {
handledCommand = true;
int id;
while (currentLine >> id) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(vertex(id));
if (v) {
# ifndef NDEBUG
cerr << "Fixing vertex " << v->id() << endl;
# endif
v->setFixed(true);
} else {
cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl;
}
}
}
if (handledCommand)
continue;
// do the mapping to an internal type if it matches
if (_renamedTypesLookup.size() > 0) {
map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token);
if (foundIt != _renamedTypesLookup.end()) {
token = foundIt->second;
}
}
if (! factory->knowsTag(token)) {
if (warnedUnknownTypes.count(token) != 1) {
warnedUnknownTypes.insert(token);
cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl;
}
continue;
}
HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
if (dynamic_cast<Vertex*>(element)) { // it's a vertex type
//cerr << "it is a vertex" << endl;
previousData = 0;
Vertex* v = static_cast<Vertex*>(element);
int id;
currentLine >> id;
bool r = v->read(currentLine);
if (! r)
cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl;
v->setId(id);
if (!addVertex(v)) {
cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl;
delete v;
} else {
previousVertex = v;
}
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
allocateSolverForSclam(optimizer);
// loading
if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
cerr << "Error while loading gm2dl file" << endl;
}
DataQueue robotLaserQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge = optimizer.findGauge();
if (gaugeFreedom) {
if (! gauge) {
cerr << "# cannot find a vertex to fix in this thing" << endl;
return 2;
} else {
cerr << "# graph is fixed by node " << gauge->id() << endl;
gauge->setFixed(true);
}
} else {
cerr << "# graph is fixed by priors" << endl;
}
addOdometryCalibLinksDifferential(optimizer, robotLaserQueue);
// sanity check
HyperDijkstra d(&optimizer);
UniformCostFunction f;
d.shortestPaths(gauge, &f);
//cerr << PVAR(d.visited().size()) << endl;
if (d.visited().size()!=optimizer.vertices().size()) {
cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
cerr << "visited: " << d.visited().size() << endl;
cerr << "vertices: " << optimizer.vertices().size() << endl;
if (1)
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (d.visited().count(v) == 0) {
cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl;
v->setFixed(true);
}
}
}
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->fixed()) {
cerr << "\t fixed vertex " << it->first << endl;
}
}
VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
signal(SIGINT, sigquit_handler);
cerr << "Doing full estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
int i=optimizer.optimize(maxIterations);
if (maxIterations > 0 && !i){
cerr << "optimize failed, result might be invalid" << endl;
}
if (laserOffset) {
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
}
if (odomParamsVertex) {
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
}
cerr << "vertices: " << optimizer.vertices().size() << endl;
cerr << "edges: " << optimizer.edges().size() << endl;
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a seperate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
const Vector3d& odomCalib = odomParamsVertex->estimate();
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurment
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
if (outputfilename.size() > 0) {
Gm2dlIO::updateLaserData(optimizer);
cerr << "Writing " << outputfilename << " ... ";
bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
cerr << (writeStatus ? "done." : "failed") << endl;
}
return 0;
}
int main (int argc , char ** argv){
int maxIterations;
bool verbose;
bool robustKernel;
double lambdaInit;
CommandArgs arg;
bool fixSensor;
bool fixPlanes;
bool fixFirstPose;
bool fixTrajectory;
bool planarMotion;
bool listSolvers;
string strSolver;
cerr << "graph" << endl;
arg.param("i", maxIterations, 5, "perform n iterations");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("solver", strSolver, "lm_var", "select one specific solver");
arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
arg.param("robustKernel", robustKernel, false, "use robust error functions");
arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot");
arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory");
arg.param("fixFirstPose", fixFirstPose, false, "fix the first robot pose");
arg.param("fixPlanes", fixPlanes, false, "fix the planes (do localization only)");
arg.param("planarMotion", planarMotion, false, "robot moves on a plane");
arg.param("listSolvers", listSolvers, false, "list the solvers");
arg.parseArgs(argc, argv);
SparseOptimizer* g=new SparseOptimizer();
ParameterSE3Offset* odomOffset=new ParameterSE3Offset();
odomOffset->setId(0);
g->addParameter(odomOffset);
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
OptimizationAlgorithmProperty solverProperty;
OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty);
g->setAlgorithm(solver);
if (listSolvers){
solverFactory->listSolvers(cerr);
return 0;
}
if (! g->solver()){
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
cerr << "available solvers: " << endl;
solverFactory->listSolvers(cerr);
cerr << "--------------" << endl;
return 0;
}
cerr << "sim" << endl;
Simulator* sim = new Simulator(g);
cerr << "robot" << endl;
Robot* r=new Robot(g);
cerr << "planeSensor" << endl;
Matrix3d R=Matrix3d::Identity();
R <<
0, 0, 1,
-1, 0, 0,
0, -1, 0;
Isometry3d sensorPose=Isometry3d::Identity();
sensorPose.matrix().block<3,3>(0,0) = R;
sensorPose.translation()= Vector3d(.3 , 0.5 , 1.2);
PlaneSensor* ps = new PlaneSensor(r, 0, sensorPose);
ps->_nplane << 0.03, 0.03, 0.005;
r->_sensors.push_back(ps);
sim->_robots.push_back(r);
cerr << "p1" << endl;
Plane3D plane;
PlaneItem* pi =new PlaneItem(g,1);
plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
plane.fromVector(Eigen::Vector4d(1.,0.,0.,5.));
pi =new PlaneItem(g,2);
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
cerr << "p2" << endl;
pi =new PlaneItem(g,3);
plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
Quaterniond q, iq;
if (planarMotion) {
r->_planarMotion = true;
r->_nmovecov << 0.01, 0.0025, 1e-9, 0.001, 0.001, 0.025;
q = Quaterniond(AngleAxisd(0.2, Vector3d::UnitZ()).toRotationMatrix());
iq = Quaterniond(AngleAxisd(-0.2, Vector3d::UnitZ()).toRotationMatrix());
} else {
r->_planarMotion = false;
//r->_nmovecov << 0.1, 0.005, 1e-9, 0.05, 0.001, 0.001;
r->_nmovecov << 0.1, 0.005, 1e-9, 0.001, 0.001, 0.05;
q = Quaterniond((AngleAxisd(M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix());
iq = Quaterniond((AngleAxisd(-M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix());
}
Isometry3d delta=Isometry3d::Identity();
sim->_lastVertexId=4;
Isometry3d startPose=Isometry3d::Identity();
startPose.matrix().block<3,3>(0,0) = AngleAxisd(-0.75*M_PI, Vector3d::UnitZ()).toRotationMatrix();
sim->move(0,startPose);
int k =20;
int l = 2;
double delta_t = 0.2;
for (int j=0; j<l; j++) {
Vector3d tr(1.,0.,0.);
delta.matrix().block<3,3>(0,0) = q.toRotationMatrix();
if (j==(l-1)){
delta.matrix().block<3,3>(0,0) = Matrix3d::Identity();
}
delta.translation()=tr*(delta_t*j);
Isometry3d iDelta = delta.inverse();
for (int a=0; a<2; a++){
for (int i=0; i<k; i++){
cerr << "m";
if (a==0)
sim->relativeMove(0,delta);
else
sim->relativeMove(0,iDelta);
cerr << "s";
sim->sense(0);
}
}
}
for (int j=0; j<l; j++) {
Vector3d tr(1.,0.,0.);
delta.matrix().block<3,3>(0,0) = iq.toRotationMatrix();
if (j==l-1){
delta.matrix().block<3,3>(0,0) = Matrix3d::Identity();
}
delta.translation()=tr*(delta_t*j);
Isometry3d iDelta = delta.inverse();
for (int a=0; a<2; a++){
for (int i=0; i<k; i++){
cerr << "m";
if (a==0)
sim->relativeMove(0,delta);
else
sim->relativeMove(0,iDelta);
cerr << "s";
sim->sense(0);
}
}
}
ofstream os("test_gt.g2o");
g->save(os);
if (fixSensor) {
ps->_offsetVertex->setFixed(true);
} else {
Vector6d noffcov;
noffcov << 0.1,0.1,0.1,0.5, 0.5, 0.5;
ps->_offsetVertex->setEstimate(ps->_offsetVertex->estimate() * sample_noise_from_se3(noffcov));
ps->_offsetVertex->setFixed(false);
}
if (fixFirstPose){
OptimizableGraph::Vertex* gauge = g->vertex(4);
if (gauge)
gauge->setFixed(true);
} // else {
// // multiply all vertices of the robot by this standard quantity
// Quaterniond q(AngleAxisd(1, Vector3d::UnitZ()).toRotationMatrix());
// Vector3d tr(1,0,0);
// Isometry3d delta;
// delta.matrix().block<3,3>(0,0)=q.toRotationMatrix();
// delta.translation()=tr;
// for (size_t i=0; i< g->vertices().size(); i++){
// VertexSE3 *v = dynamic_cast<VertexSE3 *>(g->vertex(i));
// if (v && v->id()>0){
// v->setEstimate (v->estimate()*delta);
// }
// }
// }
ofstream osp("test_preopt.g2o");
g->save(osp);
//g->setMethod(SparseOptimizer::LevenbergMarquardt);
g->initializeOptimization();
g->setVerbose(verbose);
g->optimize(maxIterations);
if (! fixSensor ){
SparseBlockMatrix<MatrixXd> spinv;
std::pair<int, int> indexParams;
indexParams.first = ps->_offsetVertex->hessianIndex();
indexParams.second = ps->_offsetVertex->hessianIndex();
std::vector<std::pair <int, int> > blockIndices;
blockIndices.push_back(indexParams);
if (!g->computeMarginals(spinv, blockIndices)){
cerr << "error in computing the covariance" << endl;
} else {
MatrixXd m = *spinv.block(ps->_offsetVertex->hessianIndex(), ps->_offsetVertex->hessianIndex());
cerr << "Param covariance" << endl;
cerr << m << endl;
cerr << "OffsetVertex: " << endl;
ps->_offsetVertex->write(cerr);
cerr << endl;
cerr << "rotationDeterminant: " << m.block<3,3>(0,0).determinant() << endl;
cerr << "translationDeterminant: " << m.block<3,3>(3,3).determinant() << endl;
cerr << endl;
}
}
ofstream os1("test_postOpt.g2o");
g->save(os1);
}