Main MRPT website > C++ reference
MRPT logo
Classes | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes

mrpt::slam::CRangeBearingKFSLAM2D Class Reference


Detailed Description

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

See also:
CRangeBearingKFSLAM

Definition at line 69 of file CRangeBearingKFSLAM2D.h.

#include <mrpt/slam/CRangeBearingKFSLAM2D.h>

Inheritance diagram for mrpt::slam::CRangeBearingKFSLAM2D:
Inheritance graph
[legend]

List of all members.

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 TDataAssocInfogetLastDataAssociation () 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 $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.
void OnTransitionJacobian (KFMatrix_VxV &out_F) const
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.
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 $ Q_k $.
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 $ h_i(x) $.
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} $.
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.

Constructor & Destructor Documentation

mrpt::slam::CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D ( )

Default constructor.

virtual mrpt::slam::CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D ( ) [virtual]

Destructor.


Member Function Documentation

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.

Parameters:
out_objects
void mrpt::slam::CRangeBearingKFSLAM2D::getCurrentRobotPose ( CPosePDFGaussian out_robotPose) const

Returns the mean & 3x3 covariance matrix of the robot 2D pose.

See also:
getCurrentState
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.

Parameters:
out_robotPoseThe mean & 3x3 covariance matrix of the robot 2D pose
out_landmarksPositionsOne entry for each of the M landmark positions (2D).
out_landmarkIDsEach element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullStateThe complete state vector (3+2M).
out_fullCovarianceThe full (3+2M)x(3+2M) covariance matrix of the filter.
See also:
getCurrentRobotPose
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.

Parameters:
out_uThe 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.

Parameters:
out_RThe 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.

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

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

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

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

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

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.

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

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 $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

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

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 $ h_i(x) $.

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

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.

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

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 $ \frac{\partial f}{\partial x} $.

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

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 $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

Parameters:
in_uThe vector returned by OnGetAction.
inout_xAt input has

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

, at output must have $ \hat{x}_{k|k-1} $ .

out_skipSet this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false

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

Parameters:
out_QMust 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.

Parameters:
actionMay contain odometry
SFThe 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.


Member Data Documentation

Set up by processActionObservation.

Definition at line 343 of file CRangeBearingKFSLAM2D.h.

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.

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