#include <sensor_se3_prior.h>

Inheritance diagram for g2o::SensorSE3Prior:

[legend]

Collaboration diagram for g2o::SensorSE3Prior:

[legend]

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 () const

OptimizableGraph *	graph () const

const 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 ()

virtual void	addNoise (EdgeType *)

Detailed Description

Definition at line 35 of file sensor_se3_prior.h.

Constructor & Destructor Documentation

◆ SensorSE3Prior()

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

Definition at line 35 of file sensor_se3_prior.cpp.

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

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

Member Function Documentation

◆ addNoise()

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

Definition at line 50 of file sensor_se3_prior.cpp.

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

References g2o::UnarySensor< Robot3D, EdgeSE3Prior >::_sampler, g2o::internal::fromVectorMQT(), g2o::GaussianSampler< SampleType, CovarianceType >::generateSample(), and g2o::UnarySensor< Robot3D, EdgeSE3Prior >::information().

Referenced by sense().

◆ addParameters()

void g2o::SensorSE3Prior::addParameters ( )

virtual

Reimplemented from g2o::BaseSensor.

Definition at line 44 of file sensor_se3_prior.cpp.

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

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

◆ offsetParam()

ParameterSE3Offset * g2o::SensorSE3Prior::offsetParam ( )

inline

Definition at line 44 of file sensor_se3_prior.h.

44{ return _offsetParam; };

Referenced by main().

◆ sense()

void g2o::SensorSE3Prior::sense ( )

virtual

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

Definition at line 57 of file sensor_se3_prior.cpp.

                           {
  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();
  if (e && graph()) {
    e->setParameterId(0, _offsetParam->id());
    graph()->addEdge(e);
    e->setMeasurementFromState();
    addNoise(e);
  }
}

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(), and g2o::BaseSensor::robot().

Member Data Documentation

◆ _offsetParam

ParameterSE3Offset* g2o::SensorSE3Prior::_offsetParam

protected

Definition at line 49 of file sensor_se3_prior.h.

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

◆ _sensorPose

RobotPoseType g2o::SensorSE3Prior::_sensorPose

protected

Definition at line 48 of file sensor_se3_prior.h.

Referenced by sense().

◆ RobotPoseType

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

Definition at line 40 of file sensor_se3_prior.h.

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

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

Public Member Functions

Public Attributes

Protected Attributes

Additional Inherited Members