simulator_3d_line.cpp File Reference

#include <fstream>
#include <iostream>
#include "g2o/core/block_solver.h"
#include "g2o/core/linear_solver.h"
#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_line.cpp:

Go to the source code of this file.

Classes
struct	SimulatorItem
struct	WorldItem
struct	Sensor
struct	Robot
struct	Simulator
struct	LineItem
struct	LineSensor

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)
Vector4d	sample_noise_from_line (const Vector4d &cov)
int	main (int argc, char **argv)

Typedef Documentation

RobotVector

typedef std::vector<Robot*> RobotVector

Definition at line 146 of file simulator_3d_line.cpp.

SensorVector

typedef std::vector<Sensor*> SensorVector

Definition at line 79 of file simulator_3d_line.cpp.

WorldItemSet

typedef std::set<WorldItem*> WorldItemSet

Definition at line 64 of file simulator_3d_line.cpp.

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 248 of file simulator_3d_line.cpp.

                                {
  bool verbose, robustKernel, fixLines, planarMotion, listSolvers;
  int maxIterations;
  double lambdaInit;
  string strSolver;
  CommandArgs arg;
  arg.param("i", maxIterations, 10, "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("fixLines", fixLines, false,
            "fix the lines (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(std::cout);
    return 0;
  }
 
  if (!g->solver()) {
    std::cout << "Error allocating solver. Allocating \"" << strSolver
              << "\" failed!" << std::endl;
    std::cout << "Available solvers: " << std::endl;
    solverFactory->listSolvers(std::cout);
    std::cout << "--------------" << std::endl;
    return 0;
  }
 
  std::cout << "Creating simulator" << std::endl;
  Simulator* sim = new Simulator(g);
 
  std::cout << "Creating robot" << std::endl;
  Robot* r = new Robot(g);
 
  std::cout << "Creating line sensor" << std::endl;
  Isometry3d sensorPose = Isometry3d::Identity();
  LineSensor* ls = new LineSensor(r, 0, sensorPose);
  ls->_nline << 0.001, 0.001, 0.001, 0.0001;
  // ls->_nline << 1e-9, 1e-9, 1e-9, 1e-9;
  r->_sensors.push_back(ls);
  sim->_robots.push_back(r);
 
  Line3D line;
  std::cout << "Creating landmark line 1" << std::endl;
  LineItem* li = new LineItem(g, 1);
  Vector6 liv;
  liv << 0.0, 0.0, 5.0, 0.0, 1.0, 0.0;
  line = Line3D::fromCartesian(liv);
  static_cast<VertexLine3D*>(li->vertex())->setEstimate(line);
  li->vertex()->setFixed(fixLines);
  sim->_world.insert(li);
 
  std::cout << "Creating landmark line 2" << std::endl;
  liv << 5.0, 0.0, 0.0, 0.0, 0.0, 1.0;
  line = Line3D::fromCartesian(liv);
  li = new LineItem(g, 2);
  static_cast<VertexLine3D*>(li->vertex())->setEstimate(line);
  li->vertex()->setFixed(fixLines);
  sim->_world.insert(li);
 
  std::cout << "Creating landmark line 3" << std::endl;
  liv << 0.0, 5.0, 0.0, 1.0, 0.0, 0.0;
  line = Line3D::fromCartesian(liv);
  li = new LineItem(g, 3);
  static_cast<VertexLine3D*>(li->vertex())->setEstimate(line);
  li->vertex()->setFixed(fixLines);
  sim->_world.insert(li);
 
  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.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.0, 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) {
        std::cout << "m";
        if (a == 0) {
          sim->relativeMove(0, delta);
        } else {
          sim->relativeMove(0, iDelta);
        }
        std::cout << "s";
        sim->sense(0);
      }
    }
  }
  for (int j = 0; j < l; ++j) {
    Vector3d tr(1.0, 0.0, 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) {
        std::cout << "m";
        if (a == 0) {
          sim->relativeMove(0, delta);
        } else {
          sim->relativeMove(0, iDelta);
        }
        std::cout << "s";
        sim->sense(0);
      }
    }
  }
  std::cout << std::endl;
 
  ls->_offsetVertex->setFixed(true);
  OptimizableGraph::Vertex* gauge = g->vertex(4);
  if (gauge) {
    gauge->setFixed(true);
  }
 
  ofstream osp("line3d.g2o");
  g->save(osp);
  std::cout << "Saved graph on file line3d.g2o, use g2o_viewer to work with it."
            << std::endl;
 
  return 0;
}

References Simulator::_lastVertexId, LineSensor::_nline, LineSensor::_offsetVertex, Simulator::_robots, g2o::BaseRobot::_sensors, Simulator::_world, g2o::OptimizableGraph::addParameter(), g2o::OptimizationAlgorithmFactory::construct(), g2o::Line3D::fromCartesian(), g2o::OptimizationAlgorithmFactory::instance(), g2o::OptimizationAlgorithmFactory::listSolvers(), Simulator::move(), g2o::CommandArgs::param(), g2o::CommandArgs::parseArgs(), Simulator::relativeMove(), g2o::OptimizableGraph::save(), Simulator::sense(), g2o::SparseOptimizer::setAlgorithm(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::Parameter::setId(), g2o::SparseOptimizer::solver(), WorldItem::vertex(), and g2o::OptimizableGraph::vertex().

sample_noise_from_line()

Vector4d sample_noise_from_line

(

const Vector4d &

cov

)

Definition at line 39 of file simulator_3d_line.cpp.

                                                     {
  return Vector4d(
      Sampler::gaussRand(0., cov(0)), Sampler::gaussRand(0., cov(1)),
      Sampler::gaussRand(0., cov(2)), Sampler::gaussRand(0., cov(3)));
}

References g2o::Sampler::gaussRand().

Referenced by LineSensor::sense().

sample_noise_from_se3()

Eigen::Isometry3d sample_noise_from_se3

(

const Vector6 &

cov

)

Definition at line 20 of file simulator_3d_line.cpp.

                                                          {
  double nx = Sampler::gaussRand(0., cov(0));
  double ny = Sampler::gaussRand(0., cov(1));
  double nz = Sampler::gaussRand(0., cov(2));
 
  double nroll = Sampler::gaussRand(0., cov(3));
  double npitch = Sampler::gaussRand(0., cov(4));
  double nyaw = 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 Robot::move().