Represents a probabilistic 2D movement of the robot mobile base.
See the tutorial on probabilistic motion models.
Definition at line 33 of file CActionRobotMovement2D.h.
#include <mrpt/obs/CActionRobotMovement2D.h>
Classes | |
struct | TMotionModelOptions |
The parameter to be passed to "computeFromOdometry". More... | |
Public Types | |
enum | TEstimationMethod { emOdometry = 0, emScan2DMatching } |
A list of posible ways for estimating the content of a CActionRobotMovement2D object. More... | |
enum | TDrawSampleMotionModel { mmGaussian = 0, mmThrun } |
Public Member Functions | |
void * | operator new (size_t size) |
void * | operator new[] (size_t size) |
void | operator delete (void *ptr) noexcept |
void | operator delete[] (void *ptr) noexcept |
void | operator delete (void *memory, void *ptr) noexcept |
void * | operator new (size_t size, const std::nothrow_t &) noexcept |
void | operator delete (void *ptr, const std::nothrow_t &) noexcept |
CActionRobotMovement2D () | |
Constructor. More... | |
double | velocityLin () const |
double | velocityAng () const |
void | computeFromOdometry (const mrpt::poses::CPose2D &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 | computeFromEncoders (double K_left, double K_right, double D) |
If "hasEncodersInfo"=true, this method updates the pose estimation according to the ticks from both encoders and the passed parameters, which is passed internally to the method "computeFromOdometry" with the last used PDF options (or the defualt ones if not explicitly called by the user). More... | |
void | drawSingleSample (mrpt::poses::CPose2D &outSample) const |
Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situations. More... | |
void | prepareFastDrawSingleSamples () const |
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "drawSingleSample". More... | |
void | fastDrawSingleSample (mrpt::poses::CPose2D &outSample) const |
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples". More... | |
virtual mxArray * | writeToMatlab () 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... | |
RTTI classes and functions | |
mrpt::utils::CObject::Ptr | 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 void * | operator new (size_t size, void *ptr) |
Public Attributes | |
mrpt::utils::poly_ptr_ptr< mrpt::poses::CPosePDF::Ptr > | poseChange |
The 2D pose change probabilistic estimation. More... | |
mrpt::poses::CPose2D | rawOdometryIncrementReading |
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry". More... | |
TEstimationMethod | estimationMethod |
This fields indicates the way in which this estimation was obtained. More... | |
bool | hasEncodersInfo |
If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values. More... | |
int32_t | encoderLeftTicks |
For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);. More... | |
int32_t | encoderRightTicks |
bool | hasVelocities |
If "true" means that "velocityLin" and "velocityAng" contain valid values. More... | |
mrpt::math::TTwist2D | velocityLocal |
If "hasVelocities"=true, the robot velocity in local (robot frame, +X forward) coordinates. More... | |
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions | motionModelConfiguration |
mrpt::system::TTimeStamp | timestamp |
The associated time-stamp. More... | |
Protected Member Functions | |
void | computeFromOdometry_modelGaussian (const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &o) |
Computes the PDF of the pose increment from an odometry reading, using a Gaussian approximation as the motion model. More... | |
void | computeFromOdometry_modelThrun (const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &o) |
Computes the PDF of the pose increment from an odometry reading, using the motion model from Thrun's book. More... | |
void | drawSingleSample_modelGaussian (mrpt::poses::CPose2D &outSample) const |
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the user invokes "drawSingleSample". More... | |
void | drawSingleSample_modelThrun (mrpt::poses::CPose2D &outSample) const |
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user invokes "drawSingleSample". More... | |
void | prepareFastDrawSingleSample_modelGaussian () const |
Internal use. More... | |
void | prepareFastDrawSingleSample_modelThrun () const |
Internal use. More... | |
void | fastDrawSingleSample_modelGaussian (mrpt::poses::CPose2D &outSample) const |
Internal use. More... | |
void | fastDrawSingleSample_modelThrun (mrpt::poses::CPose2D &outSample) const |
Internal use. More... | |
CSerializable virtual methods | |
void | writeToStream (mrpt::utils::CStream &out, int *getVersion) const override |
Introduces a pure virtual method responsible for writing to a CStream. More... | |
void | readFromStream (mrpt::utils::CStream &in, int version) override |
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... | |
Protected Attributes | |
mrpt::math::CMatrixDouble33 | m_fastDrawGauss_Z |
Auxiliary matrix. More... | |
mrpt::poses::CPose2D | m_fastDrawGauss_M |
RTTI stuff | |
using | Ptr = std::shared_ptr< CActionRobotMovement2D > |
using | ConstPtr = std::shared_ptr< const CActionRobotMovement2D > |
static mrpt::utils::CLASSINIT | _init_CActionRobotMovement2D |
static const mrpt::utils::TRuntimeClassId | runtimeClassId |
static constexpr const char * | className = "CActionRobotMovement2D" |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
static const mrpt::utils::TRuntimeClassId & | GetRuntimeClassIdStatic () |
static mrpt::utils::CObject * | CreateObject () |
template<typename... Args> | |
static Ptr | Create (Args &&... args) |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const override |
Returns information about the class of an object in runtime. More... | |
virtual mrpt::utils::CObject * | clone () const override |
Returns a deep copy (clone) of the object, indepently of its class. More... | |
using mrpt::obs::CActionRobotMovement2D::ConstPtr = std::shared_ptr<const CActionRobotMovement2D > |
Definition at line 35 of file CActionRobotMovement2D.h.
using mrpt::obs::CActionRobotMovement2D::Ptr = std::shared_ptr< CActionRobotMovement2D > |
A typedef for the associated smart pointer
Definition at line 35 of file CActionRobotMovement2D.h.
Enumerator | |
---|---|
mmGaussian | |
mmThrun |
Definition at line 76 of file CActionRobotMovement2D.h.
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
Enumerator | |
---|---|
emOdometry | |
emScan2DMatching |
Definition at line 41 of file CActionRobotMovement2D.h.
CActionRobotMovement2D::CActionRobotMovement2D | ( | ) |
Constructor.
Definition at line 32 of file CActionRobotMovement2D.cpp.
|
staticprotected |
|
overridevirtual |
Returns a deep copy (clone) of the object, indepently of its class.
Implements mrpt::utils::CObject.
void CActionRobotMovement2D::computeFromEncoders | ( | double | K_left, |
double | K_right, | ||
double | D | ||
) |
If "hasEncodersInfo"=true, this method updates the pose estimation according to the ticks from both encoders and the passed parameters, which is passed internally to the method "computeFromOdometry" with the last used PDF options (or the defualt ones if not explicitly called by the user).
K_left | The meters / tick ratio for the left encoder. |
K_right | The meters / tick ratio for the right encoder. |
D | The distance between both wheels, in meters. |
Definition at line 377 of file CActionRobotMovement2D.cpp.
References R.
void CActionRobotMovement2D::computeFromOdometry | ( | const mrpt::poses::CPose2D & | 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).
Definition at line 408 of file CActionRobotMovement2D.cpp.
References mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection.
Referenced by mrpt::slam::AuxiliaryPFStandardAndOptimal< USE_OPTIMAL_SAMPLING >::PF_SLAM_implementation_gatherActionsCheckBothActObs(), mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().
|
protected |
Computes the PDF of the pose increment from an odometry reading, using a Gaussian approximation as the motion model.
Definition at line 450 of file CActionRobotMovement2D.cpp.
References mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a1, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a2, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a3, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a4, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::gaussianModel, mrpt::utils::poly_ptr_ptr< T >::get(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdPHI, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdXY, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::poses::CPose2D::phi(), poseChange, and mrpt::math::square().
|
protected |
Computes the PDF of the pose increment from an odometry reading, using the motion model from Thrun's book.
This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and Fox, 2006, pp.136.
Definition at line 501 of file CActionRobotMovement2D.cpp.
References mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::additional_std_phi, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::additional_std_XY, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa1_rot_rot, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa2_rot_trans, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa3_trans_trans, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa4_trans_rot, mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::utils::poly_ptr_ptr< T >::get(), mrpt::random::getRandomGenerator(), motionModelConfiguration, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::nParticlesCount, mrpt::poses::CPose2D::phi(), poseChange, mrpt::poses::CPosePDFParticles::resetDeterministic(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::thrunModel, mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
inlinestatic |
Definition at line 35 of file CActionRobotMovement2D.h.
|
static |
void CActionRobotMovement2D::drawSingleSample | ( | mrpt::poses::CPose2D & | outSample | ) | const |
Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situations.
Definition at line 566 of file CActionRobotMovement2D.cpp.
References drawSingleSample_modelGaussian(), drawSingleSample_modelThrun(), emOdometry, estimationMethod, mmGaussian, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection, motionModelConfiguration, and poseChange.
|
protected |
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the user invokes "drawSingleSample".
Definition at line 587 of file CActionRobotMovement2D.cpp.
References poseChange.
Referenced by drawSingleSample().
|
protected |
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user invokes "drawSingleSample".
Definition at line 598 of file CActionRobotMovement2D.cpp.
References mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::additional_std_phi, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::additional_std_XY, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa1_rot_rot, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa2_rot_trans, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa3_trans_trans, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa4_trans_rot, mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::random::getRandomGenerator(), motionModelConfiguration, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::poses::CPose2D::normalizePhi(), mrpt::poses::CPose2D::phi(), rawOdometryIncrementReading, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::thrunModel, mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
Referenced by drawSingleSample(), and fastDrawSingleSample_modelThrun().
void CActionRobotMovement2D::fastDrawSingleSample | ( | mrpt::poses::CPose2D & | outSample | ) | const |
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples".
Definition at line 670 of file CActionRobotMovement2D.cpp.
References emOdometry, estimationMethod, fastDrawSingleSample_modelGaussian(), fastDrawSingleSample_modelThrun(), mmGaussian, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection, motionModelConfiguration, and poseChange.
Referenced by mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update().
|
protected |
Internal use.
Definition at line 726 of file CActionRobotMovement2D.cpp.
References mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::random::getRandomGenerator(), m_fastDrawGauss_M, m_fastDrawGauss_Z, mrpt::poses::CPose2D::phi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
Referenced by fastDrawSingleSample().
|
protected |
Internal use.
Definition at line 746 of file CActionRobotMovement2D.cpp.
References drawSingleSample_modelThrun().
Referenced by fastDrawSingleSample().
|
overridevirtual |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::obs::CAction.
|
static |
Definition at line 35 of file CActionRobotMovement2D.h.
|
inlinenoexcept |
Definition at line 35 of file CActionRobotMovement2D.h.
|
inlinenoexcept |
Definition at line 35 of file CActionRobotMovement2D.h.
Definition at line 35 of file CActionRobotMovement2D.h.
|
inlinestatic |
Definition at line 35 of file CActionRobotMovement2D.h.
|
inlinenoexcept |
Definition at line 35 of file CActionRobotMovement2D.h.
|
inline |
Definition at line 35 of file CActionRobotMovement2D.h.
|
inline |
Definition at line 35 of file CActionRobotMovement2D.h.
|
protected |
Internal use.
Computes the eigenvalues/eigenvector decomposition of this matrix, so that: M = Z · D · ZT, where columns in Z are the eigenvectors and the diagonal matrix D contains the eigenvalues as diagonal elements, sorted in ascending order.
Definition at line 691 of file CActionRobotMovement2D.cpp.
References ASSERT_, mrpt::poses::CPosePDFGaussian::cov, mrpt::math::cov(), mrpt::utils::poly_ptr_ptr< T >::get(), IS_CLASS, m_fastDrawGauss_M, m_fastDrawGauss_Z, mrpt::poses::CPosePDFGaussian::mean, MRPT_END, MRPT_START, and poseChange.
Referenced by prepareFastDrawSingleSamples().
|
protected |
Internal use.
Definition at line 722 of file CActionRobotMovement2D.cpp.
Referenced by prepareFastDrawSingleSamples().
void CActionRobotMovement2D::prepareFastDrawSingleSamples | ( | ) | const |
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "drawSingleSample".
Definition at line 654 of file CActionRobotMovement2D.cpp.
References emOdometry, estimationMethod, mmGaussian, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection, motionModelConfiguration, prepareFastDrawSingleSample_modelGaussian(), and prepareFastDrawSingleSample_modelThrun().
Referenced by mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update().
|
overrideprotectedvirtual |
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.
in | The input binary stream where the object data must read from. |
version | The version of the object stored in the stream: use this version number in your code to know how to read the incoming data. |
std::exception | On any error, see CStream::ReadBuffer |
Implements mrpt::utils::CSerializable.
Definition at line 109 of file CActionRobotMovement2D.cpp.
References INVALID_TIMESTAMP, and MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.
|
inline |
Definition at line 75 of file CActionRobotMovement2D.h.
References mrpt::math::TTwist2D::omega, and velocityLocal.
|
inline |
Definition at line 74 of file CActionRobotMovement2D.h.
References velocityLocal, and mrpt::math::TTwist2D::vx.
|
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.
mxArray
(caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB. Definition at line 89 of file CSerializable.h.
|
overrideprotectedvirtual |
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.
out | The output binary stream where object must be dumped. |
getVersion | If nullptr, 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. |
std::exception | On any error, see CStream::WriteBuffer |
Implements mrpt::utils::CSerializable.
Definition at line 50 of file CActionRobotMovement2D.cpp.
|
staticprotected |
Definition at line 35 of file CActionRobotMovement2D.h.
|
static |
Definition at line 35 of file CActionRobotMovement2D.h.
int32_t mrpt::obs::CActionRobotMovement2D::encoderLeftTicks |
For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);.
Definition at line 65 of file CActionRobotMovement2D.h.
int32_t mrpt::obs::CActionRobotMovement2D::encoderRightTicks |
Definition at line 65 of file CActionRobotMovement2D.h.
TEstimationMethod mrpt::obs::CActionRobotMovement2D::estimationMethod |
This fields indicates the way in which this estimation was obtained.
Definition at line 56 of file CActionRobotMovement2D.h.
Referenced by drawSingleSample(), fastDrawSingleSample(), and prepareFastDrawSingleSamples().
bool mrpt::obs::CActionRobotMovement2D::hasEncodersInfo |
If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
Definition at line 60 of file CActionRobotMovement2D.h.
bool mrpt::obs::CActionRobotMovement2D::hasVelocities |
If "true" means that "velocityLin" and "velocityAng" contain valid values.
Definition at line 69 of file CActionRobotMovement2D.h.
|
mutableprotected |
Definition at line 211 of file CActionRobotMovement2D.h.
Referenced by fastDrawSingleSample_modelGaussian(), and prepareFastDrawSingleSample_modelGaussian().
|
mutableprotected |
Auxiliary matrix.
Definition at line 210 of file CActionRobotMovement2D.h.
Referenced by fastDrawSingleSample_modelGaussian(), and prepareFastDrawSingleSample_modelGaussian().
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions mrpt::obs::CActionRobotMovement2D::motionModelConfiguration |
mrpt::utils::poly_ptr_ptr<mrpt::poses::CPosePDF::Ptr> mrpt::obs::CActionRobotMovement2D::poseChange |
The 2D pose change probabilistic estimation.
Definition at line 51 of file CActionRobotMovement2D.h.
Referenced by computeFromOdometry_modelGaussian(), computeFromOdometry_modelThrun(), drawSingleSample(), drawSingleSample_modelGaussian(), fastDrawSingleSample(), mrpt::slam::AuxiliaryPFStandardAndOptimal< USE_OPTIMAL_SAMPLING >::PF_SLAM_implementation_gatherActionsCheckBothActObs(), and prepareFastDrawSingleSample_modelGaussian().
mrpt::poses::CPose2D mrpt::obs::CActionRobotMovement2D::rawOdometryIncrementReading |
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry".
Definition at line 54 of file CActionRobotMovement2D.h.
Referenced by drawSingleSample_modelThrun().
|
staticprotected |
Definition at line 35 of file CActionRobotMovement2D.h.
|
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 45 of file CAction.h.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().
mrpt::math::TTwist2D mrpt::obs::CActionRobotMovement2D::velocityLocal |
If "hasVelocities"=true, the robot velocity in local (robot frame, +X forward) coordinates.
Definition at line 72 of file CActionRobotMovement2D.h.
Referenced by velocityAng(), and velocityLin().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |