MRPT  2.0.0
List of all members | Classes | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes
mrpt::slam::CRangeBearingKFSLAM Class Referenceabstract

Detailed Description

An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks.

The main method is "processActionObservation" which processes pairs of action/observation. The state vector comprises: 3D robot position, a quaternion for its attitude, and the 3D landmarks in the map.

The following Wiki page describes an front-end application based on this class: https://www.mrpt.org/Application:kf-slam

For the theory behind this implementation, see the technical report in: https://www.mrpt.org/6D-SLAM

See also
An implementation for 2D only: CRangeBearingKFSLAM2D

Definition at line 47 of file CRangeBearingKFSLAM.h.

#include <mrpt/slam/CRangeBearingKFSLAM.h>

Inheritance diagram for mrpt::slam::CRangeBearingKFSLAM:

Classes

struct  TDataAssocInfo
 Information for data-association: More...
 
struct  TOptions
 The options for the algorithm. More...
 

Public Types

using landmark_point_t = mrpt::math::TPoint3D
 Either mrpt::math::TPoint2D or mrpt::math::TPoint3D. More...
 
using kftype = double
 The numeric type used in the Kalman Filter (default=double) More...
 
using KFCLASS = CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >
 My class, in a shorter name! More...
 
using KFVector = mrpt::math::CVectorDynamic< double >
 
using KFMatrix = mrpt::math::CMatrixDynamic< double >
 
using KFMatrix_VxV = mrpt::math::CMatrixFixed< double, VEH_SIZE, VEH_SIZE >
 
using KFMatrix_OxO = mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE >
 
using KFMatrix_FxF = mrpt::math::CMatrixFixed< double, FEAT_SIZE, FEAT_SIZE >
 
using KFMatrix_AxA = mrpt::math::CMatrixFixed< double, ACT_SIZE, ACT_SIZE >
 
using KFMatrix_VxO = mrpt::math::CMatrixFixed< double, VEH_SIZE, OBS_SIZE >
 
using KFMatrix_VxF = mrpt::math::CMatrixFixed< double, VEH_SIZE, FEAT_SIZE >
 
using KFMatrix_FxV = mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE >
 
using KFMatrix_FxO = mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE >
 
using KFMatrix_OxF = mrpt::math::CMatrixFixed< double, OBS_SIZE, FEAT_SIZE >
 
using KFMatrix_OxV = mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE >
 
using KFArray_VEH = mrpt::math::CVectorFixed< double, VEH_SIZE >
 
using KFArray_ACT = mrpt::math::CVectorFixed< double, ACT_SIZE >
 
using KFArray_OBS = mrpt::math::CVectorFixed< double, OBS_SIZE >
 
using vector_KFArray_OBS = std::vector< KFArray_OBS >
 
using KFArray_FEAT = mrpt::math::CVectorFixed< double, FEAT_SIZE >
 

Public Member Functions

 CRangeBearingKFSLAM ()
 Constructor. More...
 
 ~CRangeBearingKFSLAM () override
 Destructor: More...
 
void reset ()
 Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). More...
 
void processActionObservation (mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &SF)
 Process one new action and observations to update the map and robot pose estimate. More...
 
void getCurrentState (mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint3D > &out_landmarksPositions, std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_landmarkIDs, mrpt::math::CVectorDouble &out_fullState, mrpt::math::CMatrixDouble &out_fullCovariance) const
 Returns the complete mean and cov. More...
 
void getCurrentState (mrpt::poses::CPose3DPDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint3D > &out_landmarksPositions, std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_landmarkIDs, mrpt::math::CVectorDouble &out_fullState, mrpt::math::CMatrixDouble &out_fullCovariance) const
 Returns the complete mean and cov. More...
 
void getCurrentRobotPose (mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose) const
 Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion). More...
 
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean () const
 Get the current robot pose mean, as a 3D+quaternion pose. More...
 
void getCurrentRobotPose (mrpt::poses::CPose3DPDFGaussian &out_robotPose) const
 Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles). More...
 
void getAs3DObject (mrpt::opengl::CSetOfObjects::Ptr &outObj) const
 Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state. More...
 
void loadOptions (const mrpt::config::CConfigFileBase &ini)
 Load options from a ini-like file/text. More...
 
const TDataAssocInfogetLastDataAssociation () const
 Returns a read-only reference to the information on the last data-association. More...
 
void getLastPartition (std::vector< std::vector< uint32_t >> &parts)
 Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only if options.doPartitioningExperiment = true. More...
 
void getLastPartitionLandmarks (std::vector< std::vector< uint32_t >> &landmarksMembership) const
 Return the partitioning of the landmarks in clusters accoring to the last partition. More...
 
void getLastPartitionLandmarksAsIfFixedSubmaps (size_t K, std::vector< std::vector< uint32_t >> &landmarksMembership)
 For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used. More...
 
double computeOffDiagonalBlocksApproximationError (const std::vector< std::vector< uint32_t >> &landmarksMembership) const
 Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks. More...
 
void reconsiderPartitionsNow ()
 The partitioning of the entire map is recomputed again. More...
 
CIncrementalMapPartitioner::TOptionsmapPartitionOptions ()
 Provides access to the parameters of the map partitioning algorithm. More...
 
void saveMapAndPath2DRepresentationAsMATLABFile (const std::string &fil, float stdCount=3.0f, const std::string &styleLandmarks=std::string("b"), const std::string &stylePath=std::string("r"), const std::string &styleRobot=std::string("r")) const
 Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D. More...
 
size_t getNumberOfLandmarksInTheMap () const
 
bool isMapEmpty () const
 
size_t getStateVectorLength () const
 
KFVectorinternal_getXkk ()
 
KFMatrixinternal_getPkk ()
 
void getLandmarkMean (size_t idx, KFArray_FEAT &feat) const
 Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems). More...
 
void getLandmarkCov (size_t idx, KFMatrix_FxF &feat_cov) const
 Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems). More...
 
mrpt::system::CTimeLoggergetProfiler ()
 

Static Public Member Functions

static constexpr size_t get_vehicle_size ()
 
static constexpr size_t get_observation_size ()
 
static constexpr size_t get_feature_size ()
 
static constexpr size_t get_action_size ()
 
static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & logging_levels_to_colors ()
 Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More...
 
static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & logging_levels_to_names ()
 Map from VerbosityLevels to their corresponding names. More...
 

Public Attributes

mrpt::slam::CRangeBearingKFSLAM::TOptions options
 
TKF_options KF_options
 Generic options for the Kalman Filter algorithm itself. More...
 

Protected Member Functions

mrpt::poses::CPose3DQuat getIncrementFromOdometry () const
 Return the last odometry, as a pose increment. More...
 
void runOneKalmanIteration ()
 The main entry point, executes one complete step: prediction + update. More...
 
Virtual methods for Kalman Filter implementation
void OnGetAction (KFArray_ACT &out_u) const override
 Must return the action vector u. More...
 
void OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $. More...
 
void OnTransitionJacobian (KFMatrix_VxV &out_F) const override
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $. More...
 
void OnTransitionNoise (KFMatrix_VxV &out_Q) const override
 Implements the transition noise covariance $ Q_k $. More...
 
void OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R) override
 This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map. More...
 
void OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
 
void OnObservationJacobians (size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
 Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $. More...
 
void OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const override
 Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
 
void OnGetObservationNoise (KFMatrix_OxO &out_R) const override
 Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More...
 
void OnPreComputingPredictions (const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const override
 This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. More...
 
void OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const override
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
void OnNewLandmarkAddedToMap (const size_t in_obsIdx, const size_t in_idxNewFeat) override
 If applicable to the given problem, do here any special handling of adding a new landmark to the map. More...
 
void OnNormalizeStateVector () override
 This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it. More...
 

Protected Attributes

mrpt::obs::CActionCollection::Ptr m_action
 Set up by processActionObservation. More...
 
mrpt::obs::CSensoryFrame::Ptr m_SF
 Set up by processActionObservation. More...
 
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
 The mapping between landmark IDs and indexes in the Pkk cov. More...
 
CIncrementalMapPartitioner mapPartitioner
 Used for map partitioning experiments. More...
 
mrpt::maps::CSimpleMap m_SFs
 The sequence of all the observations and the robot path (kept for debugging, statistics,etc) More...
 
std::vector< std::vector< uint32_t > > m_lastPartitionSet
 
TDataAssocInfo m_last_data_association
 Last data association. More...
 
mrpt::system::CTimeLogger m_timLogger
 
VerbosityLevel m_min_verbosity_level {LVL_INFO}
 Provided messages with VerbosityLevel smaller than this value shall be ignored. More...
 
Kalman filter state
KFVector m_xkk
 The system state vector. More...
 
KFMatrix m_pkk
 The system full covariance matrix. More...
 

Virtual methods for Kalman Filter implementation

virtual void OnInverseObservationModel ([[maybe_unused]] const KFArray_OBS &in_z, [[maybe_unused]] KFArray_FEAT &out_yn, [[maybe_unused]] KFMatrix_FxV &out_dyn_dxv, [[maybe_unused]] KFMatrix_FxO &out_dyn_dhn) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
virtual void OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn, [[maybe_unused]] KFMatrix_FxF &out_dyn_dhn_R_dyn_dhnT, bool &out_use_dyn_dhn_jacobian) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
virtual void OnNewLandmarkAddedToMap ([[maybe_unused]] const size_t in_obsIdx, [[maybe_unused]] const size_t in_idxNewFeat)
 If applicable to the given problem, do here any special handling of adding a new landmark to the map. More...
 
virtual void OnPostIteration ()
 This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). More...
 
virtual void OnGetAction (KFArray_ACT &out_u) const=0
 Must return the action vector u. More...
 
virtual void OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const=0
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $. More...
 
virtual void OnTransitionJacobian ([[maybe_unused]] KFMatrix_VxV &out_F) const
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $. More...
 
virtual void OnTransitionJacobianNumericGetIncrements (KFArray_VEH &out_increments) const
 Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
virtual void OnTransitionNoise (KFMatrix_VxV &out_Q) const=0
 Implements the transition noise covariance $ Q_k $. More...
 
virtual void OnPreComputingPredictions ([[maybe_unused]] const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
 This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. More...
 
virtual void OnGetObservationNoise (KFMatrix_OxO &out_R) const=0
 Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More...
 
virtual void OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R)=0
 This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map. More...
 
virtual void OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const=0
 Implements the observation prediction $ h_i(x) $. More...
 
virtual void OnObservationJacobians ([[maybe_unused]] size_t idx_landmark_to_predict, [[maybe_unused]] KFMatrix_OxV &Hx, [[maybe_unused]] KFMatrix_OxF &Hy) const
 Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $. More...
 
virtual void OnObservationJacobiansNumericGetIncrements (KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
 Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
virtual void OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const
 Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
 

Logging methods

bool logging_enable_console_output {true}
 [Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More...
 
bool logging_enable_keep_record {false}
 [Default=false] Enables storing all messages into an internal list. More...
 
void logStr (const VerbosityLevel level, std::string_view msg_str) const
 Main method to add the specified message string to the logger. More...
 
void logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
 Alternative logging method, which mimics the printf behavior. More...
 
void void logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const
 Log the given message only if the condition is satisfied. More...
 
void setLoggerName (const std::string &name)
 Set the name of the COutputLogger instance. More...
 
std::string getLoggerName () const
 Return the name of the COutputLogger instance. More...
 
void setMinLoggingLevel (const VerbosityLevel level)
 Set the minimum logging level for which the incoming logs are going to be taken into account. More...
 
void setVerbosityLevel (const VerbosityLevel level)
 alias of setMinLoggingLevel() More...
 
VerbosityLevel getMinLoggingLevel () const
 
bool isLoggingLevelVisible (VerbosityLevel level) const
 
void getLogAsString (std::string &log_contents) const
 Fill the provided string with the contents of the logger's history in std::string representation. More...
 
std::string getLogAsString () const
 Get the history of COutputLogger instance in a string representation. More...
 
void writeLogToFile (const std::string *fname_in=nullptr) const
 Write the contents of the COutputLogger instance to an external file. More...
 
void dumpLogToConsole () const
 Dump the current contents of the COutputLogger instance in the terminal window. More...
 
std::string getLoggerLastMsg () const
 Return the last Tmsg instance registered in the logger history. More...
 
void getLoggerLastMsg (std::string &msg_str) const
 Fill inputtted string with the contents of the last message in history. More...
 
void loggerReset ()
 Reset the contents of the logger instance. More...
 
void logRegisterCallback (output_logger_callback_t userFunc)
 
bool logDeregisterCallback (output_logger_callback_t userFunc)
 

Member Typedef Documentation

◆ KFArray_ACT

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_ACT = mrpt::math::CVectorFixed<double , ACT_SIZE>
inherited

Definition at line 256 of file CKalmanFilterCapable.h.

◆ KFArray_FEAT

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_FEAT = mrpt::math::CVectorFixed<double , FEAT_SIZE>
inherited

Definition at line 259 of file CKalmanFilterCapable.h.

◆ KFArray_OBS

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_OBS = mrpt::math::CVectorFixed<double , OBS_SIZE>
inherited

Definition at line 257 of file CKalmanFilterCapable.h.

◆ KFArray_VEH

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_VEH = mrpt::math::CVectorFixed<double , VEH_SIZE>
inherited

Definition at line 255 of file CKalmanFilterCapable.h.

◆ KFCLASS

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFCLASS = CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >
inherited

My class, in a shorter name!

Definition at line 237 of file CKalmanFilterCapable.h.

◆ KFMatrix

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix = mrpt::math::CMatrixDynamic<double >
inherited

Definition at line 241 of file CKalmanFilterCapable.h.

◆ KFMatrix_AxA

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_AxA = mrpt::math::CMatrixFixed<double , ACT_SIZE, ACT_SIZE>
inherited

Definition at line 246 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxF

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxF = mrpt::math::CMatrixFixed<double , FEAT_SIZE, FEAT_SIZE>
inherited

Definition at line 245 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxO

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxO = mrpt::math::CMatrixFixed<double , FEAT_SIZE, OBS_SIZE>
inherited

Definition at line 251 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxV

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxV = mrpt::math::CMatrixFixed<double , FEAT_SIZE, VEH_SIZE>
inherited

Definition at line 250 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxF

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxF = mrpt::math::CMatrixFixed<double , OBS_SIZE, FEAT_SIZE>
inherited

Definition at line 252 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxO

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxO = mrpt::math::CMatrixFixed<double , OBS_SIZE, OBS_SIZE>
inherited

Definition at line 244 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxV

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxV = mrpt::math::CMatrixFixed<double , OBS_SIZE, VEH_SIZE>
inherited

Definition at line 253 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxF

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxF = mrpt::math::CMatrixFixed<double , VEH_SIZE, FEAT_SIZE>
inherited

Definition at line 249 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxO

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxO = mrpt::math::CMatrixFixed<double , VEH_SIZE, OBS_SIZE>
inherited

Definition at line 248 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxV

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxV = mrpt::math::CMatrixFixed<double , VEH_SIZE, VEH_SIZE>
inherited

Definition at line 243 of file CKalmanFilterCapable.h.

◆ kftype

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::kftype = double
inherited

The numeric type used in the Kalman Filter (default=double)

Definition at line 234 of file CKalmanFilterCapable.h.

◆ KFVector

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFVector = mrpt::math::CVectorDynamic<double >
inherited

Definition at line 240 of file CKalmanFilterCapable.h.

◆ landmark_point_t

Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.

Definition at line 56 of file CRangeBearingKFSLAM.h.

◆ vector_KFArray_OBS

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::vector_KFArray_OBS = std::vector<KFArray_OBS>
inherited

Definition at line 258 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

◆ CRangeBearingKFSLAM()

CRangeBearingKFSLAM::CRangeBearingKFSLAM ( )

Constructor.

Definition at line 45 of file CRangeBearingKFSLAM.cpp.

References reset().

Here is the call graph for this function:

◆ ~CRangeBearingKFSLAM()

CRangeBearingKFSLAM::~CRangeBearingKFSLAM ( )
overridedefault

Destructor:

Member Function Documentation

◆ computeOffDiagonalBlocksApproximationError()

double CRangeBearingKFSLAM::computeOffDiagonalBlocksApproximationError ( const std::vector< std::vector< uint32_t >> &  landmarksMembership) const

Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.

See also
getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps

Definition at line 1169 of file CRangeBearingKFSLAM.cpp.

References mrpt::math::MatrixVectorBase< Scalar, Derived >::array(), ASSERT_, mrpt::math::countCommonElements(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, MRPT_END, MRPT_START, and mrpt::math::sum().

Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ dumpLogToConsole()

void COutputLogger::dumpLogToConsole ( ) const
inherited

Dump the current contents of the COutputLogger instance in the terminal window.

See also
writeToFile

Definition at line 190 of file COutputLogger.cpp.

◆ get_action_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_action_size ( )
inlinestaticinherited

Definition at line 227 of file CKalmanFilterCapable.h.

◆ get_feature_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_feature_size ( )
inlinestaticinherited

◆ get_observation_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_observation_size ( )
inlinestaticinherited

Definition at line 225 of file CKalmanFilterCapable.h.

Referenced by OnGetObservationsAndDataAssociation().

◆ get_vehicle_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_vehicle_size ( )
inlinestaticinherited

◆ getAs3DObject()

void CRangeBearingKFSLAM::getAs3DObject ( mrpt::opengl::CSetOfObjects::Ptr outObj) const

◆ getCurrentRobotPose() [1/2]

void CRangeBearingKFSLAM::getCurrentRobotPose ( mrpt::poses::CPose3DQuatPDFGaussian out_robotPose) const

Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion).

See also
getCurrentState, getCurrentRobotPoseMean

Definition at line 91 of file CRangeBearingKFSLAM.cpp.

References ASSERT_, mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::poses::CPose3DQuat::m_coords, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::poses::CPose3DQuatPDFGaussian::mean, MRPT_END, MRPT_START, and mrpt::math::CVectorDynamic< T >::size().

Referenced by getCurrentRobotPose(), and processActionObservation().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getCurrentRobotPose() [2/2]

void mrpt::slam::CRangeBearingKFSLAM::getCurrentRobotPose ( mrpt::poses::CPose3DPDFGaussian out_robotPose) const
inline

Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles).

See also
getCurrentState

Definition at line 142 of file CRangeBearingKFSLAM.h.

References getCurrentRobotPose(), and mrpt::math::UNINITIALIZED_QUATERNION.

Here is the call graph for this function:

◆ getCurrentRobotPoseMean()

mrpt::poses::CPose3DQuat CRangeBearingKFSLAM::getCurrentRobotPoseMean ( ) const

Get the current robot pose mean, as a 3D+quaternion pose.

See also
getCurrentRobotPose

Definition at line 116 of file CRangeBearingKFSLAM.cpp.

References ASSERTDEB_, mrpt::poses::CPose3DQuat::m_coords, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::math::CVectorDynamic< T >::size(), and mrpt::math::UNINITIALIZED_QUATERNION.

Referenced by OnInverseObservationModel(), OnObservationJacobians(), OnObservationModel(), OnTransitionJacobian(), OnTransitionModel(), and OnTransitionNoise().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getCurrentState() [1/2]

void CRangeBearingKFSLAM::getCurrentState ( mrpt::poses::CPose3DQuatPDFGaussian out_robotPose,
std::vector< mrpt::math::TPoint3D > &  out_landmarksPositions,
std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &  out_landmarkIDs,
mrpt::math::CVectorDouble out_fullState,
mrpt::math::CMatrixDouble out_fullCovariance 
) const

Returns the complete mean and cov.

Parameters
out_robotPoseThe mean and the 7x7 covariance matrix of the robot 6D pose
out_landmarksPositionsOne entry for each of the M landmark positions (3D).
out_landmarkIDsEach element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullStateThe complete state vector (7+3M).
out_fullCovarianceThe full (7+3M)x(7+3M) covariance matrix of the filter.
See also
getCurrentRobotPose

Definition at line 135 of file CRangeBearingKFSLAM.cpp.

References ASSERT_, mrpt::math::CVectorDynamic< T >::begin(), mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::math::CVectorDynamic< T >::end(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::containers::bimap< KEY, VALUE >::getInverseMap(), mrpt::poses::CPose3DQuat::m_coords, m_IDs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::poses::CPose3DQuatPDFGaussian::mean, MRPT_END, MRPT_START, mrpt::math::CVectorDynamic< T >::resize(), and mrpt::math::CVectorDynamic< T >::size().

Referenced by getCurrentState().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getCurrentState() [2/2]

void mrpt::slam::CRangeBearingKFSLAM::getCurrentState ( mrpt::poses::CPose3DPDFGaussian out_robotPose,
std::vector< mrpt::math::TPoint3D > &  out_landmarksPositions,
std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &  out_landmarkIDs,
mrpt::math::CVectorDouble out_fullState,
mrpt::math::CMatrixDouble out_fullCovariance 
) const
inline

Returns the complete mean and cov.

Parameters
out_robotPoseThe mean and the 7x7 covariance matrix of the robot 6D pose
out_landmarksPositionsOne entry for each of the M landmark positions (3D).
out_landmarkIDsEach element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullStateThe complete state vector (7+3M).
out_fullCovarianceThe full (7+3M)x(7+3M) covariance matrix of the filter.
See also
getCurrentRobotPose

Definition at line 110 of file CRangeBearingKFSLAM.h.

References getCurrentState(), and mrpt::math::UNINITIALIZED_QUATERNION.

Here is the call graph for this function:

◆ getIncrementFromOdometry()

CPose3DQuat CRangeBearingKFSLAM::getIncrementFromOdometry ( ) const
protected

Return the last odometry, as a pose increment.

Definition at line 264 of file CRangeBearingKFSLAM.cpp.

References mrpt::slam::CRangeBearingKFSLAM::TOptions::force_ignore_odometry, m_action, and options.

Referenced by OnGetAction(), and OnTransitionJacobian().

Here is the caller graph for this function:

◆ getLandmarkCov()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getLandmarkCov ( size_t  idx,
KFMatrix_FxF feat_cov 
) const
inlineinherited

Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).

Exceptions
std::exceptionOn idx>= getNumberOfLandmarksInTheMap()

Definition at line 279 of file CKalmanFilterCapable.h.

References mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), and mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_pkk.

◆ getLandmarkMean()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getLandmarkMean ( size_t  idx,
KFArray_FEAT feat 
) const
inlineinherited

◆ getLastDataAssociation()

const TDataAssocInfo& mrpt::slam::CRangeBearingKFSLAM::getLastDataAssociation ( ) const
inline

Returns a read-only reference to the information on the last data-association.

Definition at line 251 of file CRangeBearingKFSLAM.h.

References m_last_data_association.

◆ getLastPartition()

void mrpt::slam::CRangeBearingKFSLAM::getLastPartition ( std::vector< std::vector< uint32_t >> &  parts)
inline

Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only if options.doPartitioningExperiment = true.

See also
getLastPartitionLandmarks

Definition at line 261 of file CRangeBearingKFSLAM.h.

References m_lastPartitionSet.

Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().

Here is the caller graph for this function:

◆ getLastPartitionLandmarks()

void CRangeBearingKFSLAM::getLastPartitionLandmarks ( std::vector< std::vector< uint32_t >> &  landmarksMembership) const

Return the partitioning of the landmarks in clusters accoring to the last partition.

Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks) Only if options.doPartitioningExperiment = true

Parameters
landmarksMembershipThe i'th element of this vector is the set of clusters to which the i'th landmark in the map belongs to (landmark index != landmark ID !!).
See also
getLastPartition

Definition at line 1113 of file CRangeBearingKFSLAM.cpp.

References mrpt::slam::CRangeBearingKFSLAM::TOptions::doPartitioningExperiment, mrpt::maps::CSimpleMap::get(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getNumberOfLandmarksInTheMap(), mrpt::containers::bimap< KEY, VALUE >::inverse(), m_IDs, m_lastPartitionSet, m_SFs, and options.

Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment(), and getLastPartitionLandmarksAsIfFixedSubmaps().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getLastPartitionLandmarksAsIfFixedSubmaps()

void CRangeBearingKFSLAM::getLastPartitionLandmarksAsIfFixedSubmaps ( size_t  K,
std::vector< std::vector< uint32_t >> &  landmarksMembership 
)

For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used.

Definition at line 1078 of file CRangeBearingKFSLAM.cpp.

References getLastPartitionLandmarks(), m_lastPartitionSet, m_SFs, and mrpt::maps::CSimpleMap::size().

Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getLogAsString() [1/2]

void COutputLogger::getLogAsString ( std::string &  log_contents) const
inherited

Fill the provided string with the contents of the logger's history in std::string representation.

Definition at line 154 of file COutputLogger.cpp.

◆ getLogAsString() [2/2]

std::string COutputLogger::getLogAsString ( ) const
inherited

Get the history of COutputLogger instance in a string representation.

Definition at line 159 of file COutputLogger.cpp.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().

Here is the caller graph for this function:

◆ getLoggerLastMsg() [1/2]

std::string COutputLogger::getLoggerLastMsg ( ) const
inherited

Return the last Tmsg instance registered in the logger history.

Definition at line 195 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::getAsString().

Here is the call graph for this function:

◆ getLoggerLastMsg() [2/2]

void COutputLogger::getLoggerLastMsg ( std::string &  msg_str) const
inherited

Fill inputtted string with the contents of the last message in history.

Definition at line 201 of file COutputLogger.cpp.

◆ getLoggerName()

std::string COutputLogger::getLoggerName ( ) const
inherited

Return the name of the COutputLogger instance.

See also
setLoggerName

Definition at line 143 of file COutputLogger.cpp.

◆ getMinLoggingLevel()

VerbosityLevel mrpt::system::COutputLogger::getMinLoggingLevel ( ) const
inlineinherited

◆ getNumberOfLandmarksInTheMap()

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getNumberOfLandmarksInTheMap ( ) const
inlineinherited

◆ getProfiler()

mrpt::system::CTimeLogger& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getProfiler ( )
inlineinherited

◆ getStateVectorLength()

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getStateVectorLength ( ) const
inlineinherited

◆ internal_getPkk()

KFMatrix& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::internal_getPkk ( )
inlineinherited

◆ internal_getXkk()

KFVector& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::internal_getXkk ( )
inlineinherited

◆ isLoggingLevelVisible()

bool mrpt::system::COutputLogger::isLoggingLevelVisible ( VerbosityLevel  level) const
inlineinherited

Definition at line 202 of file system/COutputLogger.h.

References mrpt::system::COutputLogger::m_min_verbosity_level.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().

Here is the caller graph for this function:

◆ isMapEmpty()

bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::isMapEmpty ( ) const
inlineinherited

Definition at line 232 of file CKalmanFilterCapable.h.

References mrpt::bayes::detail::isMapEmpty().

◆ loadOptions()

void CRangeBearingKFSLAM::loadOptions ( const mrpt::config::CConfigFileBase ini)

Load options from a ini-like file/text.

Definition at line 733 of file CRangeBearingKFSLAM.cpp.

References mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::KF_options, mrpt::bayes::TKF_options::loadFromConfigFile(), mrpt::slam::CRangeBearingKFSLAM::TOptions::loadFromConfigFile(), mapPartitioner, mrpt::slam::CIncrementalMapPartitioner::options, and options.

Here is the call graph for this function:

◆ logCond()

void COutputLogger::logCond ( const VerbosityLevel  level,
bool  cond,
const std::string &  msg_str 
) const
inherited

Log the given message only if the condition is satisfied.

See also
log, logFmt

Definition at line 131 of file COutputLogger.cpp.

◆ logDeregisterCallback()

bool COutputLogger::logDeregisterCallback ( output_logger_callback_t  userFunc)
inherited
Returns
true if an entry was found and deleted.

Definition at line 291 of file COutputLogger.cpp.

References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.

Here is the call graph for this function:

◆ logFmt()

void COutputLogger::logFmt ( const VerbosityLevel  level,
const char *  fmt,
  ... 
) const
inherited

◆ loggerReset()

void COutputLogger::loggerReset ( )
inherited

Reset the contents of the logger instance.

Called upon construction.

Definition at line 206 of file COutputLogger.cpp.

References mrpt::system::LVL_INFO.

◆ logging_levels_to_colors()

std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_colors ( )
staticinherited

Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.

Handy for coloring the input based on the verbosity of the message

Definition at line 47 of file COutputLogger.cpp.

References logging_levels_to_colors.

Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().

Here is the caller graph for this function:

◆ logging_levels_to_names()

std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_names ( )
staticinherited

Map from VerbosityLevels to their corresponding names.

Handy for printing the current message VerbosityLevel along with the actual content

Definition at line 60 of file COutputLogger.cpp.

References logging_levels_to_names.

Referenced by mrpt::system::COutputLogger::TMsg::getAsString().

Here is the caller graph for this function:

◆ logRegisterCallback()

void COutputLogger::logRegisterCallback ( output_logger_callback_t  userFunc)
inherited

Definition at line 278 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::m_listCallbacks.

◆ logStr()

void COutputLogger::logStr ( const VerbosityLevel  level,
std::string_view  msg_str 
) const
inherited

Main method to add the specified message string to the logger.

See also
logCond, logFmt

Definition at line 72 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.

Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ mapPartitionOptions()

CIncrementalMapPartitioner::TOptions* mrpt::slam::CRangeBearingKFSLAM::mapPartitionOptions ( )
inline

Provides access to the parameters of the map partitioning algorithm.

Definition at line 304 of file CRangeBearingKFSLAM.h.

References mapPartitioner, and mrpt::slam::CIncrementalMapPartitioner::options.

Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().

Here is the caller graph for this function:

◆ OnGetAction() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetAction ( KFArray_ACT out_u) const
protectedpure virtualinherited

Must return the action vector u.

Parameters
out_uThe action vector which will be passed to OnTransitionModel

◆ OnGetAction() [2/2]

void CRangeBearingKFSLAM::OnGetAction ( KFArray_ACT u) const
overrideprotected

Must return the action vector u.

Parameters
out_uThe action vector which will be passed to OnTransitionModel

Definition at line 289 of file CRangeBearingKFSLAM.cpp.

References getIncrementFromOdometry().

Here is the call graph for this function:

◆ OnGetObservationNoise() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetObservationNoise ( KFMatrix_OxO out_R) const
protectedpure virtualinherited

Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.

Parameters
out_RThe noise covariance matrix. It might be non diagonal, but it'll usually be.
Note
Upon call, it can be assumed that the previous contents of out_R are all zeros.

◆ OnGetObservationNoise() [2/2]

void CRangeBearingKFSLAM::OnGetObservationNoise ( KFMatrix_OxO out_R) const
overrideprotected

Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.

Parameters
out_RThe noise covariance matrix. It might be non diagonal, but it'll usually be.

Definition at line 1337 of file CRangeBearingKFSLAM.cpp.

References options, mrpt::square(), mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_pitch, mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_yaw.

Here is the call graph for this function:

◆ OnGetObservationsAndDataAssociation() [1/2]

void CRangeBearingKFSLAM::OnGetObservationsAndDataAssociation ( vector_KFArray_OBS Z,
std::vector< int > &  data_association,
const vector_KFArray_OBS all_predictions,
const KFMatrix S,
const std::vector< size_t > &  lm_indices_in_S,
const KFMatrix_OxO R 
)
overrideprotected

This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.

Parameters
out_zN vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_data_associationAn empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
in_SThe full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix with M=length of "in_lm_indices_in_S".
in_lm_indices_in_SThe indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.

This method will be called just once for each complete KF iteration.

Note
It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
Parameters
out_zN vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_data_associationAn empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
in_SThe full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a MĀ·O x MĀ·O matrix with M=length of "in_lm_indices_in_S".
in_lm_indices_in_SThe indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.

This method will be called just once for each complete KF iteration.

Note
It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.

Definition at line 567 of file CRangeBearingKFSLAM.cpp.

References ASSERTMSG_, mrpt::slam::TDataAssociationResults::associations, mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::clear(), mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_IC_chi2_thres, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_IC_metric, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_IC_ml_threshold, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_method, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_metric, mrpt::slam::data_association_full_covariance(), mrpt::containers::bimap< KEY, VALUE >::end(), mrpt::containers::bimap< KEY, VALUE >::find_key(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_observation_size(), m_IDs, m_last_data_association, m_SF, MRPT_END, MRPT_START, options, mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::predictions_IDs, mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::results, mrpt::math::CMatrixDynamic< T >::setSize(), mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::Y_pred_covs, and mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::Y_pred_means.

Here is the call graph for this function:

◆ OnGetObservationsAndDataAssociation() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetObservationsAndDataAssociation ( vector_KFArray_OBS out_z,
std::vector< int > &  out_data_association,
const vector_KFArray_OBS in_all_predictions,
const KFMatrix in_S,
const std::vector< size_t > &  in_lm_indices_in_S,
const KFMatrix_OxO in_R 
)
protectedpure virtualinherited

This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.

Parameters
out_zN vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_data_associationAn empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
in_all_predictionsA vector with the prediction of ALL the landmarks in the map. Note that, in contrast, in_S only comprises a subset of all the landmarks.
in_SThe full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix with M=length of "in_lm_indices_in_S".
in_lm_indices_in_SThe indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.

This method will be called just once for each complete KF iteration.

Note
It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.

◆ OnInverseObservationModel() [1/3]

void CRangeBearingKFSLAM::OnInverseObservationModel ( const KFArray_OBS in_z,
KFArray_FEAT out_yn,
KFMatrix_FxV out_dyn_dxv,
KFMatrix_FxO out_dyn_dhn 
) const
overrideprotected

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters
in_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n} $.
  • O: OBS_SIZE
  • V: VEH_SIZE
  • F: FEAT_SIZE
Note
OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.

Definition at line 831 of file CRangeBearingKFSLAM.cpp.

References ASSERTMSG_, mrpt::poses::CPose3DQuat::composePoint(), getCurrentRobotPoseMean(), m_SF, MRPT_END, MRPT_START, mrpt::math::UNINITIALIZED_MATRIX, mrpt::math::UNINITIALIZED_QUATERNION, mrpt::math::TPoint3D_data< T >::x, mrpt::math::TPoint3D_data< T >::y, and mrpt::math::TPoint3D_data< T >::z.

Here is the call graph for this function:

◆ OnInverseObservationModel() [2/3]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnInverseObservationModel ( [[maybe_unused] ] const KFArray_OBS in_z,
[[maybe_unused] ] KFArray_FEAT out_yn,
[[maybe_unused] ] KFMatrix_FxV out_dyn_dxv,
[[maybe_unused] ] KFMatrix_FxO out_dyn_dhn 
) const
inlinevirtualinherited

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters
in_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n} $.
  • O: OBS_SIZE
  • V: VEH_SIZE
  • F: FEAT_SIZE
Note
OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
Deprecated:
This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.

Definition at line 489 of file CKalmanFilterCapable.h.

References MRPT_END, MRPT_START, and THROW_EXCEPTION.

◆ OnInverseObservationModel() [3/3]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnInverseObservationModel ( const KFArray_OBS in_z,
KFArray_FEAT out_yn,
KFMatrix_FxV out_dyn_dxv,
KFMatrix_FxO out_dyn_dhn,
[[maybe_unused] ] KFMatrix_FxF out_dyn_dhn_R_dyn_dhnT,
bool &  out_use_dyn_dhn_jacobian 
) const
inlinevirtualinherited

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian), and another from the uncertainty in the observation itself. By default, out_use_dyn_dhn_jacobian=true on call, and if it's left at "true", the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn. Only in some problems (e.g. MonoSLAM), it'll be needed for the application to directly return the covariance matrix out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:

$ \frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top $.

but may be computed from additional terms, or whatever needed by the user.

Parameters
in_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n} $.
out_dyn_dhn_R_dyn_dhnTSee the discussion above.
  • O: OBS_SIZE
  • V: VEH_SIZE
  • F: FEAT_SIZE
Note
OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.

Definition at line 540 of file CKalmanFilterCapable.h.

References MRPT_END, MRPT_START, and mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnInverseObservationModel().

◆ OnNewLandmarkAddedToMap() [1/2]

void CRangeBearingKFSLAM::OnNewLandmarkAddedToMap ( const size_t  in_obsIdx,
const size_t  in_idxNewFeat 
)
overrideprotected

If applicable to the given problem, do here any special handling of adding a new landmark to the map.

Parameters
in_obsIndexThe index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
in_idxNewFeatThe index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
See also
OnInverseObservationModel

Definition at line 932 of file CRangeBearingKFSLAM.cpp.

References ASSERT_, ASSERTMSG_, mrpt::containers::bimap< KEY, VALUE >::insert(), m_IDs, m_SF, MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ OnNewLandmarkAddedToMap() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnNewLandmarkAddedToMap ( [[maybe_unused] ] const size_t  in_obsIdx,
[[maybe_unused] ] const size_t  in_idxNewFeat 
)
inlinevirtualinherited

If applicable to the given problem, do here any special handling of adding a new landmark to the map.

Parameters
in_obsIndexThe index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
in_idxNewFeatThe index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
See also
OnInverseObservationModel

Definition at line 562 of file CKalmanFilterCapable.h.

◆ OnNormalizeStateVector()

void CRangeBearingKFSLAM::OnNormalizeStateVector ( )
overrideprotectedvirtual

This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.

This virtual function musts normalize the state vector and covariance matrix (only if its necessary).

Reimplemented from mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >.

Definition at line 711 of file CRangeBearingKFSLAM.cpp.

References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, and mrpt::square().

Here is the call graph for this function:

◆ OnObservationJacobians() [1/2]

void CRangeBearingKFSLAM::OnObservationJacobians ( size_t  idx_landmark_to_predict,
KFMatrix_OxV Hx,
KFMatrix_OxF Hy 
) const
overrideprotected

Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Parameters
idx_landmark_to_predictThe index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.

Definition at line 489 of file CRangeBearingKFSLAM.cpp.

References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), m_SF, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, mrpt::poses::CPose3DQuat::sphericalCoordinates(), mrpt::math::UNINITIALIZED_MATRIX, and mrpt::math::UNINITIALIZED_QUATERNION.

Here is the call graph for this function:

◆ OnObservationJacobians() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobians ( [[maybe_unused] ] size_t  idx_landmark_to_predict,
[[maybe_unused] ] KFMatrix_OxV Hx,
[[maybe_unused] ] KFMatrix_OxF Hy 
) const
inlineprotectedvirtualinherited

Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Parameters
idx_landmark_to_predictThe index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.

Definition at line 437 of file CKalmanFilterCapable.h.

References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_user_didnt_implement_jacobian.

◆ OnObservationJacobiansNumericGetIncrements()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobiansNumericGetIncrements ( KFArray_VEH out_veh_increments,
KFArray_FEAT out_feat_increments 
) const
inlineprotectedvirtualinherited

Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.

Definition at line 449 of file CKalmanFilterCapable.h.

◆ OnObservationModel() [1/2]

void CRangeBearingKFSLAM::OnObservationModel ( const std::vector< size_t > &  idx_landmarks_to_predict,
vector_KFArray_OBS out_predictions 
) const
overrideprotected

◆ OnObservationModel() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationModel ( const std::vector< size_t > &  idx_landmarks_to_predict,
vector_KFArray_OBS out_predictions 
) const
protectedpure virtualinherited

Implements the observation prediction $ h_i(x) $.

Parameters
idx_landmark_to_predictThe indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem.
out_predictionsThe predicted observations.

◆ OnPostIteration()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPostIteration ( )
inlinevirtualinherited

This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().

Definition at line 581 of file CKalmanFilterCapable.h.

◆ OnPreComputingPredictions() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPreComputingPredictions ( [[maybe_unused] ] const vector_KFArray_OBS in_all_prediction_means,
std::vector< size_t > &  out_LM_indices_to_predict 
) const
inlineprotectedvirtualinherited

This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.

For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.

Parameters
in_all_prediction_meansThe mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.
out_LM_indices_to_predictThe list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
Note
This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
See also
OnGetObservations, OnDataAssociation

Definition at line 366 of file CKalmanFilterCapable.h.

References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getNumberOfLandmarksInTheMap().

◆ OnPreComputingPredictions() [2/2]

void CRangeBearingKFSLAM::OnPreComputingPredictions ( const vector_KFArray_OBS prediction_means,
std::vector< size_t > &  out_LM_indices_to_predict 
) const
overrideprotected

This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.

For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.

Parameters
in_all_prediction_meansThe mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.
out_LM_indices_to_predictThe list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
Note
This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
See also
OnGetObservations, OnDataAssociation

Definition at line 1359 of file CRangeBearingKFSLAM.cpp.

References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SF, options, mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_pitch, mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_yaw.

◆ OnSubstractObservationVectors() [1/2]

void CRangeBearingKFSLAM::OnSubstractObservationVectors ( KFArray_OBS A,
const KFArray_OBS B 
) const
overrideprotected

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

Definition at line 1324 of file CRangeBearingKFSLAM.cpp.

References mrpt::math::wrapToPiInPlace().

Here is the call graph for this function:

◆ OnSubstractObservationVectors() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnSubstractObservationVectors ( KFArray_OBS A,
const KFArray_OBS B 
) const
inlineprotectedvirtualinherited

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

Definition at line 460 of file CKalmanFilterCapable.h.

◆ OnTransitionJacobian() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionJacobian ( [ [maybe_unused] ] KFMatrix_VxV out_F) const
inlineprotectedvirtualinherited

Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.

Parameters
out_FMust return the Jacobian. The returned matrix must be $V \times V$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).

Definition at line 328 of file CKalmanFilterCapable.h.

References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_user_didnt_implement_jacobian.

◆ OnTransitionJacobian() [2/2]

void CRangeBearingKFSLAM::OnTransitionJacobian ( KFMatrix_VxV out_F) const
overrideprotected

Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.

This virtual function musts calculate the Jacobian F of the prediction model.

Parameters
out_FMust return the Jacobian. The returned matrix must be $V \times V$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).

Definition at line 341 of file CRangeBearingKFSLAM.cpp.

References getCurrentRobotPoseMean(), getIncrementFromOdometry(), MRPT_END, MRPT_START, and mrpt::math::UNINITIALIZED_MATRIX.

Here is the call graph for this function:

◆ OnTransitionJacobianNumericGetIncrements()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionJacobianNumericGetIncrements ( KFArray_VEH out_increments) const
inlineprotectedvirtualinherited

Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.

Definition at line 338 of file CKalmanFilterCapable.h.

◆ OnTransitionModel() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionModel ( const KFArray_ACT in_u,
KFArray_VEH inout_x,
bool &  out_skipPrediction 
) const
protectedpure virtualinherited

Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

Parameters
in_uThe vector returned by OnGetAction.
inout_xAt input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .
out_skipSet this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false
Note
Even if you return "out_skip=true", the value of "inout_x" MUST be updated as usual (this is to allow numeric approximation of Jacobians).

◆ OnTransitionModel() [2/2]

void CRangeBearingKFSLAM::OnTransitionModel ( const KFArray_ACT in_u,
KFArray_VEH inout_x,
bool &  out_skipPrediction 
) const
overrideprotected

Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

This virtual function musts implement the prediction model of the Kalman filter.

Parameters
in_uThe vector returned by OnGetAction.
inout_xAt input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .
out_skipSet this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false

Definition at line 302 of file CRangeBearingKFSLAM.cpp.

References mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), mrpt::poses::CPose3DQuat::m_coords, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, mrpt::math::CVectorDynamic< T >::size(), and mrpt::math::UNINITIALIZED_QUATERNION.

Here is the call graph for this function:

◆ OnTransitionNoise() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionNoise ( KFMatrix_VxV out_Q) const
protectedpure virtualinherited

Implements the transition noise covariance $ Q_k $.

Parameters
out_QMust return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

◆ OnTransitionNoise() [2/2]

void CRangeBearingKFSLAM::OnTransitionNoise ( KFMatrix_VxV out_Q) const
overrideprotected

Implements the transition noise covariance $ Q_k $.

This virtual function musts calculate de noise matrix of the prediction model.

Parameters
out_QMust return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

Definition at line 366 of file CRangeBearingKFSLAM.cpp.

References ASSERT_, mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference(), mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::slam::CRangeBearingKFSLAM::TOptions::force_ignore_odometry, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), m_action, MRPT_END, MRPT_START, options, mrpt::math::CVectorDynamic< T >::size(), mrpt::square(), mrpt::slam::CRangeBearingKFSLAM::TOptions::std_odo_z_additional, mrpt::slam::CRangeBearingKFSLAM::TOptions::stds_Q_no_odo, and THROW_EXCEPTION.

Here is the call graph for this function:

◆ processActionObservation()

void CRangeBearingKFSLAM::processActionObservation ( mrpt::obs::CActionCollection::Ptr action,
mrpt::obs::CSensoryFrame::Ptr SF 
)

◆ reconsiderPartitionsNow()

void CRangeBearingKFSLAM::reconsiderPartitionsNow ( )

The partitioning of the entire map is recomputed again.

Only when options.doPartitioningExperiment = true. This can be used after changing the parameters of the partitioning method. After this method, you can call getLastPartitionLandmarks.

See also
getLastPartitionLandmarks

Definition at line 1217 of file CRangeBearingKFSLAM.cpp.

References m_lastPartitionSet, mapPartitioner, and mrpt::slam::CIncrementalMapPartitioner::updatePartitions().

Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset()

void CRangeBearingKFSLAM::reset ( )

Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).

Definition at line 60 of file CRangeBearingKFSLAM.cpp.

References mrpt::math::MatrixVectorBase< Scalar, Derived >::assign(), mrpt::containers::bimap< KEY, VALUE >::clear(), mrpt::slam::CIncrementalMapPartitioner::clear(), mrpt::maps::CSimpleMap::clear(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), m_action, m_IDs, m_lastPartitionSet, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SF, m_SFs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mapPartitioner, mrpt::slam::CIncrementalMapPartitioner::options, mrpt::math::CMatrixDynamic< T >::setSize(), mrpt::math::MatrixVectorBase< Scalar, Derived >::setZero(), and mrpt::slam::smOBSERVATION_OVERLAP.

Referenced by CRangeBearingKFSLAM().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ runOneKalmanIteration()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::runOneKalmanIteration ( )
protectedinherited

◆ saveMapAndPath2DRepresentationAsMATLABFile()

void CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile ( const std::string &  fil,
float  stdCount = 3.0f,
const std::string &  styleLandmarks = std::string("b"),
const std::string &  stylePath = std::string("r"),
const std::string &  styleRobot = std::string("r") 
) const

◆ setLoggerName()

void COutputLogger::setLoggerName ( const std::string &  name)
inherited

◆ setMinLoggingLevel()

void COutputLogger::setMinLoggingLevel ( const VerbosityLevel  level)
inherited

Set the minimum logging level for which the incoming logs are going to be taken into account.

String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance

Definition at line 144 of file COutputLogger.cpp.

Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().

Here is the caller graph for this function:

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel  level)
inherited

◆ writeLogToFile()

void COutputLogger::writeLogToFile ( const std::string *  fname_in = nullptr) const
inherited

Write the contents of the COutputLogger instance to an external file.

Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided

See also
dumpToConsole, getAsString

Definition at line 165 of file COutputLogger.cpp.

References ASSERTMSG_, and mrpt::format().

Here is the call graph for this function:

Member Data Documentation

◆ KF_options

TKF_options mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KF_options
inherited

Generic options for the Kalman Filter algorithm itself.

Definition at line 598 of file CKalmanFilterCapable.h.

Referenced by loadOptions().

◆ logging_enable_console_output

bool mrpt::system::COutputLogger::logging_enable_console_output {true}
inherited

[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.

Definition at line 240 of file system/COutputLogger.h.

◆ logging_enable_keep_record

bool mrpt::system::COutputLogger::logging_enable_keep_record {false}
inherited

[Default=false] Enables storing all messages into an internal list.

See also
writeLogToFile, getLogAsString

Definition at line 243 of file system/COutputLogger.h.

◆ m_action

mrpt::obs::CActionCollection::Ptr mrpt::slam::CRangeBearingKFSLAM::m_action
protected

Set up by processActionObservation.

Definition at line 480 of file CRangeBearingKFSLAM.h.

Referenced by getIncrementFromOdometry(), OnTransitionNoise(), processActionObservation(), and reset().

◆ m_IDs

mrpt::containers::bimap<mrpt::maps::CLandmark::TLandmarkID, unsigned int> mrpt::slam::CRangeBearingKFSLAM::m_IDs
protected

The mapping between landmark IDs and indexes in the Pkk cov.

matrix:

Definition at line 487 of file CRangeBearingKFSLAM.h.

Referenced by getAs3DObject(), getCurrentState(), getLastPartitionLandmarks(), OnGetObservationsAndDataAssociation(), OnNewLandmarkAddedToMap(), processActionObservation(), and reset().

◆ m_last_data_association

TDataAssocInfo mrpt::slam::CRangeBearingKFSLAM::m_last_data_association
protected

Last data association.

Definition at line 500 of file CRangeBearingKFSLAM.h.

Referenced by getLastDataAssociation(), and OnGetObservationsAndDataAssociation().

◆ m_lastPartitionSet

std::vector<std::vector<uint32_t> > mrpt::slam::CRangeBearingKFSLAM::m_lastPartitionSet
protected

◆ m_min_verbosity_level

VerbosityLevel mrpt::system::COutputLogger::m_min_verbosity_level {LVL_INFO}
protectedinherited

Provided messages with VerbosityLevel smaller than this value shall be ignored.

Definition at line 253 of file system/COutputLogger.h.

Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().

◆ m_pkk

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_pkk
protectedinherited

◆ m_SF

mrpt::obs::CSensoryFrame::Ptr mrpt::slam::CRangeBearingKFSLAM::m_SF
protected

◆ m_SFs

mrpt::maps::CSimpleMap mrpt::slam::CRangeBearingKFSLAM::m_SFs
protected

The sequence of all the observations and the robot path (kept for debugging, statistics,etc)

Definition at line 495 of file CRangeBearingKFSLAM.h.

Referenced by getAs3DObject(), getLastPartitionLandmarks(), getLastPartitionLandmarksAsIfFixedSubmaps(), processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ m_timLogger

mrpt::system::CTimeLogger mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_timLogger
protectedinherited

Definition at line 296 of file CKalmanFilterCapable.h.

◆ m_xkk

KFVector mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_xkk
protectedinherited

◆ mapPartitioner

CIncrementalMapPartitioner mrpt::slam::CRangeBearingKFSLAM::mapPartitioner
protected

Used for map partitioning experiments.

Definition at line 490 of file CRangeBearingKFSLAM.h.

Referenced by loadOptions(), mapPartitionOptions(), processActionObservation(), reconsiderPartitionsNow(), and reset().

◆ options

mrpt::slam::CRangeBearingKFSLAM::TOptions mrpt::slam::CRangeBearingKFSLAM::options



Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at miƩ abr 1 00:50:30 CEST 2020