MRPT logo

mrpt::slam::CRangeBearingKFSLAM Class Reference

An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks. More...

#include <mrpt/slam/CRangeBearingKFSLAM.h>

Inheritance diagram for mrpt::slam::CRangeBearingKFSLAM:
mrpt::utils::CDebugOutputCapable mrpt::bayes::CKalmanFilterCapable

List of all members.

Classes

struct  TOptions
 The options for the algorithm. More...

Public Member Functions

 CRangeBearingKFSLAM ()
 Constructor.
virtual ~CRangeBearingKFSLAM ()
 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 (CPose3DPDFGaussian &out_robotPose, std::vector< CPoint3D > &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 (CPose3DPDFGaussian &out_robotPose) const
 Returns the mean & 6x6 covariance matrix of the robot 6D 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 getLastPartition (std::vector< vector_uint > &parts)
 Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only if options.doPartitioningExperiment = true.
void getLastPartitionLandmarks (std::vector< vector_uint > &landmarksMembership) const
 Return the partitioning of the landmarks in clusters accoring to the last partition.
void getLastPartitionLandmarksAsIfFixedSubmaps (size_t K, std::vector< vector_uint > &landmarksMembership)
 For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used.
double computeOffDiagonalBlocksApproximationError (const std::vector< vector_uint > &landmarksMembership) const
 Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.
void reconsiderPartitionsNow ()
 The partitioning of the entire map is recomputed again.
CIncrementalMapPartitioner::TOptionsmapPartitionOptions ()
 Provides access to the parameters of the map partitioning algorithm.
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

mrpt::slam::CRangeBearingKFSLAM::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.
CIncrementalMapPartitioner mapPartitioner
 Used for map partitioning experiments.
CSensFrameProbSequence m_SFs
 The sequence of all the observations and the robot path (kept for debugging, statistics,etc).
std::vector< vector_uintm_lastPartitionSet

Detailed Description

An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks.

The main method is "processActionObservation" which processes pairs of action/observation.

The following Wiki page describes an front-end application based on this class: http://babel.isa.uma.es/mrpt/index.php/Application:kf-slam

See also:
An implementation for 2D only: CRangeBearingKFSLAM2D

Definition at line 64 of file CRangeBearingKFSLAM.h.


Constructor & Destructor Documentation

mrpt::slam::CRangeBearingKFSLAM::CRangeBearingKFSLAM (  ) 

Constructor.

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

Destructor:.


Member Function Documentation

double mrpt::slam::CRangeBearingKFSLAM::computeOffDiagonalBlocksApproximationError ( const std::vector< vector_uint > &  landmarksMembership  )  const

Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.

See also:
getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps
size_t mrpt::slam::CRangeBearingKFSLAM::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 341 of file CRangeBearingKFSLAM.h.

size_t mrpt::slam::CRangeBearingKFSLAM::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 345 of file CRangeBearingKFSLAM.h.

size_t mrpt::slam::CRangeBearingKFSLAM::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 337 of file CRangeBearingKFSLAM.h.

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

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

Parameters:
out_objects 
void mrpt::slam::CRangeBearingKFSLAM::getCurrentRobotPose ( CPose3DPDFGaussian out_robotPose  )  const

Returns the mean & 6x6 covariance matrix of the robot 6D pose.

See also:
getCurrentState
void mrpt::slam::CRangeBearingKFSLAM::getCurrentState ( CPose3DPDFGaussian out_robotPose,
std::vector< CPoint3D > &  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 & 6x6 covariance matrix of the robot 6D pose
out_landmarksPositions One entry for each of the M landmark positions (3D).
out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState The complete state vector (6+3M).
out_fullCovariance The full (6+3M)x(6+3M) covariance matrix of the filter.
See also:
getCurrentRobotPose
void mrpt::slam::CRangeBearingKFSLAM::getLastPartition ( std::vector< vector_uint > &  parts  )  [inline]

Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only if options.doPartitioningExperiment = true.

See also:
getLastPartitionLandmarks

Definition at line 167 of file CRangeBearingKFSLAM.h.

void mrpt::slam::CRangeBearingKFSLAM::getLastPartitionLandmarks ( std::vector< vector_uint > &  landmarksMembership  )  const

Return the partitioning of the landmarks in clusters accoring to the last partition.

Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks) Only if options.doPartitioningExperiment = true

Parameters:
landmarksMembership The i'th element of this vector is the set of clusters to which the i'th landmark in the map belongs to (landmark index != landmark ID !!).
See also:
getLastPartition
void mrpt::slam::CRangeBearingKFSLAM::getLastPartitionLandmarksAsIfFixedSubmaps ( size_t  K,
std::vector< vector_uint > &  landmarksMembership 
)

For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used.

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

Load options from a ini-like file/text.

CIncrementalMapPartitioner::TOptions* mrpt::slam::CRangeBearingKFSLAM::mapPartitionOptions (  )  [inline]

Provides access to the parameters of the map partitioning algorithm.

Definition at line 202 of file CRangeBearingKFSLAM.h.

void mrpt::slam::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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 275 of file CRangeBearingKFSLAM.h.

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

void mrpt::slam::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::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::CRangeBearingKFSLAM::reconsiderPartitionsNow (  ) 

The partitioning of the entire map is recomputed again.

Only when options.doPartitioningExperiment = true. This can be used after changing the parameters of the partitioning method. After this method, you can call getLastPartitionLandmarks.

See also:
getLastPartitionLandmarks
void mrpt::slam::CRangeBearingKFSLAM::reset (  ) 

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

void mrpt::slam::CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile ( const std::string &  fil,
float  stdCount = 3.0f,
const std::string &  styleLandmarks = std::string("b"),
const std::string &  stylePath = std::string("r"),
const std::string &  styleRobot = std::string("r") 
) const

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


Member Data Documentation

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

Set up by processActionObservation.

Definition at line 352 of file CRangeBearingKFSLAM.h.

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

matrix:

Definition at line 360 of file CRangeBearingKFSLAM.h.

The mapping between indexes in the Pkk cov.

matrix and landmark IDs:

Definition at line 364 of file CRangeBearingKFSLAM.h.

Definition at line 375 of file CRangeBearingKFSLAM.h.

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

Set up by processActionObservation.

Definition at line 356 of file CRangeBearingKFSLAM.h.

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

Definition at line 373 of file CRangeBearingKFSLAM.h.

Used for map partitioning experiments.

Definition at line 369 of file CRangeBearingKFSLAM.h.

The options for the algorithm.




Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009