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

#include <sensor_pointxy.h>

Inheritance diagram for g2o::SensorPointXY:
Inheritance graph
[legend]
Collaboration diagram for g2o::SensorPointXY:
Collaboration graph
[legend]

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SensorPointXY (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, EdgeSE2PointXY, WorldObjectPointXY >
 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, EdgeSE2PointXY, WorldObjectPointXY >
EdgeTypemkEdge (WorldObjectType *object)
 

Additional Inherited Members

- Public Types inherited from g2o::BinarySensor< Robot2D, EdgeSE2PointXY, WorldObjectPointXY >
typedef Robot2D RobotType
 
typedef RobotType::PoseObject PoseObject
 
typedef RobotType::TrajectoryType TrajectoryType
 
typedef RobotType::PoseObject::VertexType PoseVertexType
 
typedef EdgeSE2PointXY EdgeType
 
typedef WorldObjectPointXY WorldObjectType
 
typedef WorldObjectType::VertexType VertexType
 
typedef EdgeType::InformationType InformationType
 
- Public Attributes inherited from g2o::BinarySensor< Robot2D, EdgeSE2PointXY, WorldObjectPointXY >
 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, EdgeSE2PointXY, WorldObjectPointXY >
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 36 of file sensor_pointxy.h.

Constructor & Destructor Documentation

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

Definition at line 33 of file sensor_pointxy.cpp.

33 : BinarySensor<Robot2D, EdgeSE2PointXY, WorldObjectPointXY>(name_) {}

Member Function Documentation

void g2o::SensorPointXY::addNoise ( EdgeType e)
virtual
bool g2o::SensorPointXY::isVisible ( SensorPointXY::WorldObjectType to)
protected

Definition at line 41 of file sensor_pointxy.cpp.

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxRange2, g2o::PointSensorParameters::_minRange2, g2o::BinarySensor< Robot2D, EdgeSE2PointXY, WorldObjectPointXY >::_robotPoseObject, and g2o::WorldObject< VertexType_ >::vertex().

Referenced by sense().

41  {
42  if (! _robotPoseObject)
43  return false;
44 
45  assert(to && to->vertex());
46  VertexType* v=to->vertex();
47  VertexType::EstimateType pose=v->estimate();
48  VertexType::EstimateType delta = _robotPoseObject->vertex()->estimate().inverse()*pose;
49  Vector2d translation=delta;
50  double range2=translation.squaredNorm();
51  if (range2>_maxRange2)
52  return false;
53  if (range2<_minRange2)
54  return false;
55  translation.normalize();
56  double bearing=acos(translation.x());
57  if (fabs(bearing)>_fov)
58  return false;
59  return true;
60  }
void g2o::SensorPointXY::sense ( )
virtual

Reimplemented from g2o::BinarySensor< Robot2D, EdgeSE2PointXY, WorldObjectPointXY >.

Definition at line 63 of file sensor_pointxy.cpp.

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

63  {
65  RobotType* r= dynamic_cast<RobotType*>(robot());
66  std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
67  int count = 0;
68  while (it!=r->trajectory().rend() && count < 1){
69  if (!_robotPoseObject)
70  _robotPoseObject = *it;
71  it++;
72  count++;
73  }
74  for (std::set<BaseWorldObject*>::iterator it=world()->objects().begin();
75  it!=world()->objects().end(); it++){
76  WorldObjectType* o=dynamic_cast<WorldObjectType*>(*it);
77  if (o && isVisible(o)){
78  EdgeType* e=mkEdge(o);
79  if (e && graph()) {
80  e->setMeasurementFromState();
81  addNoise(e);
82  graph()->addEdge(e);
83  }
84  }
85  }
86  }
virtual void addNoise(EdgeType *e)
World * world()
Definition: simulator.cpp:69
TrajectoryType & trajectory()
Definition: simulator.h:119
OptimizableGraph * graph()
Definition: simulator.cpp:75
BaseRobot * robot()
Definition: simulator.h:129
bool isVisible(WorldObjectType *to)
std::set< BaseWorldObject * > & objects()
Definition: simulator.h:284
virtual bool addEdge(HyperGraph::Edge *e)

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