MRPT logo

mrpt::slam::CRangeBearingKFSLAM2D Class Reference

An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks. More...

#include <mrpt/slam/CRangeBearingKFSLAM2D.h>

Inheritance diagram for mrpt::slam::CRangeBearingKFSLAM2D:

mrpt::utils::CDebugOutputCapable mrpt::bayes::CKalmanFilterCapable

List of all members.

Classes

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.

Public Attributes

TOptions options
 The options for the algorithm.

Protected Member Functions

Virtual methods for Kalman Filter implementation
void OnGetAction (CVectorTemplate< KFTYPE > &out_u)
 Must return the action vector u.
void OnTransitionModel (const CVectorTemplate< KFTYPE > &in_u, CVectorTemplate< KFTYPE > &inout_x, bool &out_skipPrediction)
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.
void OnTransitionJacobian (CMatrixTemplateNumeric< KFTYPE > &out_F)
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.
void OnTransitionNoise (CMatrixTemplateNumeric< KFTYPE > &out_Q)
 Implements the transition noise covariance $ Q_k $.
void OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CVectorTemplate< KFTYPE > &out_R2, vector_int &out_data_association)
 This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
void OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CMatrixTemplateNumeric< KFTYPE > &out_R, vector_int &out_data_association)
 This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
void OnObservationModelAndJacobians (const CMatrixTemplateNumeric< KFTYPE > &in_z, const vector_int &in_data_association, const bool &in_full, const int &in_obsIdx, CVectorTemplate< KFTYPE > &out_innov, CMatrixTemplateNumeric< KFTYPE > &out_Hx, CMatrixTemplateNumeric< KFTYPE > &out_Hy)
 Implements the observation prediction $ h_i(x) $ and the Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.
void OnInverseObservationModel (const CMatrixTemplateNumeric< KFTYPE > &in_z, const size_t &in_obsIdx, const size_t &in_idxNewFeat, CVectorTemplate< KFTYPE > &out_yn, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dxv, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dhn)
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
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.
size_t get_vehicle_size () const
 Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
size_t get_feature_size () const
 Must return the dimension of the features in the system state (the "map"), or 0 if not applicable.
size_t get_observation_size () const
 Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).

Protected Attributes

CActionCollectionPtr m_action
 Set up by processActionObservation.
CSensoryFramePtr m_SF
 Set up by processActionObservation.
std::map
< CLandmark::TLandmarkID,
unsigned int > 
m_IDs
 The mapping between landmark IDs and indexes in the Pkk cov.
std::map< unsigned int,
CLandmark::TLandmarkID
m_IDs_inverse
 The mapping between indexes in the Pkk cov.
CSensFrameProbSequence m_SFs
 The sequence of all the observations and the robot path (kept for debugging, statistics,etc).


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://babel.isa.uma.es/mrpt/index.php/Application:2d-slam-demo

See also:
CRangeBearingKFSLAM

Definition at line 65 of file CRangeBearingKFSLAM2D.h.


Constructor & Destructor Documentation

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

Default constructor.

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

Destructor.


Member Function Documentation

size_t mrpt::slam::CRangeBearingKFSLAM2D::get_feature_size (  )  const [inline, protected, virtual]

Must return the dimension of the features in the system state (the "map"), or 0 if not applicable.

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

Definition at line 272 of file CRangeBearingKFSLAM2D.h.

size_t mrpt::slam::CRangeBearingKFSLAM2D::get_observation_size (  )  const [inline, protected, virtual]

Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).

Implements mrpt::bayes::CKalmanFilterCapable.

Definition at line 276 of file CRangeBearingKFSLAM2D.h.

size_t mrpt::slam::CRangeBearingKFSLAM2D::get_vehicle_size (  )  const [inline, protected, virtual]

Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.

Implements mrpt::bayes::CKalmanFilterCapable.

Definition at line 268 of file CRangeBearingKFSLAM2D.h.

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

void mrpt::slam::CRangeBearingKFSLAM2D::loadOptions ( const mrpt::utils::CConfigFileBase ini  ) 

Load options from a ini-like file/text.

void mrpt::slam::CRangeBearingKFSLAM2D::OnGetAction ( CVectorTemplate< KFTYPE > &  out_u  )  [protected, virtual]

Must return the action vector u.

Parameters:
out_u The action vector which will be passed to OnTransitionModel

Implements mrpt::bayes::CKalmanFilterCapable.

void mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservations ( CMatrixTemplateNumeric< KFTYPE > &  out_z,
CMatrixTemplateNumeric< KFTYPE > &  out_R,
vector_int out_data_association 
) [inline, protected, virtual]

This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.

Parameters:
out_z A $N \times O$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z.
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, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration.
Note:
Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2.

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

Definition at line 206 of file CRangeBearingKFSLAM2D.h.

References mrpt::bayes::CKalmanFilterCapable::OnGetObservations().

void mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservations ( CMatrixTemplateNumeric< KFTYPE > &  out_z,
CVectorTemplate< KFTYPE > &  out_R2,
vector_int out_data_association 
) [protected, virtual]

This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.

Parameters:
out_z A $N \times O$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z.
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, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration.

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

void mrpt::slam::CRangeBearingKFSLAM2D::OnInverseObservationModel ( const CMatrixTemplateNumeric< KFTYPE > &  in_z,
const size_t &  in_obsIdx,
const size_t &  in_idxNewFeat,
CVectorTemplate< KFTYPE > &  out_yn,
CMatrixTemplateNumeric< KFTYPE > &  out_dyn_dxv,
CMatrixTemplateNumeric< KFTYPE > &  out_dyn_dhn 
) [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_z This is the same matrix returned by OnGetObservations().
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.
out_yn The F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
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} $.

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

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.

void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationModelAndJacobians ( const CMatrixTemplateNumeric< KFTYPE > &  in_z,
const vector_int in_data_association,
const bool &  in_full,
const int &  in_obsIdx,
CVectorTemplate< KFTYPE > &  out_innov,
CMatrixTemplateNumeric< KFTYPE > &  out_Hx,
CMatrixTemplateNumeric< KFTYPE > &  out_Hy 
) [protected, virtual]

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

Parameters:
in_z This is the same matrix returned by OnGetObservations(), passed here for reference.
in_data_association The vector returned by OnGetObservations(), passed here for reference.
in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx.
in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now.
out_innov The difference between the expected observation and the real one: $ \tilde{y} = z - h(x) $. It must be a vector of length O*N (in_full=true) or O (in_full=false), with O=get_observation_size() and N=number of rows in in_z.
out_Hx One or a vertical stack of $ \frac{\partial h_i}{\partial x} $.
out_Hy An empty matrix, or one or a vertical stack of $ \frac{\partial h_i}{\partial y_i} $.
out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked $O \times V$ matrices, and out_Hy must consist of 1 or N $O \times F$ matrices, with:

Implements mrpt::bayes::CKalmanFilterCapable.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobian ( CMatrixTemplateNumeric< KFTYPE > &  out_F  )  [protected, virtual]

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

Parameters:
out_F Must return the Jacobian. The returned matrix must be $N \times N$ with N being either the size of the whole state vector or get_vehicle_size().

Implements mrpt::bayes::CKalmanFilterCapable.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionModel ( const CVectorTemplate< KFTYPE > &  in_u,
CVectorTemplate< KFTYPE > &  inout_x,
bool &  out_skipPrediction 
) [protected, virtual]

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

Implements mrpt::bayes::CKalmanFilterCapable.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionNoise ( CMatrixTemplateNumeric< KFTYPE > &  out_Q  )  [protected, virtual]

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

Implements mrpt::bayes::CKalmanFilterCapable.

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


Member Data Documentation

CActionCollectionPtr mrpt::slam::CRangeBearingKFSLAM2D::m_action [protected]

Set up by processActionObservation.

Definition at line 283 of file CRangeBearingKFSLAM2D.h.

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

matrix:

Definition at line 291 of file CRangeBearingKFSLAM2D.h.

The mapping between indexes in the Pkk cov.

matrix and landmark IDs:

Definition at line 295 of file CRangeBearingKFSLAM2D.h.

CSensoryFramePtr mrpt::slam::CRangeBearingKFSLAM2D::m_SF [protected]

Set up by processActionObservation.

Definition at line 287 of file CRangeBearingKFSLAM2D.h.

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

Definition at line 299 of file CRangeBearingKFSLAM2D.h.

The options for the algorithm.

Definition at line 137 of file CRangeBearingKFSLAM2D.h.




Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009