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 Wiki page describes an front-end application based on this class: http://www.mrpt.org/Application:2d-slam-demo
Definition at line 69 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 Member Functions | |
CRangeBearingKFSLAM2D () | |
Default constructor. | |
virtual | ~CRangeBearingKFSLAM2D () |
Destructor. | |
void | reset () |
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). | |
void | processActionObservation (CActionCollectionPtr &action, CSensoryFramePtr &SF) |
Process one new action and observations to update the map and robot pose estimate. | |
void | getCurrentState (CPosePDFGaussian &out_robotPose, std::vector< TPoint2D > &out_landmarksPositions, std::map< unsigned int, CLandmark::TLandmarkID > &out_landmarkIDs, CVectorDouble &out_fullState, CMatrixDouble &out_fullCovariance) const |
Returns the complete mean and cov. | |
void | getCurrentRobotPose (CPosePDFGaussian &out_robotPose) const |
Returns the mean & 3x3 covariance matrix of the robot 2D pose. | |
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. | |
void | loadOptions (const mrpt::utils::CConfigFileBase &ini) |
Load options from a ini-like file/text. | |
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. | |
const TDataAssocInfo & | getLastDataAssociation () const |
Returns a read-only reference to the information on the last data-association. | |
Public Attributes | |
TOptions | options |
The options for the algorithm. | |
Protected Member Functions | |
void | getLandmarkIDsFromIndexInStateVector (std::map< unsigned int, CLandmark::TLandmarkID > &out_id2index) const |
Virtual methods for Kalman Filter implementation | |
void | OnGetAction (KFArray_ACT &out_u) const |
Must return the action vector u. | |
void | OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const |
Implements the transition model ![]() | |
void | OnTransitionJacobian (KFMatrix_VxV &out_F) const |
Implements the transition Jacobian ![]() | |
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. | |
void | OnTransitionNoise (KFMatrix_VxV &out_Q) const |
Implements the transition noise covariance ![]() | |
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. | |
void | OnObservationModel (const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const |
Implements the observation prediction ![]() | |
void | OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const |
Implements the observation Jacobians ![]() ![]() | |
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. | |
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). | |
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. | |
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. | |
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". | |
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. | |
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. | |
Protected Attributes | |
CActionCollectionPtr | m_action |
Set up by processActionObservation. | |
CSensoryFramePtr | m_SF |
Set up by processActionObservation. | |
mrpt::utils::bimap < CLandmark::TLandmarkID, unsigned int > | m_IDs |
The mapping between landmark IDs and indexes in the Pkk cov. | |
CSimpleMap | m_SFs |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc) | |
TDataAssocInfo | m_last_data_association |
Last data association. |
mrpt::slam::CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D | ( | ) |
Default constructor.
virtual mrpt::slam::CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D | ( | ) | [virtual] |
Destructor.
void mrpt::slam::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 |
void mrpt::slam::CRangeBearingKFSLAM2D::getCurrentRobotPose | ( | CPosePDFGaussian & | out_robotPose | ) | const |
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
void mrpt::slam::CRangeBearingKFSLAM2D::getCurrentState | ( | CPosePDFGaussian & | out_robotPose, |
std::vector< TPoint2D > & | out_landmarksPositions, | ||
std::map< unsigned int, CLandmark::TLandmarkID > & | out_landmarkIDs, | ||
CVectorDouble & | out_fullState, | ||
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. |
void mrpt::slam::CRangeBearingKFSLAM2D::getLandmarkIDsFromIndexInStateVector | ( | std::map< unsigned int, CLandmark::TLandmarkID > & | out_id2index | ) | const [inline, protected] |
Definition at line 334 of file CRangeBearingKFSLAM2D.h.
const TDataAssocInfo& mrpt::slam::CRangeBearingKFSLAM2D::getLastDataAssociation | ( | ) | const [inline] |
Returns a read-only reference to the information on the last data-association.
Definition at line 193 of file CRangeBearingKFSLAM2D.h.
void mrpt::slam::CRangeBearingKFSLAM2D::loadOptions | ( | const mrpt::utils::CConfigFileBase & | ini | ) |
Load options from a ini-like file/text.
void mrpt::slam::CRangeBearingKFSLAM2D::OnGetAction | ( | KFArray_ACT & | out_u | ) | const [protected, virtual] |
Must return the action vector u.
out_u | The action vector which will be passed to OnTransitionModel |
Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservationNoise | ( | KFMatrix_OxO & | out_R | ) | const [protected, virtual] |
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. |
Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::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 | ||
) | [protected, virtual] |
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.
Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnInverseObservationModel | ( | const KFArray_OBS & | in_z, |
KFArray_FEAT & | out_yn, | ||
KFMatrix_FxV & | out_dyn_dxv, | ||
KFMatrix_FxO & | out_dyn_dhn | ||
) | const [protected, virtual] |
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 ![]() ![]() |
out_dyn_dhn | The ![]() ![]() |
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnNewLandmarkAddedToMap | ( | const size_t | in_obsIdx, |
const size_t | in_idxNewFeat | ||
) | [protected, virtual] |
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 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnNormalizeStateVector | ( | ) | [protected, virtual] |
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.
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationJacobians | ( | const size_t & | idx_landmark_to_predict, |
KFMatrix_OxV & | Hx, | ||
KFMatrix_OxF & | Hy | ||
) | const [protected, virtual] |
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 ![]() |
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationJacobiansNumericGetIncrements | ( | KFArray_VEH & | out_veh_increments, |
KFArray_FEAT & | out_feat_increments | ||
) | const [protected, virtual] |
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.
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationModel | ( | const vector_size_t & | idx_landmarks_to_predict, |
vector_KFArray_OBS & | out_predictions | ||
) | const [protected, virtual] |
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. |
Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnPreComputingPredictions | ( | const vector_KFArray_OBS & | in_all_prediction_means, |
vector_size_t & | out_LM_indices_to_predict | ||
) | const [protected, virtual] |
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. |
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnSubstractObservationVectors | ( | KFArray_OBS & | A, |
const KFArray_OBS & | B | ||
) | const [protected, virtual] |
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobian | ( | KFMatrix_VxV & | out_F | ) | const [protected, virtual] |
Implements the transition Jacobian .
out_F | Must return the Jacobian. The returned matrix must be ![]() |
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobianNumericGetIncrements | ( | KFArray_VEH & | out_increments | ) | const [protected, virtual] |
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.
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionModel | ( | const KFArray_ACT & | in_u, |
KFArray_VEH & | inout_x, | ||
bool & | out_skipPrediction | ||
) | const [protected, virtual] |
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 |
Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionNoise | ( | KFMatrix_VxV & | out_Q | ) | const [protected, virtual] |
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 |
Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.
void mrpt::slam::CRangeBearingKFSLAM2D::processActionObservation | ( | CActionCollectionPtr & | action, |
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 |
void mrpt::slam::CRangeBearingKFSLAM2D::reset | ( | ) |
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
void mrpt::slam::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.
Set up by processActionObservation.
Definition at line 343 of file CRangeBearingKFSLAM2D.h.
mrpt::utils::bimap<CLandmark::TLandmarkID,unsigned int> mrpt::slam::CRangeBearingKFSLAM2D::m_IDs [protected] |
The mapping between landmark IDs and indexes in the Pkk cov.
matrix:
Definition at line 351 of file CRangeBearingKFSLAM2D.h.
Last data association.
Definition at line 357 of file CRangeBearingKFSLAM2D.h.
Set up by processActionObservation.
Definition at line 347 of file CRangeBearingKFSLAM2D.h.
CSimpleMap mrpt::slam::CRangeBearingKFSLAM2D::m_SFs [protected] |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
Definition at line 355 of file CRangeBearingKFSLAM2D.h.
The options for the algorithm.
Definition at line 150 of file CRangeBearingKFSLAM2D.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |