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 ()

Public Attributes
VertexSE3 *	_offsetVertex
Vector3d	_nplane

Additional Inherited Members
Protected Attributes inherited from Sensor
Robot *	_robot

Detailed Description

Definition at line 194 of file simulator_3d_plane.cpp.

Constructor & Destructor Documentation

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

inline

Definition at line 195 of file simulator_3d_plane.cpp.

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

Member Function Documentation

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

inlinevirtual

Reimplemented from Sensor.

Definition at line 202 of file simulator_3d_plane.cpp.

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

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

inlinevirtual

Reimplemented from Sensor.

Definition at line 211 of file simulator_3d_plane.cpp.

References g2o::BaseVertex< D, T >::estimate(), g2o::Plane3D::oplus(), sample_noise_from_plane(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3PlaneSensorCalib::setMeasurement(), WorldItem::vertex(), and g2o::HyperGraph::Edge::vertices().

                                                               {
    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;
  }

Member Data Documentation

Vector3d PlaneSensor::_nplane

Definition at line 249 of file simulator_3d_plane.cpp.

Referenced by main().

VertexSE3* PlaneSensor::_offsetVertex

Definition at line 248 of file simulator_3d_plane.cpp.

Referenced by main().

---

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

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