LineSensor Struct Reference

Inheritance diagram for LineSensor:

Collaboration diagram for LineSensor:

Public Member Functions
	LineSensor (Robot *r, int offsetId, const Isometry3d &offset_)
virtual bool	isVisible (const WorldItem *wi) const
virtual bool	sense (WorldItem *wi, const Isometry3d &position)
Public Member Functions inherited from Sensor
	Sensor (Robot *robot_)
Robot *	robot ()
virtual	~Sensor ()
	Sensor (Robot *robot_)
Robot *	robot ()
virtual	~Sensor ()

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3 *	_offsetVertex
Vector4d	_nline

Additional Inherited Members
Protected Attributes inherited from Sensor
Robot *	_robot

Detailed Description

Definition at line 183 of file simulator_3d_line.cpp.

Constructor & Destructor Documentation

LineSensor()

LineSensor::LineSensor ( Robot *  r, int  offsetId, const Isometry3d &  offset_  )

inline

Definition at line 186 of file simulator_3d_line.cpp.

                                                                : Sensor(r) {
    _offsetVertex = new VertexSE3();
    _offsetVertex->setId(offsetId);
    _offsetVertex->setEstimate(offset_);
    robot()->graph()->addVertex(_offsetVertex);
  };

References _offsetVertex, g2o::OptimizableGraph::addVertex(), g2o::BaseRobot::graph(), Sensor::robot(), g2o::BaseVertex< D, T >::setEstimate(), and g2o::OptimizableGraph::Vertex::setId().

Member Function Documentation

isVisible()

virtual bool LineSensor::isVisible ( const WorldItem *  wi ) const

inlinevirtual

Reimplemented from Sensor.

Definition at line 193 of file simulator_3d_line.cpp.

                                                    {
    if (!wi) {
      return false;
    }
    const LineItem* li = dynamic_cast<const LineItem*>(wi);
    if (!li) {
      return false;
    }
    return true;
  }

sense()

virtual bool LineSensor::sense ( WorldItem *  wi, const Isometry3d &  position  )

inlinevirtual

Reimplemented from Sensor.

Definition at line 204 of file simulator_3d_line.cpp.

                                                                {
    if (!wi) {
      return false;
    }
    LineItem* li = dynamic_cast<LineItem*>(wi);
    if (!li) {
      return false;
    }
    OptimizableGraph::Vertex* rv = robot()->vertex();
    if (!rv) {
      return false;
    }
    VertexSE3* robotVertex = dynamic_cast<VertexSE3*>(rv);
    if (!robotVertex) {
      return false;
    }
    const Isometry3d& robotPose = position;
    Isometry3d sensorPose = robotPose * _offsetVertex->estimate();
    VertexLine3D* lineVertex = dynamic_cast<VertexLine3D*>(li->vertex());
    Line3D worldLine = lineVertex->estimate();
 
    Line3D measuredLine = sensorPose.inverse() * worldLine;
 
    EdgeSE3Line3D* e = new EdgeSE3Line3D();
    e->vertices()[0] = robotVertex;
    e->vertices()[1] = lineVertex;
    Vector4d noise = sample_noise_from_line(_nline);
    measuredLine.oplus(noise);
    e->setMeasurement(measuredLine);
    Matrix4d m = Matrix4d::Zero();
    m(0, 0) = 1.0 / (_nline(0));
    m(1, 1) = 1.0 / (_nline(1));
    m(2, 2) = 1.0 / (_nline(2));
    m(3, 3) = 1.0 / (_nline(3));
    e->setInformation(m);
    e->setParameterId(0, 0);
    robot()->graph()->addEdge(e);
    return true;
  }

References _nline, _offsetVertex, g2o::OptimizableGraph::addEdge(), g2o::BaseVertex< D, T >::estimate(), g2o::BaseRobot::graph(), g2o::Line3D::oplus(), Sensor::robot(), sample_noise_from_line(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3Line3D::setMeasurement(), g2o::OptimizableGraph::Edge::setParameterId(), WorldItem::vertex(), and g2o::HyperGraph::Edge::vertices().

Member Data Documentation

_nline

Vector4d LineSensor::_nline

Definition at line 245 of file simulator_3d_line.cpp.

Referenced by main(), and sense().

_offsetVertex

VertexSE3* LineSensor::_offsetVertex

Definition at line 244 of file simulator_3d_line.cpp.

Referenced by LineSensor(), main(), and sense().

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

LineSensor::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 184 of file simulator_3d_line.cpp.

---

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

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