PlaneSensor Struct Reference

Inheritance diagram for PlaneSensor:

Collaboration diagram for PlaneSensor:

Public Member Functions
	PlaneSensor (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
Vector3d	_nplane

Additional Inherited Members
Protected Attributes inherited from Sensor
Robot *	_robot

Detailed Description

Definition at line 207 of file simulator_3d_plane.cpp.

Constructor & Destructor Documentation

PlaneSensor()

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

inline

Definition at line 210 of file simulator_3d_plane.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(), and Sensor::Sensor().

Member Function Documentation

isVisible()

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

inlinevirtual

Reimplemented from Sensor.

Definition at line 217 of file simulator_3d_plane.cpp.

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

sense()

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

inlinevirtual

Reimplemented from Sensor.

Definition at line 224 of file simulator_3d_plane.cpp.

                                                                {
    if (!wi) return false;
    PlaneItem* pi = dynamic_cast<PlaneItem*>(wi);
    if (!pi) 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();
    VertexPlane* planeVertex = dynamic_cast<VertexPlane*>(pi->vertex());
    Plane3D worldPlane = planeVertex->estimate();
 
    Plane3D measuredPlane = sensorPose.inverse() * worldPlane;
 
    EdgeSE3PlaneSensorCalib* e = new EdgeSE3PlaneSensorCalib();
    e->vertices()[0] = robotVertex;
    e->vertices()[1] = planeVertex;
    e->vertices()[2] = _offsetVertex;
    Vector3d noise = sample_noise_from_plane(_nplane);
    measuredPlane.oplus(noise);
    e->setMeasurement(measuredPlane);
    Matrix3d m = Matrix3d::Zero();
    m(0, 0) = 1. / (_nplane(0));
    m(1, 1) = 1. / (_nplane(1));
    m(2, 2) = 1. / (_nplane(2));
    e->setInformation(m);
    robot()->graph()->addEdge(e);
    return true;
  }

References _nplane, _offsetVertex, g2o::OptimizableGraph::addEdge(), g2o::BaseVertex< D, T >::estimate(), g2o::BaseRobot::graph(), g2o::Plane3D::oplus(), Sensor::robot(), sample_noise_from_plane(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3PlaneSensorCalib::setMeasurement(), WorldItem::vertex(), and g2o::HyperGraph::Edge::vertices().

Member Data Documentation

_nplane

Vector3d PlaneSensor::_nplane

Definition at line 260 of file simulator_3d_plane.cpp.

Referenced by main(), and sense().

_offsetVertex

VertexSE3* PlaneSensor::_offsetVertex

Definition at line 259 of file simulator_3d_plane.cpp.

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

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

PlaneSensor::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 208 of file simulator_3d_plane.cpp.

---

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

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