simulator_3d_plane.cpp File Reference

#include <fstream>
#include <iostream>
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/sampler.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/types/slam3d_addons/types_slam3d_addons.h"

Include dependency graph for simulator_3d_plane.cpp:

Go to the source code of this file.

Classes
struct	SimulatorItem
struct	WorldItem
struct	Sensor
struct	Robot
struct	Simulator
struct	PlaneItem
struct	PlaneSensor

Typedefs
typedef std::set< WorldItem * >	WorldItemSet
typedef std::vector< Sensor * >	SensorVector
typedef std::vector< Robot * >	RobotVector

Functions
Eigen::Isometry3d	sample_noise_from_se3 (const Vector6 &cov)
Vector3d	sample_noise_from_plane (const Vector3d &cov)
int	main (int argc, char **argv)

Typedef Documentation

RobotVector

typedef std::vector<Robot*> RobotVector

Definition at line 170 of file simulator_3d_plane.cpp.

SensorVector

typedef std::vector<Sensor*> SensorVector

Definition at line 103 of file simulator_3d_plane.cpp.

WorldItemSet

typedef std::set<WorldItem*> WorldItemSet

Definition at line 88 of file simulator_3d_plane.cpp.

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 263 of file simulator_3d_plane.cpp.

                                {
  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 {
    Vector6 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);
}

References Simulator::_lastVertexId, PlaneSensor::_nplane, PlaneSensor::_offsetVertex, Simulator::_robots, g2o::BaseRobot::_sensors, Simulator::_world, g2o::OptimizableGraph::addParameter(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseOptimizer::computeMarginals(), g2o::OptimizationAlgorithmFactory::construct(), g2o::BaseVertex< D, T >::estimate(), g2o::Plane3D::fromVector(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::SparseOptimizer::initializeOptimization(), g2o::OptimizationAlgorithmFactory::instance(), g2o::OptimizationAlgorithmFactory::listSolvers(), Simulator::move(), g2o::SparseOptimizer::optimize(), g2o::CommandArgs::param(), g2o::CommandArgs::parseArgs(), Simulator::relativeMove(), sample_noise_from_se3(), g2o::OptimizableGraph::save(), Simulator::sense(), g2o::SparseOptimizer::setAlgorithm(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::Parameter::setId(), g2o::SparseOptimizer::setVerbose(), g2o::SparseOptimizer::solver(), WorldItem::vertex(), g2o::OptimizableGraph::vertex(), and g2o::VertexSE3::write().

sample_noise_from_plane()

Vector3d sample_noise_from_plane

(

const Vector3d &

cov

)

Definition at line 63 of file simulator_3d_plane.cpp.

                                                      {
  return Vector3d(g2o::Sampler::gaussRand(0., cov(0)),
                  g2o::Sampler::gaussRand(0., cov(1)),
                  g2o::Sampler::gaussRand(0., cov(2)));
}

References g2o::Sampler::gaussRand().

Referenced by PlaneSensor::sense().

sample_noise_from_se3()

Eigen::Isometry3d sample_noise_from_se3

(

const Vector6 &

cov

)

Definition at line 44 of file simulator_3d_plane.cpp.

                                                          {
  double nx = g2o::Sampler::gaussRand(0., cov(0));
  double ny = g2o::Sampler::gaussRand(0., cov(1));
  double nz = g2o::Sampler::gaussRand(0., cov(2));
 
  double nroll = g2o::Sampler::gaussRand(0., cov(3));
  double npitch = g2o::Sampler::gaussRand(0., cov(4));
  double nyaw = g2o::Sampler::gaussRand(0., cov(5));
 
  AngleAxisd aa(AngleAxisd(nyaw, Vector3d::UnitZ()) *
                AngleAxisd(nroll, Vector3d::UnitX()) *
                AngleAxisd(npitch, Vector3d::UnitY()));
 
  Eigen::Isometry3d retval = Isometry3d::Identity();
  retval.matrix().block<3, 3>(0, 0) = aa.toRotationMatrix();
  retval.translation() = Vector3d(nx, ny, nz);
  return retval;
}

References g2o::Sampler::gaussRand().

Referenced by main().