g2o
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
g2o::SensorPose2D Class Reference

#include <sensor_pose2d.h>

Inheritance diagram for g2o::SensorPose2D:
Inheritance graph
[legend]
Collaboration diagram for g2o::SensorPose2D:
Collaboration graph
[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 InformationTypeinformation ()
 
- Public Member Functions inherited from g2o::BaseSensor
 BaseSensor (const std::string &name_)
 
BaseRobotrobot ()
 
void setRobot (BaseRobot *robot_)
 
Worldworld ()
 
OptimizableGraphgraph ()
 
std::vector< Parameter * > parameters ()
 
virtual void addParameters ()
 

Protected Member Functions

bool isVisible (WorldObjectType *to)
 
- Protected Member Functions inherited from g2o::BinarySensor< Robot2D, EdgeSE2, WorldObjectSE2 >
EdgeTypemkEdge (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.

32  :
33  BinarySensor<Robot2D, EdgeSE2, WorldObjectSE2>(name_)
34  {
35  _stepsToIgnore = 10;
36  }

Member Function Documentation

void g2o::SensorPose2D::addNoise ( EdgeType e)
virtual
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().

38  {
39  if (! _robotPoseObject)
40  return false;
41  if (_posesToIgnore.find(to)!=_posesToIgnore.end())
42  return false;
43 
44  assert(to && to->vertex());
45  VertexType* v=to->vertex();
46  VertexType::EstimateType pose=v->estimate();
47  VertexType::EstimateType delta = _robotPoseObject->vertex()->estimate().inverse()*pose;
48  Vector2d translation=delta.translation();
49  double range2=translation.squaredNorm();
50  if (range2>_maxRange2)
51  return false;
52  if (range2<_minRange2)
53  return false;
54  translation.normalize();
55  double bearing=acos(translation.x());
56  if (fabs(bearing)>_fov)
57  return false;
58  if (fabs(delta.rotation().angle())>_maxAngularDifference)
59  return false;
60  return true;
61  }
std::set< PoseObject * > _posesToIgnore
Definition: sensor_pose2d.h:49
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().

72  {
74  RobotType* r= dynamic_cast<RobotType*>(robot());
75  std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
76  _posesToIgnore.clear();
77  int count = 0;
78  while (it!=r->trajectory().rend() && count < _stepsToIgnore){
79  if (!_robotPoseObject)
80  _robotPoseObject = *it;
81  _posesToIgnore.insert(*it);
82  it++;
83  count++;
84  }
85  for (std::set<BaseWorldObject*>::iterator it=world()->objects().begin();
86  it!=world()->objects().end(); it++){
87  WorldObjectType* o=dynamic_cast<WorldObjectType*>(*it);
88  if (o && isVisible(o)){
89  EdgeType* e=mkEdge(o);
90  if (e && graph()) {
91  e->setMeasurementFromState();
92  addNoise(e);
93  graph()->addEdge(e);
94  }
95  }
96  }
97  }
bool isVisible(WorldObjectType *to)
World * world()
Definition: simulator.cpp:69
TrajectoryType & trajectory()
Definition: simulator.h:119
std::set< PoseObject * > _posesToIgnore
Definition: sensor_pose2d.h:49
OptimizableGraph * graph()
Definition: simulator.cpp:75
BaseRobot * robot()
Definition: simulator.h:129
std::set< BaseWorldObject * > & objects()
Definition: simulator.h:284
virtual bool addEdge(HyperGraph::Edge *e)
virtual void addNoise(EdgeType *e)
EdgeType * mkEdge(WorldObjectType *object)
Definition: simulator.h:263
void g2o::SensorPose2D::setStepsToIgnore ( int  stepsToIgnore_)
inline

Definition at line 44 of file sensor_pose2d.h.

44 {_stepsToIgnore = stepsToIgnore_;}
int g2o::SensorPose2D::stepsToIgnore ( ) const
inline

Definition at line 43 of file sensor_pose2d.h.

43 {return _stepsToIgnore;}

Member Data Documentation

std::set<PoseObject*> g2o::SensorPose2D::_posesToIgnore
protected

Definition at line 49 of file sensor_pose2d.h.

Referenced by isVisible(), and sense().

int g2o::SensorPose2D::_stepsToIgnore
protected

Definition at line 47 of file sensor_pose2d.h.

Referenced by sense(), and SensorPose2D().


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