g2o::SensorPose3D Class Reference

#include <sensor_pose3d.h>

Inheritance diagram for g2o::SensorPose3D:

Collaboration diagram for g2o::SensorPose3D:

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 () 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< Robot3D, EdgeSE3, WorldObjectSE3 >
EdgeType *	mkEdge (WorldObjectType *object)
virtual void	addNoise (EdgeType *)

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

SensorPose3D()

g2o::SensorPose3D::SensorPose3D

(

const std::string &

name_

)

Definition at line 37 of file sensor_pose3d.cpp.

    : 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);
}

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

Member Function Documentation

addNoise()

void g2o::SensorPose3D::addNoise

(

EdgeType *

e

)

Definition at line 48 of file sensor_pose3d.cpp.

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

References g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_sampler, g2o::internal::fromVectorMQT(), and g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::information().

Referenced by sense().

isVisible()

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

protected

Definition at line 55 of file sensor_pose3d.cpp.

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

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxAngularDifference, g2o::PointSensorParameters::_maxRange2, g2o::PointSensorParameters::_minRange2, _posesToIgnore, g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_robotPoseObject, and g2o::BaseVertex< D, T >::estimate().

Referenced by sense().

sense()

void g2o::SensorPose3D::sense ( )

virtual

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

Definition at line 76 of file sensor_pose3d.cpp.

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

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::Robot< RobotPoseObject >::trajectory(), and g2o::BaseSensor::world().

setStepsToIgnore()

void g2o::SensorPose3D::setStepsToIgnore ( int stepsToIgnore_ )

inline

Definition at line 43 of file sensor_pose3d.h.

{ _stepsToIgnore = stepsToIgnore_; }

References _stepsToIgnore.

stepsToIgnore()

int g2o::SensorPose3D::stepsToIgnore ( ) const

inline

Definition at line 42 of file sensor_pose3d.h.

{ return _stepsToIgnore; }

References _stepsToIgnore.

Member Data Documentation

_posesToIgnore

std::set<PoseObject*> g2o::SensorPose3D::_posesToIgnore

protected

Definition at line 50 of file sensor_pose3d.h.

Referenced by isVisible(), and sense().

_stepsToIgnore

int g2o::SensorPose3D::_stepsToIgnore

protected

Definition at line 48 of file sensor_pose3d.h.

Referenced by sense(), SensorPose3D(), setStepsToIgnore(), and stepsToIgnore().

---

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

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