#include <sensor_pointxy_bearing.h>

Inheritance diagram for g2o::SensorPointXYBearing:

[legend]

Collaboration diagram for g2o::SensorPointXYBearing:

[legend]

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

virtual void	addNoise (EdgeType *e)

virtual void	sense ()

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, EdgeSE2PointXYBearing, WorldObjectPointXY >
	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 () const

OptimizableGraph *	graph () const

const std::vector< Parameter * > &	parameters ()

virtual void	addParameters ()

Protected Member Functions
bool	isVisible (WorldObjectType *to)

Protected Member Functions inherited from g2o::BinarySensor< Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY >
EdgeType *	mkEdge (WorldObjectType *object)

virtual void	addNoise (EdgeType *)

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

typedef RobotType::PoseObject	PoseObject

typedef RobotType::TrajectoryType	TrajectoryType

typedef RobotType::PoseObject::VertexType	PoseVertexType

typedef EdgeSE2PointXYBearing	EdgeType

typedef WorldObjectPointXY	WorldObjectType

typedef WorldObjectType::VertexType	VertexType

typedef EdgeType::InformationType	InformationType

Public Attributes inherited from g2o::BinarySensor< Robot2D, EdgeSE2PointXYBearing, 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, EdgeSE2PointXYBearing, 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_bearing.h.

Constructor & Destructor Documentation

◆ SensorPointXYBearing()

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

Definition at line 35 of file sensor_pointxy_bearing.cpp.

    : BinarySensor<Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY>(name_) {
  _information(0, 0) = 180.0 / M_PI;
}

References g2o::BinarySensor< Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY >::_information.

Member Function Documentation

◆ addNoise()

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

virtual

Definition at line 40 of file sensor_pointxy_bearing.cpp.

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

References g2o::BinarySensor< Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY >::_sampler, g2o::GaussianSampler< SampleType, CovarianceType >::generateSample(), and g2o::BinarySensor< Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY >::information().

Referenced by sense().

◆ isVisible()

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

protected

Definition at line 46 of file sensor_pointxy_bearing.cpp.

                                             {
  if (!_robotPoseObject) 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;
  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;
  return true;
}

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxRange2, g2o::PointSensorParameters::_minRange2, and g2o::BinarySensor< Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY >::_robotPoseObject.

Referenced by sense().

◆ sense()

void g2o::SensorPointXYBearing::sense ( )

virtual

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

Definition at line 65 of file sensor_pointxy_bearing.cpp.

                                 {
  _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);
      }
    }
  }
}

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

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

/build/reproducible-path/g2o-0~20230806/g2o/apps/g2o_simulator/sensor_pointxy_bearing.h
/build/reproducible-path/g2o-0~20230806/g2o/apps/g2o_simulator/sensor_pointxy_bearing.cpp

Public Member Functions

Protected Member Functions