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)
	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_)
	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 ()
	SimulatorItem (OptimizableGraph *graph_)
OptimizableGraph *	graph ()
virtual	~SimulatorItem ()

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Isometry3d	_position
SensorVector	_sensors
Vector6	_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 81 of file simulator_3d_line.cpp.

Constructor & Destructor Documentation

Robot() [1/2]

Robot::Robot ( OptimizableGraph *  graph_ )

inline

Definition at line 84 of file simulator_3d_line.cpp.

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

References _planarMotion, and _position.

Robot() [2/2]

Robot::Robot ( OptimizableGraph *  graph_ )

inline

Definition at line 108 of file simulator_3d_plane.cpp.

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

References _planarMotion, and _position.

Member Function Documentation

move() [1/2]

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

inline

Definition at line 89 of file simulator_3d_line.cpp.

                                                    {
    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();
      Matrix6 pinfo = Matrix6::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);
      Matrix6 m = Matrix6::Identity();
      for (int i = 0; i < 6; ++i) {
        m(i, i) = 1.0 / (_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);
  }

References _nmovecov, _planarMotion, _position, g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::BaseVertex< D, T >::estimate(), g2o::BaseRobot::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(), WorldItem::setVertex(), WorldItem::vertex(), and g2o::HyperGraph::Edge::vertices().

move() [2/2]

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

inline

Definition at line 113 of file simulator_3d_plane.cpp.

                                                    {
    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();
      Matrix6 pinfo = Matrix6::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);
      Matrix6 m = Matrix6::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);
  }

References _nmovecov, _planarMotion, _position, g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::BaseVertex< D, T >::estimate(), g2o::BaseRobot::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(), WorldItem::setVertex(), WorldItem::vertex(), and g2o::HyperGraph::Edge::vertices().

relativeMove() [1/2]

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

inline

Definition at line 128 of file simulator_3d_line.cpp.

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

References _position, and g2o::Robot< RobotPoseObject >::move().

relativeMove() [2/2]

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

inline

Definition at line 152 of file simulator_3d_plane.cpp.

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

References _position, and g2o::Robot< RobotPoseObject >::move().

sense() [1/2]

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

inline

Definition at line 133 of file simulator_3d_line.cpp.

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

References _position, g2o::BaseRobot::_sensors, and Sensor::sense().

sense() [2/2]

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

inline

Definition at line 157 of file simulator_3d_plane.cpp.

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

References _position, g2o::BaseRobot::_sensors, and Sensor::sense().

Member Data Documentation

_nmovecov

Vector6 Robot::_nmovecov

Definition at line 142 of file simulator_3d_line.cpp.

Referenced by move().

_planarMotion

bool Robot::_planarMotion

Definition at line 143 of file simulator_3d_line.cpp.

Referenced by move(), and Robot().

_position

Isometry3d Robot::_position

Definition at line 140 of file simulator_3d_line.cpp.

Referenced by move(), relativeMove(), Robot(), and sense().

_sensors

SensorVector Robot::_sensors

Definition at line 141 of file simulator_3d_line.cpp.

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Robot::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 82 of file simulator_3d_line.cpp.

---

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

• /build/reproducible-path/g2o-0~20230806/g2o/examples/line_slam/simulator_3d_line.cpp
• /build/reproducible-path/g2o-0~20230806/g2o/examples/plane_slam/simulator_3d_plane.cpp