tutorial_slam2d.cpp File Reference

#include <cmath>
#include <iostream>
#include "edge_se2.h"
#include "edge_se2_pointxy.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "simulator.h"
#include "types_tutorial_slam2d.h"
#include "vertex_point_xy.h"
#include "vertex_se2.h"

Include dependency graph for tutorial_slam2d.cpp:

Go to the source code of this file.

Functions
int	main ()

Function Documentation

main()

int main

(

)

Definition at line 47 of file tutorial_slam2d.cpp.

           {
  // TODO simulate different sensor offset
  // simulate a robot observing landmarks while travelling on a grid
  SE2 sensorOffsetTransf(0.2, 0.1, -0.1);
  int numNodes = 300;
  Simulator simulator;
  simulator.simulate(numNodes, sensorOffsetTransf);
 
  /*********************************************************************************
   * creating the optimization problem
   ********************************************************************************/
 
  typedef BlockSolver<BlockSolverTraits<-1, -1> > SlamBlockSolver;
  typedef LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
 
  // allocating the optimizer
  SparseOptimizer optimizer;
  auto linearSolver = std::make_unique<SlamLinearSolver>();
  linearSolver->setBlockOrdering(false);
  OptimizationAlgorithmGaussNewton* solver =
      new OptimizationAlgorithmGaussNewton(
          std::make_unique<SlamBlockSolver>(std::move(linearSolver)));
 
  optimizer.setAlgorithm(solver);
 
  // add the parameter representing the sensor offset
  ParameterSE2Offset* sensorOffset = new ParameterSE2Offset;
  sensorOffset->setOffset(sensorOffsetTransf);
  sensorOffset->setId(0);
  optimizer.addParameter(sensorOffset);
 
  // adding the odometry to the optimizer
  // first adding all the vertices
  cerr << "Optimization: Adding robot poses ... ";
  for (size_t i = 0; i < simulator.poses().size(); ++i) {
    const Simulator::GridPose& p = simulator.poses()[i];
    const SE2& t = p.simulatorPose;
    VertexSE2* robot = new VertexSE2;
    robot->setId(p.id);
    robot->setEstimate(t);
    optimizer.addVertex(robot);
  }
  cerr << "done." << endl;
 
  // second add the odometry constraints
  cerr << "Optimization: Adding odometry measurements ... ";
  for (size_t i = 0; i < simulator.odometry().size(); ++i) {
    const Simulator::GridEdge& simEdge = simulator.odometry()[i];
 
    EdgeSE2* odometry = new EdgeSE2;
    odometry->vertices()[0] = optimizer.vertex(simEdge.from);
    odometry->vertices()[1] = optimizer.vertex(simEdge.to);
    odometry->setMeasurement(simEdge.simulatorTransf);
    odometry->setInformation(simEdge.information);
    optimizer.addEdge(odometry);
  }
  cerr << "done." << endl;
 
  // add the landmark observations
  cerr << "Optimization: add landmark vertices ... ";
  for (size_t i = 0; i < simulator.landmarks().size(); ++i) {
    const Simulator::Landmark& l = simulator.landmarks()[i];
    VertexPointXY* landmark = new VertexPointXY;
    landmark->setId(l.id);
    landmark->setEstimate(l.simulatedPose);
    optimizer.addVertex(landmark);
  }
  cerr << "done." << endl;
 
  cerr << "Optimization: add landmark observations ... ";
  for (size_t i = 0; i < simulator.landmarkObservations().size(); ++i) {
    const Simulator::LandmarkEdge& simEdge =
        simulator.landmarkObservations()[i];
    EdgeSE2PointXY* landmarkObservation = new EdgeSE2PointXY;
    landmarkObservation->vertices()[0] = optimizer.vertex(simEdge.from);
    landmarkObservation->vertices()[1] = optimizer.vertex(simEdge.to);
    landmarkObservation->setMeasurement(simEdge.simulatorMeas);
    landmarkObservation->setInformation(simEdge.information);
    landmarkObservation->setParameterId(0, sensorOffset->id());
    optimizer.addEdge(landmarkObservation);
  }
  cerr << "done." << endl;
 
  /*********************************************************************************
   * optimization
   ********************************************************************************/
 
  // dump initial state to the disk
  optimizer.save("tutorial_before.g2o");
 
  // prepare and run the optimization
  // fix the first robot pose to account for gauge freedom
  VertexSE2* firstRobotPose = dynamic_cast<VertexSE2*>(optimizer.vertex(0));
  firstRobotPose->setFixed(true);
  optimizer.setVerbose(true);
 
  cerr << "Optimizing" << endl;
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  cerr << "done." << endl;
 
  optimizer.save("tutorial_after.g2o");
 
  // freeing the graph memory
  optimizer.clear();
 
  return 0;
}

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addParameter(), g2o::OptimizableGraph::addVertex(), g2o::SparseOptimizer::clear(), g2o::tutorial::Simulator::GridEdge::from, g2o::tutorial::Simulator::LandmarkEdge::from, g2o::Parameter::id(), g2o::tutorial::Simulator::Landmark::id, g2o::tutorial::Simulator::GridPose::id, g2o::tutorial::Simulator::GridEdge::information, g2o::tutorial::Simulator::LandmarkEdge::information, g2o::SparseOptimizer::initializeOptimization(), g2o::SparseOptimizer::optimize(), g2o::OptimizableGraph::save(), g2o::SparseOptimizer::setAlgorithm(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::OptimizableGraph::Vertex::setId(), g2o::Parameter::setId(), g2o::BaseEdge< D, E >::setInformation(), g2o::BaseEdge< D, E >::setMeasurement(), g2o::tutorial::EdgeSE2::setMeasurement(), g2o::tutorial::ParameterSE2Offset::setOffset(), g2o::OptimizableGraph::Edge::setParameterId(), g2o::SparseOptimizer::setVerbose(), g2o::tutorial::Simulator::Landmark::simulatedPose, g2o::tutorial::Simulator::LandmarkEdge::simulatorMeas, g2o::tutorial::Simulator::GridPose::simulatorPose, g2o::tutorial::Simulator::GridEdge::simulatorTransf, g2o::tutorial::Simulator::GridEdge::to, g2o::tutorial::Simulator::LandmarkEdge::to, g2o::OptimizableGraph::vertex(), and g2o::HyperGraph::Edge::vertices().