g2o::SensorPose3DOffset Class Reference

#include <sensor_pose3d_offset.h>

Inheritance diagram for g2o::SensorPose3DOffset:

Collaboration diagram for g2o::SensorPose3DOffset:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SensorPose3DOffset (const std::string &name_)
virtual void	sense ()
int	stepsToIgnore () const
void	setStepsToIgnore (int stepsToIgnore_)
void	addNoise (EdgeType *e)
virtual void	addParameters ()
ParameterSE3Offset *	offsetParam1 ()
ParameterSE3Offset *	offsetParam2 ()
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, EdgeSE3Offset, 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 ()

Protected Member Functions
bool	isVisible (WorldObjectType *to)
Protected Member Functions inherited from g2o::BinarySensor< Robot3D, EdgeSE3Offset, WorldObjectSE3 >
EdgeType *	mkEdge (WorldObjectType *object)

Protected Attributes
int	_stepsToIgnore
ParameterSE3Offset *	_offsetParam1
ParameterSE3Offset *	_offsetParam2
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, EdgeSE3Offset, 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, EdgeSE3Offset, WorldObjectSE3 >
typedef Robot3D	RobotType
typedef RobotType::PoseObject	PoseObject
typedef RobotType::TrajectoryType	TrajectoryType
typedef RobotType::PoseObject::VertexType	PoseVertexType
typedef EdgeSE3Offset	EdgeType
typedef WorldObjectSE3	WorldObjectType
typedef WorldObjectType::VertexType	VertexType
typedef EdgeType::InformationType	InformationType
Public Attributes inherited from g2o::BinarySensor< Robot3D, EdgeSE3Offset, WorldObjectSE3 >
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Detailed Description

Definition at line 34 of file sensor_pose3d_offset.h.

Constructor & Destructor Documentation

g2o::SensorPose3DOffset::SensorPose3DOffset

(

const std::string &

name_

)

Definition at line 35 of file sensor_pose3d_offset.cpp.

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

                                                              : 
    BinarySensor<Robot3D, EdgeSE3Offset, WorldObjectSE3>(name_){  
    _offsetParam1 = _offsetParam2 =0;
    _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::SensorPose3DOffset::addNoise ( EdgeType *  e )

virtual

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

Definition at line 57 of file sensor_pose3d_offset.cpp.

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

Referenced by sense(), and setStepsToIgnore().

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

void g2o::SensorPose3DOffset::addParameters ( )

virtual

Reimplemented from g2o::BaseSensor.

Definition at line 47 of file sensor_pose3d_offset.cpp.

References _offsetParam1, _offsetParam2, g2o::World::addParameter(), and g2o::BaseSensor::world().

Referenced by setStepsToIgnore().

                                        {
    if (!_offsetParam1)
      _offsetParam1 = new ParameterSE3Offset();
    if (!_offsetParam2)
      _offsetParam2 = new ParameterSE3Offset();
    assert(world());
    world()->addParameter(_offsetParam1);
    world()->addParameter(_offsetParam2);
  }

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

protected

Definition at line 64 of file sensor_pose3d_offset.cpp.

References g2o::PointSensorParameters::_fov, g2o::PointSensorParameters::_maxAngularDifference, g2o::PointSensorParameters::_maxRange2, g2o::PointSensorParameters::_minRange2, _posesToIgnore, g2o::BinarySensor< Robot3D, EdgeSE3Offset, WorldObjectSE3 >::_robotPoseObject, and g2o::WorldObject< VertexType_ >::vertex().

Referenced by offsetParam2(), and 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;
  }

ParameterSE3Offset* g2o::SensorPose3DOffset::offsetParam1 ( )

inline

Definition at line 43 of file sensor_pose3d_offset.h.

References _offsetParam1.

Referenced by main().

{return _offsetParam1;};

ParameterSE3Offset* g2o::SensorPose3DOffset::offsetParam2 ( )

inline

Definition at line 44 of file sensor_pose3d_offset.h.

References _offsetParam2, and isVisible().

Referenced by main().

{return _offsetParam2;};

void g2o::SensorPose3DOffset::sense ( )

virtual

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

Definition at line 91 of file sensor_pose3d_offset.cpp.

References _offsetParam1, _offsetParam2, _posesToIgnore, g2o::BinarySensor< Robot3D, EdgeSE3Offset, WorldObjectSE3 >::_robotPoseObject, _stepsToIgnore, g2o::OptimizableGraph::addEdge(), addNoise(), g2o::BaseSensor::graph(), g2o::Parameter::id(), isVisible(), g2o::BinarySensor< Robot3D, EdgeSE3Offset, WorldObjectSE3 >::mkEdge(), g2o::World::objects(), g2o::BaseSensor::robot(), g2o::EdgeSE3Offset::setMeasurementFromState(), g2o::OptimizableGraph::Edge::setParameterId(), 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()) {
          e->setParameterId(0,_offsetParam1->id());
          e->setParameterId(1,_offsetParam2->id());
    graph()->addEdge(e);
    e->setMeasurementFromState();
          addNoise(e);
  }
      }
    }
  }

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

inline

Definition at line 40 of file sensor_pose3d_offset.h.

References _stepsToIgnore, addNoise(), and addParameters().

{_stepsToIgnore = stepsToIgnore_;}

int g2o::SensorPose3DOffset::stepsToIgnore ( ) const

inline

Definition at line 39 of file sensor_pose3d_offset.h.

References _stepsToIgnore.

{return _stepsToIgnore;}

Member Data Documentation

ParameterSE3Offset* g2o::SensorPose3DOffset::_offsetParam1

protected

Definition at line 50 of file sensor_pose3d_offset.h.

Referenced by addParameters(), offsetParam1(), sense(), and SensorPose3DOffset().

ParameterSE3Offset * g2o::SensorPose3DOffset::_offsetParam2

protected

Definition at line 50 of file sensor_pose3d_offset.h.

Referenced by addParameters(), offsetParam2(), sense(), and SensorPose3DOffset().

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

protected

Definition at line 53 of file sensor_pose3d_offset.h.

Referenced by isVisible(), and sense().

int g2o::SensorPose3DOffset::_stepsToIgnore

protected

Definition at line 49 of file sensor_pose3d_offset.h.

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

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/apps/g2o_simulator/sensor_pose3d_offset.h
• /home/xuezhisd/CLionProjects/g2o/g2o/apps/g2o_simulator/sensor_pose3d_offset.cpp