#include <mrpt/slam/CRangeBearingKFSLAM2D.h>
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 ![]() | |
void | OnTransitionJacobian (CMatrixTemplateNumeric< KFTYPE > &out_F) |
Implements the transition Jacobian ![]() | |
void | OnTransitionNoise (CMatrixTemplateNumeric< KFTYPE > &out_Q) |
Implements the transition noise covariance ![]() | |
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 ![]() ![]() ![]() | |
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). |
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
Definition at line 65 of file CRangeBearingKFSLAM2D.h.
mrpt::slam::CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D | ( | ) |
Default constructor.
virtual mrpt::slam::CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D | ( | ) | [virtual] |
Destructor.
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.
out_objects |
void mrpt::slam::CRangeBearingKFSLAM2D::getCurrentRobotPose | ( | CPosePDFGaussian & | out_robotPose | ) | const |
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::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.
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.
out_z | A ![]() | |
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.
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.
out_z | A ![]() | |
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".
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 ![]() | |
out_dyn_dxv | The ![]() ![]() | |
out_dyn_dhn | The ![]() ![]() |
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 and the Jacobians
and (when applicable)
.
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: ![]() | |
out_Hx | One or a vertical stack of ![]() | |
out_Hy | An empty matrix, or one or a vertical stack of ![]() |
Implements mrpt::bayes::CKalmanFilterCapable.
void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobian | ( | CMatrixTemplateNumeric< KFTYPE > & | out_F | ) | [protected, virtual] |
Implements the transition Jacobian .
out_F | Must return the Jacobian. The returned matrix must be ![]() |
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 .
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.
void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionNoise | ( | CMatrixTemplateNumeric< KFTYPE > & | out_Q | ) | [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.
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.
CActionCollectionPtr mrpt::slam::CRangeBearingKFSLAM2D::m_action [protected] |
std::map<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 291 of file CRangeBearingKFSLAM2D.h.
std::map<unsigned int,CLandmark::TLandmarkID> mrpt::slam::CRangeBearingKFSLAM2D::m_IDs_inverse [protected] |
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] |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc).
Definition at line 299 of file CRangeBearingKFSLAM2D.h.
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |