g2o::SensorOdometry3D Class Reference

#include <sensor_odometry3d.h>

Inheritance diagram for g2o::SensorOdometry3D:

Collaboration diagram for g2o::SensorOdometry3D:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SensorOdometry3D (const std::string &name_)
virtual void	sense ()
void	addNoise (EdgeType *e)
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 ()
OptimizableGraph *	graph ()
std::vector< Parameter * >	parameters ()
virtual void	addParameters ()

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
Protected Member Functions inherited from g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >
EdgeType *	mkEdge (WorldObjectType *object)
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

Detailed Description

Definition at line 35 of file sensor_odometry3d.h.

Constructor & Destructor Documentation

g2o::SensorOdometry3D::SensorOdometry3D

(

const std::string &

name_

)

Definition at line 35 of file sensor_odometry3d.cpp.

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

                                                          :
    BinarySensor<Robot3D, EdgeSE3, WorldObjectSE3>(name_){
    _information.setIdentity();
    _information*=100;
    _information(3,3)=10000;
    _information(4,4)=10000;
    _information(5,5)=10000;
    setInformation(_information);
  }

Member Function Documentation

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

virtual

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

Definition at line 45 of file sensor_odometry3d.cpp.

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

Referenced by sense().

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

void g2o::SensorOdometry3D::sense ( )

virtual

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

Definition at line 52 of file sensor_odometry3d.cpp.

References __PRETTY_FUNCTION__, g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::_robotPoseObject, g2o::OptimizableGraph::addEdge(), addNoise(), g2o::BaseSensor::graph(), g2o::BinarySensor< Robot3D, EdgeSE3, WorldObjectSE3 >::mkEdge(), g2o::BaseSensor::robot(), g2o::EdgeSE3::setMeasurementFromState(), and g2o::Robot< RobotPoseObject >::trajectory().

                              {
    
    if (! robot())
      return;
    
    RobotType* r =dynamic_cast<RobotType*>(robot());
    if (!r)
      return;
    
    PoseObject* pprev=0, *pcurr=0;
    std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
    if (it!=r->trajectory().rend()){
      pcurr = *it; 
      it++;
    }
    if (it!=r->trajectory().rend()){
      pprev = *it; 
      it++;
    }
    if (!(pcurr&&pprev)) {
      cerr << __PRETTY_FUNCTION__ << ": fatal, trajectory empty" << endl;
      return;
    }
    _robotPoseObject = pprev;
    EdgeType* e=mkEdge(pcurr);
    if (e){
      if (graph()) {
    graph()->addEdge(e);
  e->setMeasurementFromState();
        addNoise(e);
      }
    }
    _robotPoseObject = pcurr;
  }

---

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

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