g2o::SensorSegment2D Class Reference

#include <sensor_segment2d.h>

Inheritance diagram for g2o::SensorSegment2D:

Collaboration diagram for g2o::SensorSegment2D:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SensorSegment2D (const std::string &name_)
virtual void	sense ()
virtual void	addNoise (EdgeType *e)
Public Member Functions inherited from g2o::PointSensorParameters
	PointSensorParameters ()
double	maxRange () const
void	setMaxRange (double maxRange_)
double	minRange () const
void	setMinRange (double minRange_)
double	fov () const
void	setFov (double fov_)
double	maxAngularDifference () const
void	setMaxAngularDifference (double angularDifference)
Public Member Functions inherited from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >
	BinarySensor (const std::string &name)
void	setInformation (const InformationType &information_)
const InformationType &	information ()
Public Member Functions inherited from g2o::BaseSensor
	BaseSensor (const std::string &name_)
BaseRobot *	robot ()
void	setRobot (BaseRobot *robot_)
World *	world ()
OptimizableGraph *	graph ()
std::vector< Parameter * >	parameters ()
virtual void	addParameters ()

Protected Member Functions
bool	isVisible (WorldObjectType *to)
Protected Member Functions inherited from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >
EdgeType *	mkEdge (WorldObjectType *object)

Additional Inherited Members
Public Types inherited from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >
typedef Robot2D	RobotType
typedef RobotType::PoseObject	PoseObject
typedef RobotType::TrajectoryType	TrajectoryType
typedef RobotType::PoseObject::VertexType	PoseVertexType
typedef EdgeSE2Segment2D	EdgeType
typedef WorldObjectSegment2D	WorldObjectType
typedef WorldObjectType::VertexType	VertexType
typedef EdgeType::InformationType	InformationType
Public Attributes inherited from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Protected Attributes inherited from g2o::PointSensorParameters
double	_maxRange2
double	_minRange2
double	_fov
double	_maxAngularDifference
Protected Attributes inherited from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >
PoseObject *	_robotPoseObject
InformationType	_information
GaussianSampler< typename EdgeType::ErrorVector, InformationType >	_sampler
Protected Attributes inherited from g2o::BaseSensor
std::string	_name
std::vector< Parameter * >	_parameters
BaseRobot *	_robot

Detailed Description

Definition at line 37 of file sensor_segment2d.h.

Constructor & Destructor Documentation

g2o::SensorSegment2D::SensorSegment2D

(

const std::string &

name_

)

Definition at line 35 of file sensor_segment2d.cpp.

                                                        : BinarySensor<Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D>(name_) {
    cerr << "I am the constructor" << endl;
  }

Member Function Documentation

void g2o::SensorSegment2D::addNoise ( EdgeType *  e )

virtual

Reimplemented from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >.

Definition at line 39 of file sensor_segment2d.cpp.

References g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >::_sampler, g2o::GaussianSampler< SampleType, CovarianceType >::generateSample(), g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >::information(), g2o::BaseEdge< D, E >::measurement(), g2o::BaseEdge< D, E >::setInformation(), and g2o::BaseEdge< D, E >::setMeasurement().

Referenced by sense().

                                           {
    EdgeType::ErrorVector n=_sampler.generateSample();
    e->setMeasurement(e->measurement()+n);
    e->setInformation(information());
  }

bool g2o::SensorSegment2D::isVisible ( SensorSegment2D::WorldObjectType *  to )

protected

Definition at line 45 of file sensor_segment2d.cpp.

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxRange2, g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >::_robotPoseObject, g2o::clipSegmentCircle(), g2o::clipSegmentFov(), and g2o::WorldObject< VertexType_ >::vertex().

Referenced by sense().

                                                                   {
    if (! _robotPoseObject)
      return false;

    assert(to && to->vertex());
    VertexType* v=to->vertex();

    Vector2d p1, p2;
    SE2 iRobot=_robotPoseObject->vertex()->estimate().inverse();
    p1 = iRobot * v->estimateP1();
    p2 = iRobot * v->estimateP2();

    Vector3d vp1(p1.x(), p1.y(), 0.);
    Vector3d vp2(p2.x(), p2.y(), 0.);
    Vector3d cp=vp1.cross(vp2); // visibility check
    if (cp[2]<0)
      return false;

    int circleClip = clipSegmentCircle(p1,p2,sqrt(_maxRange2));
    bool clip1=false, clip2=false;
    switch(circleClip){
      case -1:
        return false;
      case  0:
        clip1 = true;
        break;
      case  1:
        clip2 = true;
        break;
      case  3:
        clip1 = true;
        clip2 = true;
        break;
      default:;
    }

    int fovClip=clipSegmentFov(p1,p2,-_fov, +_fov);
    switch(fovClip){
      case -1:
        return false;
      case  0:
        clip1 = true;
        break;
      case  1:
        clip2 = true;
        break;
      case  3:
        clip1 = true;
        clip2 = true;
        break;
      default:;
    }
    if (!clip1 && !clip2){  // only if both endpoints have not been clipped do something
      return true;
    }
    // if all these checks went well, we set the point
    return false;
  }

void g2o::SensorSegment2D::sense ( )

virtual

Reimplemented from g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >.

Definition at line 104 of file sensor_segment2d.cpp.

References g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >::_robotPoseObject, g2o::OptimizableGraph::addEdge(), addNoise(), g2o::BaseSensor::graph(), isVisible(), g2o::BinarySensor< Robot2D, EdgeSE2Segment2D, WorldObjectSegment2D >::mkEdge(), g2o::World::objects(), g2o::BaseSensor::robot(), g2o::EdgeSE2Segment2D::setMeasurementFromState(), g2o::Robot< RobotPoseObject >::trajectory(), and g2o::BaseSensor::world().

                              {
    _robotPoseObject=0;
    RobotType* r= dynamic_cast<RobotType*>(robot());
    std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
    int count = 0;
    while (it!=r->trajectory().rend() && count < 1){
      if (!_robotPoseObject)
        _robotPoseObject = *it;
      it++;
      count++;
    }
    for (std::set<BaseWorldObject*>::iterator it=world()->objects().begin(); it!=world()->objects().end(); it++){
      WorldObjectType* o=dynamic_cast<WorldObjectType*>(*it);
      if (o && isVisible(o)){
        EdgeType* e=mkEdge(o);
        if (e && graph()) {
          e->setMeasurementFromState();
          addNoise(e);
          graph()->addEdge(e);
        }
      }
    }
  }

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/apps/g2o_simulator/sensor_segment2d.h
• /home/xuezhisd/CLionProjects/g2o/g2o/apps/g2o_simulator/sensor_segment2d.cpp