Robot Struct Reference

Inheritance diagram for Robot:

Collaboration diagram for Robot:

Public Member Functions
	Robot (OptimizableGraph *graph_)
void	move (const Isometry3d &newPosition, int &id)
void	relativeMove (const Isometry3d &delta, int &id)
void	sense (WorldItem *wi=0)
Public Member Functions inherited from WorldItem
	WorldItem (OptimizableGraph *graph_, OptimizableGraph::Vertex *vertex_=0)
OptimizableGraph::Vertex *	vertex ()
void	setVertex (OptimizableGraph::Vertex *vertex_)
Public Member Functions inherited from SimulatorItem
	SimulatorItem (OptimizableGraph *graph_)
OptimizableGraph *	graph ()
virtual	~SimulatorItem ()

Public Attributes
Isometry3d	_position
SensorVector	_sensors
Vector6d	_nmovecov
bool	_planarMotion

Additional Inherited Members
Protected Attributes inherited from WorldItem
OptimizableGraph::Vertex *	_vertex
Protected Attributes inherited from SimulatorItem
OptimizableGraph *	_graph

Detailed Description

Definition at line 94 of file simulator_3d_plane.cpp.

Constructor & Destructor Documentation

Robot::Robot ( OptimizableGraph *  graph_ )

inline

Definition at line 96 of file simulator_3d_plane.cpp.

                                 : WorldItem(graph_) {
    _planarMotion=false;
    _position = Isometry3d::Identity();
  }

Member Function Documentation

void Robot::move ( const Isometry3d &  newPosition, int &  id  )

inline

Definition at line 102 of file simulator_3d_plane.cpp.

References g2o::BaseVertex< D, T >::estimate(), graph, g2o::BaseEdge< D, E >::measurement(), sample_noise_from_se3(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setId(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3::setMeasurement(), g2o::EdgeSE3Prior::setMeasurement(), g2o::OptimizableGraph::Edge::setParameterId(), and g2o::HyperGraph::Edge::vertices().

                                                    {
    Isometry3d delta = _position.inverse()*newPosition;
    _position = newPosition;
    VertexSE3* v=new VertexSE3();
    v->setId(id);
    id++;
    graph()->addVertex(v);
    if (_planarMotion){
      // add a singleton constraint that locks the position of the robot on the plane
      EdgeSE3Prior* planeConstraint=new EdgeSE3Prior();
      Matrix6d pinfo = Matrix6d::Zero();
      pinfo(2,2)=1e9;
      planeConstraint->setInformation(pinfo);
      planeConstraint->setMeasurement(Isometry3d::Identity());
      planeConstraint->vertices()[0]=v;
      planeConstraint->setParameterId(0,0);
      graph()->addEdge(planeConstraint);
    }
    if (vertex()){
      VertexSE3* oldV=dynamic_cast<VertexSE3*>(vertex());
      EdgeSE3* e=new EdgeSE3();
      Isometry3d noise=sample_noise_from_se3(_nmovecov);
      e->setMeasurement(delta*noise);
      Matrix6d m=Matrix6d::Identity();
      for (int i=0; i<6; i++){
        m(i,i)=1./(_nmovecov(i));
      }
      e->setInformation(m);
      e->vertices()[0]=vertex();
      e->vertices()[1]=v;
      graph()->addEdge(e);
      v->setEstimate(oldV->estimate()*e->measurement());
    } else {
      v->setEstimate(_position);
    }
    setVertex(v);
  }

void Robot::relativeMove ( const Isometry3d &  delta, int &  id  )

inline

Definition at line 140 of file simulator_3d_plane.cpp.

                                                     {
    Isometry3d newPosition = _position*delta;
    move(newPosition, id);
  }

void Robot::sense ( WorldItem *  wi = 0 )

inline

Definition at line 145 of file simulator_3d_plane.cpp.

References Sensor::sense().

                             {
    for (size_t i=0; i<_sensors.size(); i++){
      Sensor* s=_sensors[i];
      s->sense(wi, _position);
    }
  }

Member Data Documentation

Vector6d Robot::_nmovecov

Definition at line 154 of file simulator_3d_plane.cpp.

bool Robot::_planarMotion

Definition at line 155 of file simulator_3d_plane.cpp.

Isometry3d Robot::_position

Definition at line 152 of file simulator_3d_plane.cpp.

SensorVector Robot::_sensors

Definition at line 153 of file simulator_3d_plane.cpp.

---

The documentation for this struct was generated from the following file:

• /home/xuezhisd/CLionProjects/g2o/g2o/examples/plane_slam/simulator_3d_plane.cpp