simulator_3d_plane.cpp File Reference

#include <fstream>
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/linear_solver.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/types/slam3d_addons/types_slam3d_addons.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/command_args.h"
#include <iostream>

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
double	uniform_rand (double lowerBndr, double upperBndr)
double	gauss_rand (double sigma)
Eigen::Isometry3d	sample_noise_from_se3 (const Vector6d &cov)
Vector3d	sample_noise_from_plane (const Vector3d &cov)
int	main (int argc, char **argv)

Typedef Documentation

typedef std::vector<Robot*> RobotVector

Definition at line 158 of file simulator_3d_plane.cpp.

typedef std::vector<Sensor*> SensorVector

Definition at line 92 of file simulator_3d_plane.cpp.

typedef std::set<WorldItem*> WorldItemSet

Definition at line 78 of file simulator_3d_plane.cpp.

Function Documentation

double gauss_rand

(

double

sigma

)

Definition at line 27 of file simulator_3d_plane.cpp.

References uniform_rand().

Referenced by sample_noise_from_plane(), sample_noise_from_se3(), g2o::tutorial::Simulator::sampleTransformation(), and g2o::tutorial::Simulator::simulate().

{
  double x, y, r2;
  do {
    x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
    y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
    r2 = x * x + y * y;
  } while (r2 > 1.0 || r2 == 0.0);
  return sigma * y * std::sqrt(-2.0 * log(r2) / r2);
}

int main	(	int	argc,
		char **	argv
	)

Definition at line 252 of file simulator_3d_plane.cpp.

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(), M_PI, 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(), g2o::SparseOptimizer, WorldItem::vertex(), g2o::OptimizableGraph::vertex(), and g2o::VertexSE3::write().

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

}

Vector3d sample_noise_from_plane

(

const Vector3d &

cov

)

Definition at line 57 of file simulator_3d_plane.cpp.

References gauss_rand().

Referenced by PlaneSensor::sense().

                                                      {
  return Vector3d(gauss_rand(cov(0)), gauss_rand(cov(1)), gauss_rand(cov(2)));
}

Eigen::Isometry3d sample_noise_from_se3

(

const Vector6d &

cov

)

Definition at line 38 of file simulator_3d_plane.cpp.

References gauss_rand().

Referenced by main(), and Robot::move().

                                                           {
  double nx=gauss_rand(cov(0));
  double ny=gauss_rand(cov(1));
  double nz=gauss_rand(cov(2));

  double nroll=gauss_rand(cov(3));
  double npitch=gauss_rand(cov(4));
  double nyaw=gauss_rand(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;
}

double uniform_rand	(	double	lowerBndr,
		double	upperBndr
	)

Definition at line 22 of file simulator_3d_plane.cpp.

Referenced by gauss_rand(), g2o::tutorial::Rand::gauss_rand(), and g2o::tutorial::Simulator::simulate().

{
  return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
}