MRPT
1.9.9
|
Represents a probabilistic 2D movement of the robot mobile base.
See docs: https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
Definition at line 30 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 | |
CActionRobotMovement2D () | |
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 void | getDescriptionAsText (std::ostream &o) const override |
Build a detailed, multi-line textual description of the action contents and dump it to the output stream. More... | |
std::string | getDescriptionAsTextValue () const |
Return by value version of getDescriptionAsText(std::ostream&) 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 for polymorphic hierarchies | |
mrpt::rtti::CObject::Ptr | duplicateGetSmartPtr () const |
Makes a deep copy of the object and returns a smart pointer to it. More... | |
Public Attributes | |
mrpt::containers::deepcopy_poly_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 {emOdometry} |
This fields indicates the way in which this estimation was obtained. More... | |
bool | hasEncodersInfo {false} |
If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values. More... | |
int32_t | encoderLeftTicks {0} |
For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);. More... | |
int32_t | encoderRightTicks {0} |
bool | hasVelocities {false} |
If "true" means that "velocityLin" and "velocityAng" contain valid values. More... | |
mrpt::math::TTwist2D | velocityLocal {.0, .0, .0} |
If "hasVelocities"=true, the robot velocity in local (robot frame, +X forward) coordinates. More... | |
TMotionModelOptions | motionModelConfiguration |
mrpt::system::TTimeStamp | timestamp {INVALID_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 | |
uint8_t | serializeGetVersion () const override |
Must return the current versioning number of the object. More... | |
void | serializeTo (mrpt::serialization::CArchive &out) const override |
Pure virtual method for writing (serializing) to an abstract archive. More... | |
void | serializeFrom (mrpt::serialization::CArchive &in, uint8_t serial_version) override |
Pure virtual method for reading (deserializing) from an abstract archive. More... | |
CSerializable virtual methods | |
virtual void | serializeTo (CSchemeArchiveBase &out) const |
Virtual method for writing (serializing) to an abstract schema based archive. More... | |
virtual void | serializeFrom (CSchemeArchiveBase &in) |
Virtual method for reading (deserializing) from an abstract schema based archive. 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< mrpt::obs ::CActionRobotMovement2D > |
using | ConstPtr = std::shared_ptr< const mrpt::obs ::CActionRobotMovement2D > |
using | UniquePtr = std::unique_ptr< mrpt::obs ::CActionRobotMovement2D > |
using | ConstUniquePtr = std::unique_ptr< const mrpt::obs ::CActionRobotMovement2D > |
static const mrpt::rtti::TRuntimeClassId | runtimeClassId |
static constexpr const char * | className = "mrpt::obs" "::" "CActionRobotMovement2D" |
static const mrpt::rtti::TRuntimeClassId * | _GetBaseClass () |
static constexpr auto | getClassName () |
static const mrpt::rtti::TRuntimeClassId & | GetRuntimeClassIdStatic () |
static std::shared_ptr< CObject > | CreateObject () |
template<typename... Args> | |
static Ptr | Create (Args &&... args) |
template<typename Alloc , typename... Args> | |
static Ptr | CreateAlloc (const Alloc &alloc, Args &&... args) |
template<typename... Args> | |
static UniquePtr | CreateUnique (Args &&... args) |
virtual const mrpt::rtti::TRuntimeClassId * | GetRuntimeClass () const override |
Returns information about the class of an object in runtime. More... | |
virtual mrpt::rtti::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 mrpt::obs :: CActionRobotMovement2D > |
Definition at line 32 of file CActionRobotMovement2D.h.
using mrpt::obs::CActionRobotMovement2D::ConstUniquePtr = std::unique_ptr<const mrpt::obs :: CActionRobotMovement2D > |
Definition at line 32 of file CActionRobotMovement2D.h.
using mrpt::obs::CActionRobotMovement2D::Ptr = std::shared_ptr< mrpt::obs :: CActionRobotMovement2D > |
A type for the associated smart pointer
Definition at line 32 of file CActionRobotMovement2D.h.
using mrpt::obs::CActionRobotMovement2D::UniquePtr = std::unique_ptr< mrpt::obs :: CActionRobotMovement2D > |
Definition at line 32 of file CActionRobotMovement2D.h.
Enumerator | |
---|---|
mmGaussian | |
mmThrun |
Definition at line 72 of file CActionRobotMovement2D.h.
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
Enumerator | |
---|---|
emOdometry | |
emScan2DMatching |
Definition at line 38 of file CActionRobotMovement2D.h.
CActionRobotMovement2D::CActionRobotMovement2D | ( | ) |
Definition at line 32 of file CActionRobotMovement2D.cpp.
|
staticprotected |
|
overridevirtual |
Returns a deep copy (clone) of the object, indepently of its class.
Implements mrpt::rtti::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 356 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 387 of file CActionRobotMovement2D.cpp.
References mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection.
Referenced by mrpt::apps::MonteCarloLocalization_Base::do_pf_localization(), mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_gatherActionsCheckBothActObs(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal(), 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 404 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, ASSERT_, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::gaussianModel, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdPHI, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdXY, mrpt::math::multiply_HCHt(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::norm(), mrpt::poses::CPose2D::phi(), and mrpt::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 455 of file CActionRobotMovement2D.cpp.
References 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, ASSERT_, mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::random::getRandomGenerator(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::norm(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::nParticlesCount, mrpt::poses::CPose2D::phi(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::thrunModel, mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().
|
inlinestatic |
Definition at line 32 of file CActionRobotMovement2D.h.
|
inlinestatic |
Definition at line 32 of file CActionRobotMovement2D.h.
|
static |
|
inlinestatic |
Definition at line 32 of file CActionRobotMovement2D.h.
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 520 of file CActionRobotMovement2D.cpp.
|
protected |
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the user invokes "drawSingleSample".
Definition at line 541 of file CActionRobotMovement2D.cpp.
|
protected |
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user invokes "drawSingleSample".
Definition at line 552 of file CActionRobotMovement2D.cpp.
References mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::random::getRandomGenerator(), mrpt::poses::CPose2D::normalizePhi(), mrpt::poses::CPose2D::phi(), mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().
|
inlineinherited |
Makes a deep copy of the object and returns a smart pointer to it.
Definition at line 204 of file CObject.h.
References mrpt::rtti::CObject::clone().
Referenced by mrpt::obs::CRawlog::insert().
void CActionRobotMovement2D::fastDrawSingleSample | ( | mrpt::poses::CPose2D & | outSample | ) | const |
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples".
Definition at line 624 of file CActionRobotMovement2D.cpp.
Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal().
|
protected |
Internal use.
Definition at line 681 of file CActionRobotMovement2D.cpp.
References mrpt::math::MatrixVectorBase< Scalar, Derived >::assign(), mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), and mrpt::random::getRandomGenerator().
|
protected |
Internal use.
Definition at line 702 of file CActionRobotMovement2D.cpp.
|
inlinestatic |
Definition at line 32 of file CActionRobotMovement2D.h.
|
overridevirtual |
Build a detailed, multi-line textual description of the action contents and dump it to the output stream.
Reimplemented from mrpt::obs::CAction.
Definition at line 708 of file CActionRobotMovement2D.cpp.
References CLASS_ID, mrpt::math::MatrixBase< Scalar, Derived >::det(), mrpt::format(), mrpt::obs::CAction::getDescriptionAsText(), and mrpt::typemeta::TEnumType< ENUMTYPE >::value2name().
|
inherited |
Return by value version of getDescriptionAsText(std::ostream&)
Definition at line 34 of file CAction.cpp.
References mrpt::obs::CAction::getDescriptionAsText().
|
overridevirtual |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::obs::CAction.
|
static |
|
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 645 of file CActionRobotMovement2D.cpp.
References mrpt::math::MatrixVectorBase< Scalar, Derived >::array(), mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), ASSERT_, mrpt::math::cov(), mrpt::math::MatrixBase< Scalar, Derived >::eig_symmetric(), IS_CLASS, MRPT_END, MRPT_START, and mrpt::math::MatrixBase< Scalar, Derived >::setDiagonal().
|
protected |
Internal use.
Definition at line 677 of file CActionRobotMovement2D.cpp.
void CActionRobotMovement2D::prepareFastDrawSingleSamples | ( | ) | const |
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "drawSingleSample".
Definition at line 608 of file CActionRobotMovement2D.cpp.
Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal().
|
overrideprotectedvirtual |
Pure virtual method for reading (deserializing) from an abstract archive.
Users don't call this method directly. Instead, use stream >> object;
.
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 I/O error |
Implements mrpt::serialization::CSerializable.
Definition at line 87 of file CActionRobotMovement2D.cpp.
References INVALID_TIMESTAMP, MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION, mrpt::serialization::CArchive::ReadAs(), and mrpt::serialization::CArchive::ReadBuffer().
|
inlineprotectedvirtualinherited |
Virtual method for reading (deserializing) from an abstract schema based archive.
Definition at line 74 of file CSerializable.h.
References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.
|
overrideprotectedvirtual |
Must return the current versioning number of the object.
Start in zero for new classes, and increments each time there is a change in the stored format.
Implements mrpt::serialization::CSerializable.
Definition at line 39 of file CActionRobotMovement2D.cpp.
|
overrideprotectedvirtual |
Pure virtual method for writing (serializing) to an abstract archive.
Users don't call this method directly. Instead, use stream << object;
.
std::exception | On any I/O error |
Implements mrpt::serialization::CSerializable.
Definition at line 40 of file CActionRobotMovement2D.cpp.
References out.
|
inlineprotectedvirtualinherited |
Virtual method for writing (serializing) to an abstract schema based archive.
Definition at line 64 of file CSerializable.h.
References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.
|
inline |
Definition at line 71 of file CActionRobotMovement2D.h.
References mrpt::math::TTwist2D::omega, and velocityLocal.
|
inline |
Definition at line 70 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 90 of file CSerializable.h.
|
static |
Definition at line 32 of file CActionRobotMovement2D.h.
int32_t mrpt::obs::CActionRobotMovement2D::encoderLeftTicks {0} |
For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);.
Definition at line 61 of file CActionRobotMovement2D.h.
int32_t mrpt::obs::CActionRobotMovement2D::encoderRightTicks {0} |
Definition at line 61 of file CActionRobotMovement2D.h.
TEstimationMethod mrpt::obs::CActionRobotMovement2D::estimationMethod {emOdometry} |
This fields indicates the way in which this estimation was obtained.
Definition at line 52 of file CActionRobotMovement2D.h.
bool mrpt::obs::CActionRobotMovement2D::hasEncodersInfo {false} |
If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
Definition at line 56 of file CActionRobotMovement2D.h.
bool mrpt::obs::CActionRobotMovement2D::hasVelocities {false} |
If "true" means that "velocityLin" and "velocityAng" contain valid values.
Definition at line 65 of file CActionRobotMovement2D.h.
|
mutableprotected |
Definition at line 239 of file CActionRobotMovement2D.h.
|
mutableprotected |
Auxiliary matrix.
Definition at line 238 of file CActionRobotMovement2D.h.
TMotionModelOptions mrpt::obs::CActionRobotMovement2D::motionModelConfiguration |
Definition at line 147 of file CActionRobotMovement2D.h.
Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal().
mrpt::containers::deepcopy_poly_ptr<mrpt::poses::CPosePDF::Ptr> mrpt::obs::CActionRobotMovement2D::poseChange |
The 2D pose change probabilistic estimation.
Definition at line 47 of file CActionRobotMovement2D.h.
Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_gatherActionsCheckBothActObs(), and mrpt::detectors::CDetectorDoorCrossing::process().
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 50 of file CActionRobotMovement2D.h.
Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal().
|
staticprotected |
Definition at line 32 of file CActionRobotMovement2D.h.
|
inherited |
The associated time-stamp.
Definition at line 33 of file CAction.h.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().
mrpt::math::TTwist2D mrpt::obs::CActionRobotMovement2D::velocityLocal {.0, .0, .0} |
If "hasVelocities"=true, the robot velocity in local (robot frame, +X forward) coordinates.
Definition at line 68 of file CActionRobotMovement2D.h.
Referenced by velocityAng(), and velocityLin().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020 |