An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.
The main method is "processActionObservation" which processes pairs of action/observation.
The following pages describe front-end applications based on this class:
Definition at line 45 of file CRangeBearingKFSLAM2D.h.
#include <mrpt/slam/CRangeBearingKFSLAM2D.h>
Classes | |
struct | TDataAssocInfo |
Information for data-association: More... | |
struct | TOptions |
The options for the algorithm. More... | |
Public Types | |
typedef mrpt::math::TPoint2D | 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 | |
CRangeBearingKFSLAM2D () | |
Default constructor. More... | |
virtual | ~CRangeBearingKFSLAM2D () |
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::CPosePDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint2D > &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::CPosePDFGaussian &out_robotPose) const |
Returns the mean & 3x3 covariance matrix of the robot 2D pose. 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... | |
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... | |
const TDataAssocInfo & | getLastDataAssociation () const |
Returns a read-only reference to the information on the last data-association. 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 | |
TOptions | options |
The options for the algorithm. More... | |
TKF_options | KF_options |
Generic options for the Kalman Filter algorithm itself. More... | |
Protected Member Functions | |
void | getLandmarkIDsFromIndexInStateVector (std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_id2index) const |
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 . More... | |
void | OnTransitionJacobian (KFMatrix_VxV &out_F) const |
Implements the transition Jacobian . More... | |
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... | |
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 and (when applicable) . More... | |
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... | |
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... | |
mrpt::maps::CSimpleMap | m_SFs |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc) More... | |
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 . More... | |
virtual void | OnTransitionJacobian (KFMatrix_VxV &out_F) const |
Implements the transition Jacobian . 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 and (when applicable) . 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... | |
|
inherited |
Definition at line 193 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 196 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 194 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 192 of file CKalmanFilterCapable.h.
|
inherited |
My class, in a shorter name!
Definition at line 172 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 176 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 181 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 180 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 187 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 186 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 189 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 179 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 190 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 184 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 183 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 178 of file CKalmanFilterCapable.h.
|
inherited |
The numeric type used in the Kalman Filter (default=double)
Definition at line 171 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 175 of file CKalmanFilterCapable.h.
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.
Definition at line 50 of file CRangeBearingKFSLAM2D.h.
|
inherited |
Definition at line 195 of file CKalmanFilterCapable.h.
CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D | ( | ) |
|
virtual |
Destructor.
Definition at line 72 of file CRangeBearingKFSLAM2D.cpp.
|
inlinestaticinherited |
Definition at line 166 of file CKalmanFilterCapable.h.
|
inlinestaticinherited |
Definition at line 165 of file CKalmanFilterCapable.h.
Referenced by OnObservationJacobians(), OnObservationJacobiansNumericGetIncrements(), OnObservationModel(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
inlinestaticinherited |
Definition at line 164 of file CKalmanFilterCapable.h.
Referenced by OnGetObservationsAndDataAssociation().
|
inlinestaticinherited |
Definition at line 163 of file CKalmanFilterCapable.h.
Referenced by OnObservationJacobians(), OnObservationJacobiansNumericGetIncrements(), OnObservationModel(), OnTransitionJacobianNumericGetIncrements(), OnTransitionModel(), and saveMapAndPath2DRepresentationAsMATLABFile().
void CRangeBearingKFSLAM2D::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.
out_objects |
Definition at line 873 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::opengl::stock_objects::CornerXYZ(), mrpt::poses::CPoint2DPDFGaussian::cov, mrpt::opengl::CEllipsoid::Create(), mrpt::format(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, mrpt::poses::CPoint2DPDFGaussian::mean, options, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::quantiles_3D_representation, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
void CRangeBearingKFSLAM2D::getCurrentRobotPose | ( | mrpt::poses::CPosePDFGaussian & | out_robotPose | ) | const |
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
Definition at line 79 of file CRangeBearingKFSLAM2D.cpp.
References ASSERT_, mrpt::poses::CPosePDFGaussian::cov, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, mrpt::poses::CPosePDFGaussian::mean, MRPT_END, and MRPT_START.
Referenced by processActionObservation().
void CRangeBearingKFSLAM2D::getCurrentState | ( | mrpt::poses::CPosePDFGaussian & | out_robotPose, |
std::vector< mrpt::math::TPoint2D > & | 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.
out_robotPose | The mean & 3x3 covariance matrix of the robot 2D pose |
out_landmarksPositions | One entry for each of the M landmark positions (2D). |
out_landmarkIDs | Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. |
out_fullState | The complete state vector (3+2M). |
out_fullCovariance | The full (3+2M)x(3+2M) covariance matrix of the filter. |
Definition at line 101 of file CRangeBearingKFSLAM2D.cpp.
References ASSERT_, mrpt::poses::CPosePDFGaussian::cov, mrpt::utils::bimap< KEY, VALUE >::getInverseMap(), m_IDs, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, mrpt::poses::CPosePDFGaussian::mean, MRPT_END, and MRPT_START.
|
inlineinherited |
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
std::exception | On idx>= getNumberOfLandmarksInTheMap() |
Definition at line 213 of file CKalmanFilterCapable.h.
|
inlineprotected |
Definition at line 302 of file CRangeBearingKFSLAM2D.h.
|
inlineinherited |
Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
std::exception | On idx>= getNumberOfLandmarksInTheMap() |
Definition at line 206 of file CKalmanFilterCapable.h.
|
inline |
Returns a read-only reference to the information on the last data-association.
Definition at line 162 of file CRangeBearingKFSLAM2D.h.
|
inlineinherited |
Definition at line 167 of file CKalmanFilterCapable.h.
Referenced by OnObservationModel(), and processActionObservation().
|
inlineinherited |
Definition at line 460 of file CKalmanFilterCapable.h.
|
inlineinherited |
Definition at line 198 of file CKalmanFilterCapable.h.
|
inlineinherited |
Definition at line 201 of file CKalmanFilterCapable.h.
|
inlineinherited |
Definition at line 200 of file CKalmanFilterCapable.h.
|
inlineinherited |
Definition at line 168 of file CKalmanFilterCapable.h.
void CRangeBearingKFSLAM2D::loadOptions | ( | const mrpt::utils::CConfigFileBase & | ini | ) |
Load options from a ini-like file/text.
Definition at line 683 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KF_options, mrpt::bayes::TKF_options::loadFromConfigFile(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::loadFromConfigFile(), and options.
|
protected |
Must return the action vector u.
out_u | The action vector which will be passed to OnTransitionModel |
Definition at line 183 of file CRangeBearingKFSLAM2D.cpp.
References m_action, mrpt::poses::CPose3DPDFGaussian::mean, mrpt::poses::CPose2D::phi(), mrpt::obs::CActionRobotMovement3D::poseChange, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
protectedpure virtualinherited |
Must return the action vector u.
out_u | The action vector which will be passed to OnTransitionModel |
|
protected |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
out_R | The noise covariance matrix. It might be non diagonal, but it'll usually be. |
Definition at line 1031 of file CRangeBearingKFSLAM2D.cpp.
References options, mrpt::math::square(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_yaw.
|
protectedpure virtualinherited |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
out_R | The noise covariance matrix. It might be non diagonal, but it'll usually be. |
|
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.
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.
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.
Definition at line 491 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::all_predictions, ASSERTMSG_, mrpt::slam::TDataAssociationResults::associations, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::clear(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_chi2_thres, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_metric, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_ml_threshold, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_method, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_metric, mrpt::slam::data_association_full_covariance(), mrpt::utils::bimap< KEY, VALUE >::end(), mrpt::utils::find_in_vector(), mrpt::utils::bimap< KEY, VALUE >::find_key(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_observation_size(), m_IDs, m_last_data_association, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SF, mrpt::math::mahalanobisDistance2AndLogPDF(), MRPT_END, MRPT_START, options, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::predictions_IDs, mrpt::utils::CStream::printf(), R, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::results, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::S, mrpt::math::UNINITIALIZED_MATRIX, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::Y_pred_covs, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::Y_pred_means, and mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::Z.
|
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.
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 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.
|
protected |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
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 Jacobian of the inv. sensor model wrt the robot pose . |
out_dyn_dhn | The Jacobian of the inv. sensor model wrt the observation vector . |
Definition at line 755 of file CRangeBearingKFSLAM2D.cpp.
References ASSERTMSG_, m_SF, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, and MRPT_START.
|
inlinevirtualinherited |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
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 Jacobian of the inv. sensor model wrt the robot pose . |
out_dyn_dhn | The Jacobian of the inv. sensor model wrt the observation vector . |
Definition at line 372 of file CKalmanFilterCapable.h.
|
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:
.
but may be computed from additional terms, or whatever needed by the user.
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 Jacobian of the inv. sensor model wrt the robot pose . |
out_dyn_dhn | The Jacobian of the inv. sensor model wrt the observation vector . |
out_dyn_dhn_R_dyn_dhnT | See the discussion above. |
Definition at line 407 of file CKalmanFilterCapable.h.
|
protectedvirtual |
If applicable to the given problem, do here any special handling of adding a new landmark to the map.
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. |
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
Definition at line 837 of file CRangeBearingKFSLAM2D.cpp.
References ASSERT_, ASSERTMSG_, mrpt::utils::bimap< KEY, VALUE >::insert(), m_IDs, m_last_data_association, m_SF, MRPT_END, MRPT_START, and mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::newly_inserted_landmarks.
|
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< 3, 2, 2, 3 >.
Definition at line 673 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, and mrpt::math::wrapToPiInPlace().
|
protected |
Implements the observation Jacobians and (when applicable) .
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 . |
Hy | The output Jacobian . |
Definition at line 389 of file CRangeBearingKFSLAM2D.cpp.
References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size(), m_SF, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, and mrpt::math::square().
|
inlineprotectedvirtualinherited |
Implements the observation Jacobians and (when applicable) .
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 . |
Hy | The output Jacobian . |
Definition at line 330 of file CKalmanFilterCapable.h.
|
protected |
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 1080 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_feature_size(), and mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size().
|
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.
|
protected |
Definition at line 329 of file CRangeBearingKFSLAM2D.cpp.
References ASSERTDEB_, ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::getNumberOfLandmarksInTheMap(), m_SF, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, mrpt::poses::CPose2D::phi(), mrpt::poses::CPose2D::size(), mrpt::math::square(), mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
protectedpure virtualinherited |
Implements the observation prediction .
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. |
|
inlinevirtualinherited |
This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
Definition at line 445 of file CKalmanFilterCapable.h.
|
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.
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. |
Definition at line 1044 of file CRangeBearingKFSLAM2D.cpp.
References ASSERTMSG_, DEG2RAD, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SF, options, STATS_EXPERIMENT, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_yaw.
|
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.
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. |
Definition at line 279 of file CKalmanFilterCapable.h.
|
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 1020 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::math::wrapToPiInPlace().
|
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.
|
protected |
Implements the transition Jacobian .
This virtual function musts calculate the Jacobian F of the prediction model.
out_F | Must return the Jacobian. The returned matrix must be with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems). |
Definition at line 243 of file CRangeBearingKFSLAM2D.cpp.
References m_action, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, THROW_EXCEPTION, mrpt::math::TPoint2D::x, and mrpt::math::TPoint2D::y.
|
inlineprotectedvirtualinherited |
Implements the transition Jacobian .
out_F | Must return the Jacobian. The returned matrix must be 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.
|
protected |
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 1072 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size().
|
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.
|
protected |
Implements the transition model .
This virtual function musts implement the prediction model of the Kalman filter.
in_u | The vector returned by OnGetAction. |
inout_x | At input has , at output must have . |
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 213 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, mrpt::poses::CPose2D::phi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
protectedpure virtualinherited |
Implements the transition model .
in_u | The vector returned by OnGetAction. |
inout_x | At input has , at output must have . |
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 |
|
protected |
Implements the transition noise covariance .
This virtual function musts calculate de noise matrix of the prediction model.
out_Q | Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian |
Definition at line 286 of file CRangeBearingKFSLAM2D.cpp.
References ASSERT_, mrpt::poses::CPosePDFGaussian::cov, m_action, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, options, mrpt::poses::CPosePDFGaussian::rotateCov(), mrpt::math::square(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::stds_Q_no_odo, and THROW_EXCEPTION.
|
protectedpure virtualinherited |
Implements the transition noise covariance .
out_Q | Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian |
void CRangeBearingKFSLAM2D::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.
action | May contain odometry |
SF | The set of observations, must contain at least one CObservationBearingRange |
Definition at line 149 of file CRangeBearingKFSLAM2D.cpp.
References ASSERT_, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::create_simplemap, getCurrentRobotPose(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::getNumberOfLandmarksInTheMap(), mrpt::maps::CSimpleMap::insert(), m_action, m_IDs, m_SF, m_SFs, MRPT_END, MRPT_START, options, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::runOneKalmanIteration(), and mrpt::utils::bimap< KEY, VALUE >::size().
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
Definition at line 54 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::utils::bimap< KEY, VALUE >::clear(), mrpt::maps::CSimpleMap::clear(), m_action, m_IDs, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SF, m_SFs, and mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk.
Referenced by CRangeBearingKFSLAM2D().
|
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.
Referenced by processActionObservation().
void CRangeBearingKFSLAM2D::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 935 of file CRangeBearingKFSLAM2D.cpp.
References mrpt::math::cov(), mrpt::system::os::fclose(), mrpt::system::os::fopen(), mrpt::system::os::fprintf(), mrpt::maps::CSimpleMap::get(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SFs, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, mrpt::math::MATLAB_plotCovariance2D(), mean(), mrpt::maps::CSimpleMap::size(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
inherited |
Generic options for the Kalman Filter algorithm itself.
Definition at line 462 of file CKalmanFilterCapable.h.
Referenced by loadOptions().
|
protected |
Set up by processActionObservation.
Definition at line 310 of file CRangeBearingKFSLAM2D.h.
Referenced by OnGetAction(), OnTransitionJacobian(), OnTransitionNoise(), processActionObservation(), and reset().
|
protected |
The mapping between landmark IDs and indexes in the Pkk cov.
matrix:
Definition at line 316 of file CRangeBearingKFSLAM2D.h.
Referenced by getCurrentState(), OnGetObservationsAndDataAssociation(), OnNewLandmarkAddedToMap(), processActionObservation(), and reset().
|
protected |
Last data association.
Definition at line 321 of file CRangeBearingKFSLAM2D.h.
Referenced by OnGetObservationsAndDataAssociation(), and OnNewLandmarkAddedToMap().
|
protectedinherited |
The system full covariance matrix.
Definition at line 222 of file CKalmanFilterCapable.h.
Referenced by getAs3DObject(), getCurrentRobotPose(), getCurrentState(), OnGetObservationsAndDataAssociation(), OnPreComputingPredictions(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
protected |
Set up by processActionObservation.
Definition at line 313 of file CRangeBearingKFSLAM2D.h.
Referenced by OnGetObservationsAndDataAssociation(), OnInverseObservationModel(), OnNewLandmarkAddedToMap(), OnObservationJacobians(), OnObservationModel(), OnPreComputingPredictions(), processActionObservation(), and reset().
|
protected |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
Definition at line 319 of file CRangeBearingKFSLAM2D.h.
Referenced by processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
protectedinherited |
Definition at line 226 of file CKalmanFilterCapable.h.
|
protectedinherited |
The system state vector.
Definition at line 221 of file CKalmanFilterCapable.h.
Referenced by getAs3DObject(), getCurrentRobotPose(), getCurrentState(), OnInverseObservationModel(), OnNormalizeStateVector(), OnObservationJacobians(), OnObservationModel(), OnTransitionJacobian(), OnTransitionModel(), OnTransitionNoise(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
TOptions mrpt::slam::CRangeBearingKFSLAM2D::options |
The options for the algorithm.
Definition at line 119 of file CRangeBearingKFSLAM2D.h.
Referenced by getAs3DObject(), loadOptions(), OnGetObservationNoise(), OnGetObservationsAndDataAssociation(), OnPreComputingPredictions(), OnTransitionNoise(), and processActionObservation().
Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019 |