#include <sensor_pose2d.h>

Inheritance diagram for g2o::SensorPose2D:

Collaboration diagram for g2o::SensorPose2D:

[legend]

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SensorPose2D (const std::string &name_)

virtual void	sense ()

virtual void	addNoise (EdgeType *e)

int	stepsToIgnore () const

void	setStepsToIgnore (int stepsToIgnore_)

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, EdgeSE2, WorldObjectSE2 >
	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, EdgeSE2, WorldObjectSE2 >
EdgeType *	mkEdge (WorldObjectType *object)

Protected Attributes
int	_stepsToIgnore

std::set< PoseObject * >	_posesToIgnore

Protected Attributes inherited from g2o::PointSensorParameters
double	_maxRange2

double	_minRange2

double	_fov

double	_maxAngularDifference

Protected Attributes inherited from g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >
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

Additional Inherited Members
Public Types inherited from g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >
typedef Robot2D	RobotType

typedef RobotType::PoseObject	PoseObject

typedef RobotType::TrajectoryType	TrajectoryType

typedef RobotType::PoseObject::VertexType	PoseVertexType

typedef EdgeSE2	EdgeType

typedef WorldObjectSE2	WorldObjectType

typedef WorldObjectType::VertexType	VertexType

typedef EdgeType::InformationType	InformationType

Public Attributes inherited from g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Detailed Description

Definition at line 36 of file sensor_pose2d.h.

Constructor & Destructor Documentation

g2o::SensorPose2D::SensorPose2D ( const std::string & name_ )

Definition at line 32 of file sensor_pose2d.cpp.

References _stepsToIgnore.

                                                   :
     BinarySensor<Robot2D, EdgeSE2, WorldObjectSE2>(name_)
   {
     _stepsToIgnore = 10;
   }

Member Function Documentation

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

virtual

Reimplemented from g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >.

Definition at line 64 of file sensor_pose2d.cpp.

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

Referenced by sense().

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

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

protected

Definition at line 38 of file sensor_pose2d.cpp.

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxAngularDifference, g2o::PointSensorParameters::_maxRange2, g2o::PointSensorParameters::_minRange2, _posesToIgnore, g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >::_robotPoseObject, and g2o::WorldObject< VertexType_ >::vertex().

Referenced by sense().

                                                              {
     if (! _robotPoseObject)
       return false;
     if (_posesToIgnore.find(to)!=_posesToIgnore.end())
       return false;
     
     assert(to && to->vertex());
     VertexType* v=to->vertex();
     VertexType::EstimateType pose=v->estimate();
     VertexType::EstimateType delta = _robotPoseObject->vertex()->estimate().inverse()*pose;
     Vector2d translation=delta.translation();
     double range2=translation.squaredNorm();
     if (range2>_maxRange2)
       return false;
     if (range2<_minRange2)
       return false;
     translation.normalize();
     double bearing=acos(translation.x());
     if (fabs(bearing)>_fov)
       return false;
     if (fabs(delta.rotation().angle())>_maxAngularDifference)
       return false;
     return true;
   }

void g2o::SensorPose2D::sense ( )

virtual

Reimplemented from g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >.

Definition at line 72 of file sensor_pose2d.cpp.

References _posesToIgnore, g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >::_robotPoseObject, _stepsToIgnore, g2o::OptimizableGraph::addEdge(), addNoise(), g2o::BaseSensor::graph(), isVisible(), g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >::mkEdge(), g2o::World::objects(), g2o::BaseSensor::robot(), g2o::EdgeSE2::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();
     _posesToIgnore.clear();
     int count = 0;
     while (it!=r->trajectory().rend() && count < _stepsToIgnore){
       if (!_robotPoseObject)
   _robotPoseObject = *it;
       _posesToIgnore.insert(*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);
         }
       }
     }
   }