Main MRPT website > C++ reference for MRPT 1.5.7
List of all members | Classes | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Static Public Attributes
mrpt::obs::CActionRobotMovement3D Class Reference

Detailed Description

Represents a probabilistic 3D (6D) movement.

Currently this can be determined from visual odometry for full 6D, or from wheel encoders for 2D movements only. Here implemented the motion model from the next article: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci, F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF Monte Carlo Localization 4th Workshop on Planning, Perception and Navigation for Intelligent Vehicles, IROS, 2012

See also
CAction

Definition at line 27 of file obs/CActionRobotMovement3D.h.

#include <mrpt/obs/CActionRobotMovement3D.h>

Inheritance diagram for mrpt::obs::CActionRobotMovement3D:
Inheritance graph

Classes

struct  TMotionModelOptions
 The parameter to be passed to "computeFromOdometry". More...
 

Public Types

enum  TEstimationMethod { emOdometry = 0, emVisualOdometry }
 A list of posible ways for estimating the content of a CActionRobotMovement3D object. More...
 
enum  TDrawSampleMotionModel { mmGaussian = 0, mm6DOF }
 

Public Member Functions

voidoperator new (size_t size)
 
voidoperator new[] (size_t size)
 
void operator delete (void *ptr) throw ()
 
void operator delete[] (void *ptr) throw ()
 
void operator delete (void *memory, void *ptr) throw ()
 
voidoperator new (size_t size, const std::nothrow_t &) throw ()
 
void operator delete (void *ptr, const std::nothrow_t &) throw ()
 
 CActionRobotMovement3D ()
 
void computeFromOdometry (const mrpt::poses::CPose3D &odometryIncrement, const TMotionModelOptions &options)
 Computes the PDF of the pose increment from an odometry reading and according to the given motion model (speed and encoder ticks information is not modified). More...
 
void computeFromOdometry_model6DOF (const mrpt::poses::CPose3D &odometryIncrement, const TMotionModelOptions &o)
 Computes the PDF of the pose increment from an odometry reading, using the motion model for 6 DOF. More...
 
virtual mxArraywriteToMatlab () const
 Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. More...
 
CObjectclone () const
 Cloning interface for smart pointers. More...
 
RTTI classes and functions
mrpt::utils::CObjectPtr duplicateGetSmartPtr () const
 Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer). More...
 

Static Public Member Functions

static voidoperator new (size_t size, void *ptr)
 

Public Attributes

mrpt::poses::CPose3DPDFGaussian poseChange
 The 3D pose change probabilistic estimation. More...
 
mrpt::poses::CPose3D rawOdometryIncrementReading
 This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry". More...
 
TEstimationMethod estimationMethod
 This fields indicates the way this estimation was obtained. More...
 
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motionModelConfiguration
 
vector_bool hasVelocities
 Each "true" entry means that the corresponding "velocities" element contains valid data - There are 6 entries. More...
 
mrpt::math::CVectorFloat velocities
 The velocity of the robot in each of 6D: v_x,v_y,v_z,v_yaw,v_pitch,v_roll (linear in meters/sec and angular in rad/sec). More...
 
mrpt::system::TTimeStamp timestamp
 The associated time-stamp. More...
 

Static Public Attributes

static const mrpt::utils::TRuntimeClassId classCObject
 
RTTI stuff
static const mrpt::utils::TRuntimeClassId classCAction
 
RTTI stuff
static const mrpt::utils::TRuntimeClassId classCSerializable
 

Protected Member Functions

CSerializable virtual methods
void writeToStream (mrpt::utils::CStream &out, int *getVersion) const
 Introduces a pure virtual method responsible for writing to a CStream. More...
 
void readFromStream (mrpt::utils::CStream &in, int version)
 Introduces a pure virtual method responsible for loading from a CStream This can not be used directly be users, instead use "stream >> object;" for reading it from a stream or "stream >> object_ptr;" if the class is unknown apriori. More...
 

RTTI stuff

typedef CActionRobotMovement3DPtr Ptr
 
typedef CActionRobotMovement3DPtr ConstPtr
 
static mrpt::utils::CLASSINIT _init_CActionRobotMovement3D
 
static mrpt::utils::TRuntimeClassId classCActionRobotMovement3D
 
static const mrpt::utils::TRuntimeClassIdclassinfo
 
static const mrpt::utils::TRuntimeClassId_GetBaseClass ()
 
virtual const mrpt::utils::TRuntimeClassIdGetRuntimeClass () const
 Returns information about the class of an object in runtime. More...
 
virtual mrpt::utils::CObjectduplicate () const
 Returns a copy of the object, indepently of its class. More...
 
static mrpt::utils::CObjectCreateObject ()
 
static CActionRobotMovement3DPtr Create ()
 

Member Typedef Documentation

◆ ConstPtr

typedef CActionRobotMovement3DPtr mrpt::obs::CActionRobotMovement3D::ConstPtr

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ Ptr

typedef CActionRobotMovement3DPtr mrpt::obs::CActionRobotMovement3D::Ptr

A typedef for the associated smart pointer

Definition at line 30 of file obs/CActionRobotMovement3D.h.

Member Enumeration Documentation

◆ TDrawSampleMotionModel

Enumerator
mmGaussian 
mm6DOF 

Definition at line 54 of file obs/CActionRobotMovement3D.h.

◆ TEstimationMethod

A list of posible ways for estimating the content of a CActionRobotMovement3D object.

Enumerator
emOdometry 
emVisualOdometry 

Definition at line 35 of file obs/CActionRobotMovement3D.h.

Constructor & Destructor Documentation

◆ CActionRobotMovement3D()

CActionRobotMovement3D::CActionRobotMovement3D ( )

Definition at line 29 of file CActionRobotMovement3D.cpp.

Member Function Documentation

◆ _GetBaseClass()

static const mrpt::utils::TRuntimeClassId* mrpt::obs::CActionRobotMovement3D::_GetBaseClass ( )
staticprotected

◆ clone()

CObject* mrpt::utils::CObject::clone ( ) const
inlineinherited

Cloning interface for smart pointers.

Definition at line 133 of file CObject.h.

◆ computeFromOdometry()

void CActionRobotMovement3D::computeFromOdometry ( const mrpt::poses::CPose3D odometryIncrement,
const TMotionModelOptions options 
)

Computes the PDF of the pose increment from an odometry reading and according to the given motion model (speed and encoder ticks information is not modified).

According to the parameters in the passed struct, it will be called one the private sampling functions (see "see also" next).

See also
computeFromOdometry_model6DOF

Definition at line 94 of file CActionRobotMovement3D.cpp.

References mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::modelSelection.

◆ computeFromOdometry_model6DOF()

void CActionRobotMovement3D::computeFromOdometry_model6DOF ( const mrpt::poses::CPose3D odometryIncrement,
const TMotionModelOptions o 
)

Computes the PDF of the pose increment from an odometry reading, using the motion model for 6 DOF.

The source: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci, F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF Monte Carlo Localization 4th Workshop on Planning, Perception and Navigation for Intelligent Vehicles, IROS, 2012

See also
computeFromOdometry

Definition at line 135 of file CActionRobotMovement3D.cpp.

References mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a1, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a10, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a2, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a3, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a4, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a5, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a6, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a7, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a8, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a9, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::additional_std_angle, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::additional_std_XYZ, mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::mm6DOFModel, motionModelConfiguration, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::nParticlesCount, mrpt::poses::CPose3D::pitch(), poseChange, mrpt::random::randomGenerator, mrpt::poses::CPose3DPDFParticles::resetDeterministic(), mrpt::poses::CPose3D::roll(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), and mrpt::poses::CPose3D::yaw().

◆ Create()

static CActionRobotMovement3DPtr mrpt::obs::CActionRobotMovement3D::Create ( )
static

◆ CreateObject()

static mrpt::utils::CObject* mrpt::obs::CActionRobotMovement3D::CreateObject ( )
static

◆ duplicate()

virtual mrpt::utils::CObject* mrpt::obs::CActionRobotMovement3D::duplicate ( ) const
virtual

Returns a copy of the object, indepently of its class.

Implements mrpt::utils::CObject.

◆ duplicateGetSmartPtr()

mrpt::utils::CObjectPtr CObject::duplicateGetSmartPtr ( ) const
inlineinherited

Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer).

Definition at line 162 of file CObject.h.

References mrpt::utils::CObjectPtr.

Referenced by mrpt::obs::CRawlog::addActions(), mrpt::slam::CIncrementalMapPartitioner::addMapFrame(), and mrpt::obs::CRawlog::addObservations().

◆ GetRuntimeClass()

virtual const mrpt::utils::TRuntimeClassId* mrpt::obs::CActionRobotMovement3D::GetRuntimeClass ( ) const
virtual

Returns information about the class of an object in runtime.

Reimplemented from mrpt::obs::CAction.

◆ operator delete() [1/3]

void mrpt::obs::CActionRobotMovement3D::operator delete ( void memory,
void ptr 
)
throw (
)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator delete() [2/3]

void mrpt::obs::CActionRobotMovement3D::operator delete ( void ptr)
throw (
)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator delete() [3/3]

void mrpt::obs::CActionRobotMovement3D::operator delete ( void ptr,
const std::nothrow_t &   
)
throw (
)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator delete[]()

void mrpt::obs::CActionRobotMovement3D::operator delete[] ( void ptr)
throw (
)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator new() [1/3]

void* mrpt::obs::CActionRobotMovement3D::operator new ( size_t  size,
const std::nothrow_t &   
)
throw (
)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator new() [2/3]

static void* mrpt::obs::CActionRobotMovement3D::operator new ( size_t  size,
void ptr 
)
inlinestatic

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator new() [3/3]

void* mrpt::obs::CActionRobotMovement3D::operator new ( size_t  size)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ operator new[]()

void* mrpt::obs::CActionRobotMovement3D::operator new[] ( size_t  size)
inline

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ readFromStream()

void CActionRobotMovement3D::readFromStream ( mrpt::utils::CStream in,
int  version 
)
protectedvirtual

Introduces a pure virtual method responsible for loading from a CStream This can not be used directly be users, instead use "stream >> object;" for reading it from a stream or "stream >> object_ptr;" if the class is unknown apriori.

Parameters
inThe input binary stream where the object data must read from.
versionThe version of the object stored in the stream: use this version number in your code to know how to read the incoming data.
Exceptions
std::exceptionOn any error, see CStream::ReadBuffer
See also
CStream

Implements mrpt::utils::CSerializable.

Definition at line 66 of file CActionRobotMovement3D.cpp.

References INVALID_TIMESTAMP, MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION, and version.

◆ writeToMatlab()

virtual mxArray* mrpt::utils::CSerializable::writeToMatlab ( ) const
inlinevirtualinherited

Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.

Returns
A new mxArray (caller is responsible of memory freeing) or NULL is class does not support conversion to MATLAB.

Definition at line 79 of file CSerializable.h.

◆ writeToStream()

void CActionRobotMovement3D::writeToStream ( mrpt::utils::CStream out,
int *  getVersion 
) const
protectedvirtual

Introduces a pure virtual method responsible for writing to a CStream.

This can not be used directly be users, instead use "stream << object;" for writing it to a stream.

Parameters
outThe output binary stream where object must be dumped.
getVersionIf NULL, the object must be dumped. If not, only the version of the object dump must be returned in this pointer. This enables the versioning of objects dumping and backward compatibility with previously stored data.
Exceptions
std::exceptionOn any error, see CStream::WriteBuffer
See also
CStream

Implements mrpt::utils::CSerializable.

Definition at line 44 of file CActionRobotMovement3D.cpp.

References version.

Member Data Documentation

◆ _init_CActionRobotMovement3D

mrpt::utils::CLASSINIT mrpt::obs::CActionRobotMovement3D::_init_CActionRobotMovement3D
staticprotected

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ classCAction

const mrpt::utils::TRuntimeClassId mrpt::obs::CAction::classCAction
staticinherited

Definition at line 36 of file obs/CAction.h.

◆ classCActionRobotMovement3D

mrpt::utils::TRuntimeClassId mrpt::obs::CActionRobotMovement3D::classCActionRobotMovement3D
static

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ classCObject

const mrpt::utils::TRuntimeClassId mrpt::utils::CObject::classCObject
staticinherited

Definition at line 118 of file CObject.h.

◆ classCSerializable

const mrpt::utils::TRuntimeClassId mrpt::utils::CSerializable::classCSerializable
staticinherited

Definition at line 42 of file CSerializable.h.

◆ classinfo

const mrpt::utils::TRuntimeClassId* mrpt::obs::CActionRobotMovement3D::classinfo
static

Definition at line 30 of file obs/CActionRobotMovement3D.h.

◆ estimationMethod

TEstimationMethod mrpt::obs::CActionRobotMovement3D::estimationMethod

This fields indicates the way this estimation was obtained.

Definition at line 52 of file obs/CActionRobotMovement3D.h.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

◆ hasVelocities

vector_bool mrpt::obs::CActionRobotMovement3D::hasVelocities

Each "true" entry means that the corresponding "velocities" element contains valid data - There are 6 entries.

Definition at line 100 of file obs/CActionRobotMovement3D.h.

◆ motionModelConfiguration

struct OBS_IMPEXP mrpt::obs::CActionRobotMovement3D::TMotionModelOptions mrpt::obs::CActionRobotMovement3D::motionModelConfiguration

◆ poseChange

mrpt::poses::CPose3DPDFGaussian mrpt::obs::CActionRobotMovement3D::poseChange

◆ rawOdometryIncrementReading

mrpt::poses::CPose3D mrpt::obs::CActionRobotMovement3D::rawOdometryIncrementReading

This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry".

Definition at line 49 of file obs/CActionRobotMovement3D.h.

◆ timestamp

mrpt::system::TTimeStamp mrpt::obs::CAction::timestamp
inherited

The associated time-stamp.

This was added at 2-Dec-2007, new serialization versions have been added to derived classes to manage this time-stamp. Prior versions will be read as having a INVALID_TIMESTAMP value.

Definition at line 50 of file obs/CAction.h.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

◆ velocities

mrpt::math::CVectorFloat mrpt::obs::CActionRobotMovement3D::velocities

The velocity of the robot in each of 6D: v_x,v_y,v_z,v_yaw,v_pitch,v_roll (linear in meters/sec and angular in rad/sec).

Definition at line 104 of file obs/CActionRobotMovement3D.h.




Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019