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: http://www.mrpt.org/Application:kf-slam

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

See also: An implementation for 2D only: CRangeBearingKFSLAM2D

Definition at line 50 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
typedef mrpt::math::TPoint3D	landmark_point_t
	Either mrpt::math::TPoint2D or mrpt::math::TPoint3D. More...

typedef double	kftype
	The numeric type used in the Kalman Filter (default=double) More...

typedef CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >	KFCLASS
	My class, in a shorter name! More...

typedef Eigen::Matrix< double, Eigen::Dynamic, 1 >	KFVector

typedef mrpt::math::CMatrixTemplateNumeric< double >	KFMatrix

typedef mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE >	KFMatrix_VxV

typedef mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE >	KFMatrix_OxO

typedef mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, FEAT_SIZE >	KFMatrix_FxF

typedef mrpt::math::CMatrixFixedNumeric< double, ACT_SIZE, ACT_SIZE >	KFMatrix_AxA

typedef mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, OBS_SIZE >	KFMatrix_VxO

typedef mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, FEAT_SIZE >	KFMatrix_VxF

typedef mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE >	KFMatrix_FxV

typedef mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE >	KFMatrix_FxO

typedef mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE >	KFMatrix_OxF

typedef mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE >	KFMatrix_OxV

typedef mrpt::math::CArrayNumeric< double, VEH_SIZE >	KFArray_VEH

typedef mrpt::math::CArrayNumeric< double, ACT_SIZE >	KFArray_ACT

typedef mrpt::math::CArrayNumeric< double, OBS_SIZE >	KFArray_OBS

typedef mrpt::aligned_containers< KFArray_OBS >::vector_t	vector_KFArray_OBS

typedef mrpt::math::CArrayNumeric< double, FEAT_SIZE >	KFArray_FEAT

Public Member Functions
	CRangeBearingKFSLAM ()
	Constructor. More...

virtual	~CRangeBearingKFSLAM ()
	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::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &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::CSetOfObjectsPtr &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::utils::CConfigFileBase &ini)
	Load options from a ini-like file/text. More...

const TDataAssocInfo &	getLastDataAssociation () const
	Returns a read-only reference to the information on the last data-association. More...

void	getLastPartition (std::vector< vector_uint > &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< vector_uint > &landmarksMembership) const
	Return the partitioning of the landmarks in clusters accoring to the last partition. More...

void	getLastPartitionLandmarksAsIfFixedSubmaps (size_t K, std::vector< vector_uint > &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< vector_uint > &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::TOptions *	mapPartitionOptions ()
	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

KFVector &	internal_getXkk ()

KFMatrix &	internal_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::utils::CTimeLogger &	getProfiler ()

Static Public Member Functions
static size_t	get_vehicle_size ()

static size_t	get_observation_size ()

static size_t	get_feature_size ()

static size_t	get_action_size ()

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
	Must return the action vector u. More...

void	OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
	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
	Implements the transition Jacobian $\frac{\partial f}{\partial x}$ . More...

void	OnTransitionNoise (KFMatrix_VxV &out_Q) const
	Implements the transition noise covariance . More...

void	OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const vector_size_t &in_lm_indices_in_S, const KFMatrix_OxO &in_R)
	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 vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const

void	OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, 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...

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...

void	OnGetObservationNoise (KFMatrix_OxO &out_R) const
	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, 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...

void	OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, 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...

void	OnNewLandmarkAddedToMap (const size_t in_obsIdx, 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...

void	OnNormalizeStateVector ()
	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::CActionCollectionPtr	m_action
	Set up by processActionObservation. More...

mrpt::obs::CSensoryFramePtr	m_SF
	Set up by processActionObservation. More...

mrpt::utils::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< vector_uint >	m_lastPartitionSet

TDataAssocInfo	m_last_data_association
	Last data association. More...

mrpt::utils::CTimeLogger	m_timLogger

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 (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, 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, 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	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 (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 . More...

virtual void	OnPreComputingPredictions (const vector_KFArray_OBS &in_all_prediction_means, mrpt::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, mrpt::vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const 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 mrpt::vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const=0
	Implements the observation prediction . More...

virtual void	OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, 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...

Member Typedef Documentation

◆ KFArray_ACT

typedef mrpt::math::CArrayNumeric<double ,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_ACT

inherited

Definition at line 193 of file CKalmanFilterCapable.h.

◆ KFArray_FEAT

typedef mrpt::math::CArrayNumeric<double ,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_FEAT

inherited

Definition at line 196 of file CKalmanFilterCapable.h.

◆ KFArray_OBS

typedef mrpt::math::CArrayNumeric<double ,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_OBS

inherited

Definition at line 194 of file CKalmanFilterCapable.h.

◆ KFArray_VEH

typedef mrpt::math::CArrayNumeric<double ,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_VEH

inherited

Definition at line 192 of file CKalmanFilterCapable.h.

◆ KFCLASS

typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,double > mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFCLASS

inherited

My class, in a shorter name!

Definition at line 172 of file CKalmanFilterCapable.h.

◆ KFMatrix

typedef mrpt::math::CMatrixTemplateNumeric<double > mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix

inherited

Definition at line 176 of file CKalmanFilterCapable.h.

◆ KFMatrix_AxA

typedef mrpt::math::CMatrixFixedNumeric<double ,ACT_SIZE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_AxA

inherited

Definition at line 181 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxF

typedef mrpt::math::CMatrixFixedNumeric<double ,FEAT_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxF

inherited

Definition at line 180 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxO

typedef mrpt::math::CMatrixFixedNumeric<double ,FEAT_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxO

inherited

Definition at line 187 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxV

typedef mrpt::math::CMatrixFixedNumeric<double ,FEAT_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxV

inherited

Definition at line 186 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxF

typedef mrpt::math::CMatrixFixedNumeric<double ,OBS_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxF

inherited

Definition at line 189 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxO

typedef mrpt::math::CMatrixFixedNumeric<double ,OBS_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxO

inherited

Definition at line 179 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxV

typedef mrpt::math::CMatrixFixedNumeric<double ,OBS_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxV

inherited

Definition at line 190 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxF

typedef mrpt::math::CMatrixFixedNumeric<double ,VEH_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxF

inherited

Definition at line 184 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxO

typedef mrpt::math::CMatrixFixedNumeric<double ,VEH_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxO

inherited

Definition at line 183 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxV

typedef mrpt::math::CMatrixFixedNumeric<double ,VEH_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxV

inherited

Definition at line 178 of file CKalmanFilterCapable.h.

◆ kftype

typedef double mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::kftype

inherited

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

Definition at line 171 of file CKalmanFilterCapable.h.

◆ KFVector

typedef Eigen::Matrix<double ,Eigen::Dynamic,1> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFVector

inherited

Definition at line 175 of file CKalmanFilterCapable.h.

◆ landmark_point_t

typedef mrpt::math::TPoint3D mrpt::slam::CRangeBearingKFSLAM::landmark_point_t

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

Definition at line 55 of file CRangeBearingKFSLAM.h.

◆ vector_KFArray_OBS

typedef mrpt::aligned_containers<KFArray_OBS>::vector_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::vector_KFArray_OBS

inherited

Definition at line 195 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

◆ CRangeBearingKFSLAM()

CRangeBearingKFSLAM::CRangeBearingKFSLAM ( )

Constructor.

Definition at line 47 of file CRangeBearingKFSLAM.cpp.

References reset().

◆ ~CRangeBearingKFSLAM()

CRangeBearingKFSLAM::~CRangeBearingKFSLAM ( )

virtual

Destructor:

Definition at line 87 of file CRangeBearingKFSLAM.cpp.

Member Function Documentation

◆ computeOffDiagonalBlocksApproximationError()

double CRangeBearingKFSLAM::computeOffDiagonalBlocksApproximationError ( const std::vector< vector_uint > & 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 1119 of file CRangeBearingKFSLAM.cpp.

References 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().

◆ get_action_size()

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

inlinestaticinherited

Definition at line 166 of file CKalmanFilterCapable.h.

◆ get_feature_size()

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

inlinestaticinherited

Definition at line 165 of file CKalmanFilterCapable.h.

Referenced by computeOffDiagonalBlocksApproximationError(), getAs3DObject(), getCurrentState(), OnObservationJacobians(), OnObservationModel(), processActionObservation(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ get_observation_size()

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

inlinestaticinherited

Definition at line 164 of file CKalmanFilterCapable.h.

Referenced by OnGetObservationsAndDataAssociation().

◆ get_vehicle_size()

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

inlinestaticinherited

Definition at line 163 of file CKalmanFilterCapable.h.

Referenced by computeOffDiagonalBlocksApproximationError(), getAs3DObject(), getCurrentState(), OnObservationJacobians(), OnObservationModel(), OnTransitionModel(), OnTransitionNoise(), processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ getAs3DObject()

void CRangeBearingKFSLAM::getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr & outObj ) const

Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.

Parameters

out_objects

Definition at line 924 of file CRangeBearingKFSLAM.cpp.

References mrpt::opengl::stock_objects::CornerXYZ(), mrpt::poses::CPointPDFGaussian::cov, mrpt::opengl::CEllipsoid::Create(), mrpt::slam::CRangeBearingKFSLAM::TOptions::doPartitioningExperiment, mrpt::format(), mrpt::maps::CSimpleMap::get(), 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 >::getNumberOfLandmarksInTheMap(), mrpt::utils::bimap< KEY, VALUE >::inverse(), m_IDs, m_lastPartitionSet, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SFs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::poses::CPointPDFGaussian::mean, options, mrpt::slam::CRangeBearingKFSLAM::TOptions::quantiles_3D_representation, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

◆ 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 95 of file CRangeBearingKFSLAM.cpp.

References ASSERT_, 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, and MRPT_START.

Referenced by processActionObservation().

◆ 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 123 of file CRangeBearingKFSLAM.h.

References mrpt::math::UNINITIALIZED_QUATERNION.

◆ getCurrentRobotPoseMean()

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

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

See also: getCurrentRobotPose

Definition at line 120 of file CRangeBearingKFSLAM.cpp.

References ASSERTDEB_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, and mrpt::math::UNINITIALIZED_QUATERNION.

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

◆ 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_robotPose	The mean and the 7x7 covariance matrix of the robot 6D pose
out_landmarksPositions	One entry for each of the M landmark positions (3D).
out_landmarkIDs	Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState	The complete state vector (7+3M).
out_fullCovariance	The full (7+3M)x(7+3M) covariance matrix of the filter.

See also: getCurrentRobotPose

Definition at line 140 of file CRangeBearingKFSLAM.cpp.

References ASSERT_, mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::utils::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, and MRPT_START.

◆ 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_robotPose	The mean and the 7x7 covariance matrix of the robot 6D pose
out_landmarksPositions	One entry for each of the M landmark positions (3D).
out_landmarkIDs	Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState	The complete state vector (7+3M).
out_fullCovariance	The full (7+3M)x(7+3M) covariance matrix of the filter.

See also: getCurrentRobotPose

Definition at line 97 of file CRangeBearingKFSLAM.h.

References mrpt::math::UNINITIALIZED_QUATERNION.

◆ getIncrementFromOdometry()

CPose3DQuat CRangeBearingKFSLAM::getIncrementFromOdometry ( ) const

protected

Return the last odometry, as a pose increment.

Definition at line 266 of file CRangeBearingKFSLAM.cpp.

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

Referenced by OnGetAction(), and OnTransitionJacobian().

◆ 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::exception On idx>= getNumberOfLandmarksInTheMap()

Definition at line 213 of file CKalmanFilterCapable.h.

References 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

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

Exceptions

std::exception On idx>= getNumberOfLandmarksInTheMap()

Definition at line 206 of file CKalmanFilterCapable.h.

References ASSERT_, mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getNumberOfLandmarksInTheMap(), mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_xkk, and mrpt::system::os::memcpy().

◆ 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 212 of file CRangeBearingKFSLAM.h.

◆ getLastPartition()

void mrpt::slam::CRangeBearingKFSLAM::getLastPartition ( std::vector< vector_uint > & 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 221 of file CRangeBearingKFSLAM.h.

◆ getLastPartitionLandmarks()

void CRangeBearingKFSLAM::getLastPartitionLandmarks ( std::vector< vector_uint > & 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

landmarksMembership The 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 1065 of file CRangeBearingKFSLAM.cpp.

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

Referenced by getLastPartitionLandmarksAsIfFixedSubmaps().

◆ getLastPartitionLandmarksAsIfFixedSubmaps()

void CRangeBearingKFSLAM::getLastPartitionLandmarksAsIfFixedSubmaps	(	size_t	K,
		std::vector< vector_uint > &	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 1032 of file CRangeBearingKFSLAM.cpp.

References getLastPartitionLandmarks(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::K, m_lastPartitionSet, m_SFs, and mrpt::maps::CSimpleMap::size().

◆ getNumberOfLandmarksInTheMap()

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

inlineinherited

Definition at line 167 of file CKalmanFilterCapable.h.

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

Referenced by getAs3DObject(), getLastPartitionLandmarks(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ getProfiler()

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

inlineinherited

Definition at line 460 of file CKalmanFilterCapable.h.

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

◆ getStateVectorLength()

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

inlineinherited

Definition at line 198 of file CKalmanFilterCapable.h.

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

◆ internal_getPkk()

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

inlineinherited

Definition at line 201 of file CKalmanFilterCapable.h.

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

◆ internal_getXkk()

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

inlineinherited

Definition at line 200 of file CKalmanFilterCapable.h.

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

◆ isMapEmpty()

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

inlineinherited

Definition at line 168 of file CKalmanFilterCapable.h.

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

◆ loadOptions()

void CRangeBearingKFSLAM::loadOptions ( const mrpt::utils::CConfigFileBase & ini )

Load options from a ini-like file/text.

Definition at line 709 of file CRangeBearingKFSLAM.cpp.

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

◆ mapPartitionOptions()

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

inline

Provides access to the parameters of the map partitioning algorithm.

Definition at line 256 of file CRangeBearingKFSLAM.h.

◆ 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_u The action vector which will be passed to OnTransitionModel

◆ OnGetAction() [2/2]

void CRangeBearingKFSLAM::OnGetAction ( KFArray_ACT & u ) const

protected

Must return the action vector u.

Parameters

out_u The action vector which will be passed to OnTransitionModel

Definition at line 289 of file CRangeBearingKFSLAM.cpp.

References getIncrementFromOdometry().

◆ 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_R The 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

protected

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

Parameters

out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.

Definition at line 1270 of file CRangeBearingKFSLAM.cpp.

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

◆ OnGetObservationsAndDataAssociation() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetObservationsAndDataAssociation	(	vector_KFArray_OBS &	out_z,
		mrpt::vector_int &	out_data_association,
		const vector_KFArray_OBS &	in_all_predictions,
		const KFMatrix &	in_S,
		const 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_z	N 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_association	An 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_predictions	A 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_S	The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a MO x MO matrix with M=length of "in_lm_indices_in_S".
in_lm_indices_in_S	The 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.

◆ OnGetObservationsAndDataAssociation() [2/2]

void CRangeBearingKFSLAM::OnGetObservationsAndDataAssociation	(	vector_KFArray_OBS &	Z,
		vector_int &	data_association,
		const vector_KFArray_OBS &	all_predictions,
		const KFMatrix &	S,
		const vector_size_t &	lm_indices_in_S,
		const KFMatrix_OxO &	R
	)

protected

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_z	N 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_association	An 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_S	The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a MO x MO matrix with M=length of "in_lm_indices_in_S".
in_lm_indices_in_S	The 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_z	N 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_association	An 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_S	The 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_S	The 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 549 of file CRangeBearingKFSLAM.cpp.

◆ OnInverseObservationModel() [1/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
	)		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_z	The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_yn	The F-length vector with the inverse observation model .
out_dyn_dxv	The $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $\frac{\partial y_n}{\partial x_v}$ .
out_dyn_dhn	The $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 372 of file CKalmanFilterCapable.h.

References MRPT_END, MRPT_START, MRPT_UNUSED_PARAM, and THROW_EXCEPTION.

◆ OnInverseObservationModel() [2/3]

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

protected

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

Parameters

in_z	The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
out_yn	The F-length vector with the inverse observation model .
out_dyn_dxv	The $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $\frac{\partial y_n}{\partial x_v}$ .
out_dyn_dhn	The $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 799 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::x, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.

◆ 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,
		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_z	The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_yn	The F-length vector with the inverse observation model .
out_dyn_dxv	The $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $\frac{\partial y_n}{\partial x_v}$ .
out_dyn_dhn	The $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_dhnT	See 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 407 of file CKalmanFilterCapable.h.

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

◆ OnNewLandmarkAddedToMap()

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

protectedvirtual

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

Parameters

in_obsIndex	The 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_idxNewFeat	The 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

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

Definition at line 890 of file CRangeBearingKFSLAM.cpp.

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

◆ OnNormalizeStateVector()

void CRangeBearingKFSLAM::OnNormalizeStateVector ( )

protectedvirtual

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 689 of file CRangeBearingKFSLAM.cpp.

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

◆ OnObservationJacobians() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobians	(	const size_t &	idx_landmark_to_predict,
		KFMatrix_OxV &	Hx,
		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_predict	The 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.
Hx	The output Jacobian $\frac{\partial h_i}{\partial x}$ .
Hy	The output Jacobian $\frac{\partial h_i}{\partial y_i}$ .

Definition at line 330 of file CKalmanFilterCapable.h.

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

◆ OnObservationJacobians() [2/2]

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

protected

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

Parameters

idx_landmark_to_predict	The 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.
Hx	The output Jacobian $\frac{\partial h_i}{\partial x}$ .
Hy	The output Jacobian $\frac{\partial h_i}{\partial y_i}$ .

Definition at line 480 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.

◆ 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 342 of file CKalmanFilterCapable.h.

◆ OnObservationModel() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationModel	(	const mrpt::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_predict	The 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_predictions	The predicted observations.

◆ OnObservationModel() [2/2]

void CRangeBearingKFSLAM::OnObservationModel	(	const vector_size_t &	idx_landmarks_to_predict,
		vector_KFArray_OBS &	out_predictions
	)		const

protected

Definition at line 415 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::size(), and mrpt::poses::CPose3DQuat::sphericalCoordinates().

◆ 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 445 of file CKalmanFilterCapable.h.

◆ OnPreComputingPredictions() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPreComputingPredictions	(	const vector_KFArray_OBS &	in_all_prediction_means,
		mrpt::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_means	The 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_predict	The 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 279 of file CKalmanFilterCapable.h.

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

◆ OnPreComputingPredictions() [2/2]

void CRangeBearingKFSLAM::OnPreComputingPredictions	(	const vector_KFArray_OBS &	prediction_means,
		vector_size_t &	out_LM_indices_to_predict
	)		const

protected

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_means	The 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_predict	The 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 1284 of file CRangeBearingKFSLAM.cpp.

References ASSERTMSG_, DEG2RAD, 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

protected

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 1259 of file CRangeBearingKFSLAM.cpp.

References mrpt::math::wrapToPiInPlace().

◆ 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 352 of file CKalmanFilterCapable.h.

◆ OnTransitionJacobian() [1/2]

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

inlineprotectedvirtualinherited

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

Parameters

out_F Must 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 253 of file CKalmanFilterCapable.h.

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

◆ OnTransitionJacobian() [2/2]

void CRangeBearingKFSLAM::OnTransitionJacobian ( KFMatrix_VxV & out_F ) const

protected

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

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

Parameters

out_F Must 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 339 of file CRangeBearingKFSLAM.cpp.

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

◆ 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 261 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_u	The vector returned by OnGetAction.
inout_x	At input has $\hat{x}_{k-1\|k-1}$ , at output must have $\hat{x}_{k\|k-1}$ .
out_skip	Set 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

protected

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_u	The vector returned by OnGetAction.
inout_x	At input has $\hat{x}_{k-1\|k-1}$ , at output must have $\hat{x}_{k\|k-1}$ .
out_skip	Set 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 300 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, and mrpt::math::UNINITIALIZED_QUATERNION.

◆ 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_Q Must 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

protected

Implements the transition noise covariance $ Q_k $ .

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

Parameters

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

Definition at line 363 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::square(), mrpt::slam::CRangeBearingKFSLAM::TOptions::std_odo_z_additional, mrpt::slam::CRangeBearingKFSLAM::TOptions::stds_Q_no_odo, and THROW_EXCEPTION.

◆ processActionObservation()

void CRangeBearingKFSLAM::processActionObservation	(	mrpt::obs::CActionCollectionPtr &	action,
		mrpt::obs::CSensoryFramePtr &	SF
	)

Process one new action and observations to update the map and robot pose estimate.

See the description of the class at the top of this page.

Parameters

action	May contain odometry
SF	The set of observations, must contain at least one CObservationBearingRange

Definition at line 192 of file CRangeBearingKFSLAM.cpp.

References mrpt::slam::CIncrementalMapPartitioner::addMapFrame(), ASSERT_, mrpt::slam::CRangeBearingKFSLAM::TOptions::create_simplemap, mrpt::slam::CRangeBearingKFSLAM::TOptions::doPartitioningExperiment, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPose(), mrpt::maps::CSimpleMap::insert(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::K, m_action, m_IDs, m_lastPartitionSet, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SF, m_SFs, mapPartitioner, MRPT_END, MRPT_START, options, mrpt::slam::CRangeBearingKFSLAM::TOptions::partitioningMethod, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::runOneKalmanIteration(), mrpt::utils::bimap< KEY, VALUE >::size(), mrpt::maps::CSimpleMap::size(), mrpt::math::UNINITIALIZED_QUATERNION, and mrpt::slam::CIncrementalMapPartitioner::updatePartitions().

◆ 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 1160 of file CRangeBearingKFSLAM.cpp.

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

◆ reset()

void CRangeBearingKFSLAM::reset ( void )

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

Definition at line 61 of file CRangeBearingKFSLAM.cpp.

References mrpt::slam::CIncrementalMapPartitioner::clear(), mrpt::utils::bimap< KEY, VALUE >::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, and mrpt::slam::CIncrementalMapPartitioner::TOptions::useMapMatching.

Referenced by CRangeBearingKFSLAM().

◆ runOneKalmanIteration()

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

protectedinherited

The main entry point, executes one complete step: prediction + update.

It is protected since derived classes must provide a problem-specific entry point for users. The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters

Definition at line 22 of file CKalmanFilterCapable_impl.h.

References mrpt::bayes::detail::addNewLandmarks(), ASSERT_, ASSERTDEB_, ASSERTMSG_, mrpt::math::distance(), mrpt::math::estimateJacobian(), mrpt::utils::find_in_vector(), mrpt::mrpt::format(), mrpt::bayes::kfEKFAlaDavison, mrpt::bayes::kfEKFNaive, mrpt::bayes::kfIKF, mrpt::bayes::kfIKFFull, mrpt::system::os::memcpy(), MRPT_END, MRPT_LOG_DEBUG, MRPT_LOG_WARN_STREAM, MRPT_START, R, mrpt::math::sum(), THROW_EXCEPTION, mrpt::math::UNINITIALIZED_MATRIX, and mrpt::system::vectorToTextFile().

Referenced by processActionObservation().

◆ 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

Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D.

Definition at line 1174 of file CRangeBearingKFSLAM.cpp.

References mrpt::math::cov(), mrpt::system::os::fclose(), mrpt::system::os::fopen(), mrpt::system::os::fprintf(), mrpt::maps::CSimpleMap::get(), 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 >::getNumberOfLandmarksInTheMap(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SFs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::math::MATLAB_plotCovariance2D(), mean(), and mrpt::maps::CSimpleMap::size().

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 462 of file CKalmanFilterCapable.h.

Referenced by loadOptions().

◆ m_action

mrpt::obs::CActionCollectionPtr mrpt::slam::CRangeBearingKFSLAM::m_action

protected

Set up by processActionObservation.

Definition at line 396 of file CRangeBearingKFSLAM.h.

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

◆ m_IDs

mrpt::utils::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 402 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 414 of file CRangeBearingKFSLAM.h.

Referenced by OnGetObservationsAndDataAssociation().

◆ m_lastPartitionSet

std::vector<vector_uint> mrpt::slam::CRangeBearingKFSLAM::m_lastPartitionSet

protected

Definition at line 412 of file CRangeBearingKFSLAM.h.

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

◆ m_pkk

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

protectedinherited

The system full covariance matrix.

Definition at line 222 of file CKalmanFilterCapable.h.

Referenced by computeOffDiagonalBlocksApproximationError(), getAs3DObject(), getCurrentRobotPose(), getCurrentState(), OnGetObservationsAndDataAssociation(), OnPreComputingPredictions(), processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ m_SF

mrpt::obs::CSensoryFramePtr mrpt::slam::CRangeBearingKFSLAM::m_SF

protected

Set up by processActionObservation.

Definition at line 399 of file CRangeBearingKFSLAM.h.

Referenced by OnGetObservationsAndDataAssociation(), OnInverseObservationModel(), OnNewLandmarkAddedToMap(), OnObservationJacobians(), OnObservationModel(), OnPreComputingPredictions(), processActionObservation(), and reset().

◆ 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 410 of file CRangeBearingKFSLAM.h.

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

◆ m_timLogger

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

protectedinherited

Definition at line 226 of file CKalmanFilterCapable.h.

◆ m_xkk

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

protectedinherited

The system state vector.

Definition at line 221 of file CKalmanFilterCapable.h.

Referenced by getAs3DObject(), getCurrentRobotPose(), getCurrentRobotPoseMean(), getCurrentState(), OnNormalizeStateVector(), OnObservationJacobians(), OnObservationModel(), OnTransitionModel(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ mapPartitioner

CIncrementalMapPartitioner mrpt::slam::CRangeBearingKFSLAM::mapPartitioner

protected

Used for map partitioning experiments.

Definition at line 406 of file CRangeBearingKFSLAM.h.

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

◆ options

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

Referenced by getAs3DObject(), getIncrementFromOdometry(), getLastPartitionLandmarks(), loadOptions(), OnGetObservationNoise(), OnGetObservationsAndDataAssociation(), OnPreComputingPredictions(), OnTransitionNoise(), and processActionObservation().

Detailed Description

Classes

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Virtual methods for Kalman Filter implementation

Member Typedef Documentation

◆ KFArray_ACT

◆ KFArray_FEAT

◆ KFArray_OBS

◆ KFArray_VEH

◆ KFCLASS

◆ KFMatrix

◆ KFMatrix_AxA

◆ KFMatrix_FxF

◆ KFMatrix_FxO

◆ KFMatrix_FxV

◆ KFMatrix_OxF

◆ KFMatrix_OxO

◆ KFMatrix_OxV

◆ KFMatrix_VxF

◆ KFMatrix_VxO

◆ KFMatrix_VxV

◆ kftype

◆ KFVector

◆ landmark_point_t

◆ vector_KFArray_OBS

Constructor & Destructor Documentation

◆ CRangeBearingKFSLAM()

◆ ~CRangeBearingKFSLAM()

Member Function Documentation

◆ computeOffDiagonalBlocksApproximationError()

◆ get_action_size()

◆ get_feature_size()

◆ get_observation_size()

◆ get_vehicle_size()

◆ getAs3DObject()

◆ getCurrentRobotPose() [1/2]

◆ getCurrentRobotPose() [2/2]

◆ getCurrentRobotPoseMean()

◆ getCurrentState() [1/2]

◆ getCurrentState() [2/2]

◆ getIncrementFromOdometry()

◆ getLandmarkCov()

◆ getLandmarkMean()

◆ getLastDataAssociation()

◆ getLastPartition()

◆ getLastPartitionLandmarks()

◆ getLastPartitionLandmarksAsIfFixedSubmaps()

◆ getNumberOfLandmarksInTheMap()

◆ getProfiler()

◆ getStateVectorLength()

◆ internal_getPkk()

◆ internal_getXkk()

◆ isMapEmpty()

◆ loadOptions()

◆ mapPartitionOptions()

◆ OnGetAction() [1/2]

◆ OnGetAction() [2/2]

◆ OnGetObservationNoise() [1/2]

◆ OnGetObservationNoise() [2/2]

◆ OnGetObservationsAndDataAssociation() [1/2]

◆ OnGetObservationsAndDataAssociation() [2/2]

◆ OnInverseObservationModel() [1/3]

◆ OnInverseObservationModel() [2/3]

◆ OnInverseObservationModel() [3/3]

◆ OnNewLandmarkAddedToMap()

◆ OnNormalizeStateVector()

◆ OnObservationJacobians() [1/2]

◆ OnObservationJacobians() [2/2]

◆ OnObservationJacobiansNumericGetIncrements()

◆ OnObservationModel() [1/2]

◆ OnObservationModel() [2/2]

◆ OnPostIteration()

◆ OnPreComputingPredictions() [1/2]

◆ OnPreComputingPredictions() [2/2]

◆ OnSubstractObservationVectors() [1/2]