#include <sensor_pose3d.h>

Inheritance diagram for g2o::SensorPose3D:

Collaboration diagram for g2o::SensorPose3D:

[legend]

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

virtual void	sense ()

int	stepsToIgnore () const

void	setStepsToIgnore (int stepsToIgnore_)

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< Robot3D, EdgeSE3, WorldObjectSE3 >
	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< Robot3D, EdgeSE3, WorldObjectSE3 >
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< Robot3D, EdgeSE3, WorldObjectSE3 >
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< Robot3D, EdgeSE3, WorldObjectSE3 >
typedef Robot3D	RobotType

typedef RobotType::PoseObject	PoseObject

typedef RobotType::TrajectoryType	TrajectoryType

typedef RobotType::PoseObject::VertexType	PoseVertexType

typedef EdgeSE3	EdgeType

typedef WorldObjectSE3	WorldObjectType

typedef WorldObjectType::VertexType	VertexType

typedef EdgeType::InformationType	InformationType

Public Attributes inherited from g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Detailed Description

Definition at line 35 of file sensor_pose3d.h.

Constructor & Destructor Documentation

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

Definition at line 35 of file sensor_pose3d.cpp.

References g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_information, _stepsToIgnore, and g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::setInformation().

                                                   : BinarySensor<Robot3D, EdgeSE3, WorldObjectSE3>(name_){  
     _stepsToIgnore = 10;
     _information.setIdentity();
     _information*=100;
     _information(3,3)=10000;
     _information(4,4)=10000;
     _information(5,5)=1000;
     setInformation(_information);
   }

Member Function Documentation

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

virtual

Reimplemented from g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >.

Definition at line 45 of file sensor_pose3d.cpp.

References g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_sampler, g2o::internal::fromVectorMQT(), g2o::GaussianSampler< SampleType, CovarianceType >::generateSample(), g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::information(), g2o::BaseEdge< D, E >::measurement(), g2o::BaseEdge< D, E >::setInformation(), and g2o::EdgeSE3::setMeasurement().

Referenced by sense().

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

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

protected

Definition at line 52 of file sensor_pose3d.cpp.

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxAngularDifference, g2o::PointSensorParameters::_maxRange2, g2o::PointSensorParameters::_minRange2, _posesToIgnore, g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_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;
     Vector3d 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;
     AngleAxisd a(delta.rotation());
     if (fabs(a.angle())>_maxAngularDifference)
       return false;
     return true;
   }

void g2o::SensorPose3D::sense ( )

virtual

Reimplemented from g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >.

Definition at line 79 of file sensor_pose3d.cpp.

References _posesToIgnore, g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_robotPoseObject, _stepsToIgnore, g2o::OptimizableGraph::addEdge(), addNoise(), g2o::BaseSensor::graph(), isVisible(), g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::mkEdge(), g2o::World::objects(), g2o::BaseSensor::robot(), g2o::EdgeSE3::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()) {
     graph()->addEdge(e);
     e->setMeasurementFromState();
           addNoise(e);
   }
       }
     }
   }