g2o::SensorSE3Prior Class Reference

#include <sensor_se3_prior.h>

Inheritance diagram for g2o::SensorSE3Prior:

Collaboration diagram for g2o::SensorSE3Prior:

Public Member Functions
	SensorSE3Prior (const std::string &name_)
virtual void	sense ()
virtual void	addParameters ()
ParameterSE3Offset *	offsetParam ()
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::UnarySensor< Robot3D, EdgeSE3Prior >
	UnarySensor (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 ()

Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef PoseVertexType::EstimateType	RobotPoseType
Public Attributes inherited from g2o::UnarySensor< Robot3D, EdgeSE3Prior >
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Protected Attributes
RobotPoseType	_sensorPose
ParameterSE3Offset *	_offsetParam
Protected Attributes inherited from g2o::PointSensorParameters
double	_maxRange2
double	_minRange2
double	_fov
double	_maxAngularDifference
Protected Attributes inherited from g2o::UnarySensor< Robot3D, EdgeSE3Prior >
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::UnarySensor< Robot3D, EdgeSE3Prior >
typedef Robot3D	RobotType
typedef RobotType::PoseObject	PoseObject
typedef RobotType::TrajectoryType	TrajectoryType
typedef RobotType::PoseObject::VertexType	PoseVertexType
typedef EdgeSE3Prior	EdgeType
typedef EdgeType::InformationType	InformationType
Protected Member Functions inherited from g2o::UnarySensor< Robot3D, EdgeSE3Prior >
EdgeType *	mkEdge ()

Detailed Description

Definition at line 35 of file sensor_se3_prior.h.

Constructor & Destructor Documentation

g2o::SensorSE3Prior::SensorSE3Prior

(

const std::string &

name_

)

Definition at line 33 of file sensor_se3_prior.cpp.

References g2o::UnarySensor< Robot3D, EdgeSE3Prior >::_information, _offsetParam, and g2o::UnarySensor< Robot3D, EdgeSE3Prior >::setInformation().

                                                      : UnarySensor<Robot3D, EdgeSE3Prior>(name_) {
    _offsetParam = 0;
    _information.setIdentity();
    _information*=1000;
    _information(2,2)=10;
    setInformation(_information);
  }

Member Function Documentation

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

virtual

Reimplemented from g2o::UnarySensor< Robot3D, EdgeSE3Prior >.

Definition at line 49 of file sensor_se3_prior.cpp.

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

Referenced by sense().

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

void g2o::SensorSE3Prior::addParameters ( )

virtual

Reimplemented from g2o::BaseSensor.

Definition at line 42 of file sensor_se3_prior.cpp.

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

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

ParameterSE3Offset* g2o::SensorSE3Prior::offsetParam ( )

inline

Definition at line 42 of file sensor_se3_prior.h.

Referenced by main().

{return _offsetParam;};

void g2o::SensorSE3Prior::sense ( )

virtual

Reimplemented from g2o::UnarySensor< Robot3D, EdgeSE3Prior >.

Definition at line 56 of file sensor_se3_prior.cpp.

References _offsetParam, g2o::UnarySensor< Robot3D, EdgeSE3Prior >::_robotPoseObject, _sensorPose, g2o::OptimizableGraph::addEdge(), addNoise(), g2o::BaseSensor::graph(), g2o::Parameter::id(), g2o::UnarySensor< Robot3D, EdgeSE3Prior >::mkEdge(), g2o::ParameterSE3Offset::offset(), g2o::BaseSensor::robot(), g2o::EdgeSE3Prior::setMeasurementFromState(), g2o::OptimizableGraph::Edge::setParameterId(), and g2o::Robot< RobotPoseObject >::trajectory().

                             {
    if (! _offsetParam){
      return;
    }
    _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++;
    }
    if (!_robotPoseObject)
      return;
    _sensorPose = _robotPoseObject->vertex()->estimate()*_offsetParam->offset();
    EdgeType* e=mkEdge();
    e->setParameterId(0,_offsetParam->id());
    if (e && graph()) {
      graph()->addEdge(e);
      e->setMeasurementFromState();
      addNoise(e);
    }
  }

Member Data Documentation

ParameterSE3Offset* g2o::SensorSE3Prior::_offsetParam

protected

Definition at line 46 of file sensor_se3_prior.h.

Referenced by addParameters(), sense(), and SensorSE3Prior().

RobotPoseType g2o::SensorSE3Prior::_sensorPose

protected

Definition at line 45 of file sensor_se3_prior.h.

Referenced by sense().

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef PoseVertexType::EstimateType g2o::SensorSE3Prior::RobotPoseType

Definition at line 38 of file sensor_se3_prior.h.

---

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

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