Main MRPT website > C++ reference
MRPT logo

CRangeBearingKFSLAM2D.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CRangeBearingKFSLAM2D_H
00029 #define CRangeBearingKFSLAM2D_H
00030 
00031 #include <mrpt/utils/CDebugOutputCapable.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/math/CVectorTemplate.h>
00034 #include <mrpt/utils/CConfigFileBase.h>
00035 #include <mrpt/utils/CLoadableOptions.h>
00036 #include <mrpt/opengl.h>
00037 #include <mrpt/bayes/CKalmanFilterCapable.h>
00038 
00039 #include <mrpt/utils/safe_pointers.h>
00040 #include <mrpt/utils/bimap.h>
00041 
00042 #include <mrpt/slam/CSensoryFrame.h>
00043 #include <mrpt/slam/CActionCollection.h>
00044 #include <mrpt/slam/CObservationBearingRange.h>
00045 #include <mrpt/poses/CPoint2D.h>
00046 #include <mrpt/poses/CPosePDFGaussian.h>
00047 #include <mrpt/slam/CLandmark.h>
00048 #include <mrpt/slam/CSimpleMap.h>
00049 #include <mrpt/slam/CIncrementalMapPartitioner.h>
00050 #include <mrpt/slam/data_association.h>
00051 
00052 #include <mrpt/slam/link_pragmas.h>
00053 
00054 namespace mrpt
00055 {
00056         namespace slam
00057         {
00058                 using namespace mrpt::bayes;
00059                 using namespace mrpt::poses;
00060 
00061                 /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.
00062                   *  The main method is "processActionObservation" which processes pairs of action/observation.
00063                   *
00064                   *   The following Wiki page describes an front-end application based on this class:
00065                   *     http://www.mrpt.org/Application:2d-slam-demo
00066                   *
00067                   * \sa CRangeBearingKFSLAM
00068                   */
00069                 class SLAM_IMPEXP  CRangeBearingKFSLAM2D :
00070                         public bayes::CKalmanFilterCapable<3 /* x y yaw */, 2 /* range yaw */, 2 /* x y */,      3 /* Ax Ay Ayaw */>
00071                                                        // <size_t VEH_SIZE, size_t OBS_SIZE,   size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double>
00072                 {
00073                  public:
00074                          CRangeBearingKFSLAM2D( ); //!< Default constructor
00075                          virtual ~CRangeBearingKFSLAM2D();      //!< Destructor
00076                          void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
00077 
00078                         /** 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.
00079                          *  \param action May contain odometry
00080                          *      \param SF The set of observations, must contain at least one CObservationBearingRange
00081                          */
00082                         void  processActionObservation(
00083                                 CActionCollectionPtr &action,
00084                                 CSensoryFramePtr     &SF );
00085 
00086                         /** Returns the complete mean and cov.
00087                           *  \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D pose
00088                           *  \param out_landmarksPositions One entry for each of the M landmark positions (2D).
00089                           *  \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
00090                           *  \param out_fullState The complete state vector (3+2M).
00091                           *  \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of the filter.
00092                           * \sa getCurrentRobotPose
00093                           */
00094                         void  getCurrentState(
00095                                 CPosePDFGaussian &out_robotPose,
00096                                 std::vector<TPoint2D>  &out_landmarksPositions,
00097                                 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
00098                                 CVectorDouble      &out_fullState,
00099                                 CMatrixDouble      &out_fullCovariance
00100                                 ) const;
00101 
00102                         /** Returns the mean & 3x3 covariance matrix of the robot 2D pose.
00103                           * \sa getCurrentState
00104                           */
00105                         void  getCurrentRobotPose(
00106                                 CPosePDFGaussian &out_robotPose ) const;
00107 
00108                         /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
00109                           *  \param out_objects
00110                           */
00111                         void  getAs3DObject( mrpt::opengl::CSetOfObjectsPtr     &outObj ) const;
00112 
00113                         /** Load options from a ini-like file/text
00114                           */
00115                         void loadOptions( const mrpt::utils::CConfigFileBase &ini );
00116 
00117                         /** The options for the algorithm
00118                           */
00119                         struct SLAM_IMPEXP TOptions : utils::CLoadableOptions
00120                         {
00121                                 /** Default values
00122                                   */
00123                                 TOptions();
00124 
00125                                 /** Load from a config file/text
00126                                   */
00127                                 void loadFromConfigFile(
00128                                         const mrpt::utils::CConfigFileBase      &source,
00129                                         const std::string               &section);
00130 
00131                                 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
00132                                 */
00133                                 void  dumpToTextStream(CStream  &out) const;
00134 
00135 
00136                                 vector_float    stds_Q_no_odo;  //!< A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y: In meters, phi: radians (but in degrees when loading from a configuration ini-file!)
00137                                 float                   std_sensor_range, std_sensor_yaw;       //!< The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.
00138                                 float                   quantiles_3D_representation;    //!< Default = 3
00139                                 bool                    create_simplemap; //!< Whether to fill m_SFs (default=false)
00140 
00141                                 // Data association:
00142                                 TDataAssociationMethod  data_assoc_method;
00143                                 TDataAssociationMetric  data_assoc_metric;
00144                                 double                                  data_assoc_IC_chi2_thres;  //!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)
00145                                 TDataAssociationMetric  data_assoc_IC_metric;      //!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
00146                                 double                                  data_assoc_IC_ml_threshold;//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
00147 
00148                         };
00149 
00150                         TOptions options; //!< The options for the algorithm
00151 
00152 
00153                         /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D
00154                           */
00155                         void saveMapAndPath2DRepresentationAsMATLABFile(
00156                                 const std::string &fil,
00157                                 float              stdCount=3.0f,
00158                                 const std::string &styleLandmarks = std::string("b"),
00159                                 const std::string &stylePath = std::string("r"),
00160                                 const std::string &styleRobot = std::string("r") ) const;
00161 
00162 
00163                         /** Information for data-association:
00164                           * \sa getLastDataAssociation
00165                           */
00166                         struct SLAM_IMPEXP TDataAssocInfo
00167                         {
00168                                 TDataAssocInfo() :
00169                                         Y_pred_means(0,0),
00170                                         Y_pred_covs(0,0)
00171                                 {
00172                                 }
00173 
00174                                 void clear() {
00175                                         results.clear();
00176                                         predictions_IDs.clear();
00177                                         newly_inserted_landmarks.clear();
00178                                 }
00179 
00180                                 // Predictions from the map:
00181                                 CMatrixTemplateNumeric<kftype>  Y_pred_means,Y_pred_covs;
00182                                 mrpt::vector_size_t                             predictions_IDs;
00183 
00184                                 /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector)
00185                                     Only used for stats and so. */
00186                                 std::map<size_t,size_t>  newly_inserted_landmarks;
00187 
00188                                 // DA results:
00189                                 TDataAssociationResults                 results;
00190                         };
00191 
00192                         /** Returns a read-only reference to the information on the last data-association */
00193                         const TDataAssocInfo & getLastDataAssociation() const {
00194                                 return m_last_data_association;
00195                         }
00196 
00197                  protected:
00198 
00199                         /** @name Virtual methods for Kalman Filter implementation
00200                                 @{
00201                          */
00202 
00203                         /** Must return the action vector u.
00204                           * \param out_u The action vector which will be passed to OnTransitionModel
00205                           */
00206                         void OnGetAction( KFArray_ACT &out_u ) const;
00207 
00208                         /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
00209                           * \param in_u The vector returned by OnGetAction.
00210                           * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must have \f$ \hat{x}_{k|k-1} \f$ .
00211                           * \param 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
00212                           */
00213                         void OnTransitionModel(
00214                                 const KFArray_ACT &in_u,
00215                                 KFArray_VEH       &inout_x,
00216                                 bool &out_skipPrediction
00217                                 ) const;
00218 
00219                         /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
00220                           * \param out_F Must return the Jacobian.
00221                           *  The returned matrix must be \f$V \times V\f$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).
00222                           */
00223                         void OnTransitionJacobian( KFMatrix_VxV  &out_F ) const;
00224 
00225                         /** 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.
00226                           */
00227                         void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const;
00228 
00229 
00230                         /** Implements the transition noise covariance \f$ Q_k \f$
00231                           * \param out_Q Must return the covariance matrix.
00232                           *  The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
00233                           */
00234                         void OnTransitionNoise( KFMatrix_VxV &out_Q ) const;
00235 
00236                         /** 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.
00237                           *
00238                           * \param 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.
00239                           * \param 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.
00240                           * \param 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".
00241                           * \param in_lm_indices_in_S The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
00242                           *
00243                           *  This method will be called just once for each complete KF iteration.
00244                           * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
00245                           */
00246                         void OnGetObservationsAndDataAssociation(
00247                                 vector_KFArray_OBS    &out_z,
00248                                 vector_int                  &out_data_association,
00249                                 const vector_KFArray_OBS   &in_all_predictions,
00250                                 const KFMatrix              &in_S,
00251                                 const vector_size_t         &in_lm_indices_in_S,
00252                                 const KFMatrix_OxO          &in_R
00253                                 );
00254 
00255                         void OnObservationModel(
00256                                 const vector_size_t       &idx_landmarks_to_predict,
00257                                 vector_KFArray_OBS  &out_predictions
00258                                 ) const;
00259 
00260                         /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
00261                           * \param 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.
00262                           * \param Hx  The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
00263                           * \param Hy  The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
00264                           */
00265                         void OnObservationJacobians(
00266                                 const size_t &idx_landmark_to_predict,
00267                                 KFMatrix_OxV &Hx,
00268                                 KFMatrix_OxF &Hy
00269                                 ) const;
00270 
00271                         /** 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.
00272                           */
00273                         void OnObservationJacobiansNumericGetIncrements(
00274                                         KFArray_VEH  &out_veh_increments,
00275                                         KFArray_FEAT &out_feat_increments ) const;
00276 
00277 
00278                         /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
00279                           */
00280                         void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const;
00281 
00282                         /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
00283                           * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
00284                           */
00285                         void OnGetObservationNoise(KFMatrix_OxO &out_R) const;
00286 
00287                         /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
00288                           *  For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
00289                           * \param 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.
00290                           * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
00291                           * \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.
00292                           * \sa OnGetObservations, OnDataAssociation
00293                           */
00294                         void OnPreComputingPredictions(
00295                                 const vector_KFArray_OBS        &in_all_prediction_means,
00296                                 vector_size_t                           &out_LM_indices_to_predict ) const;
00297 
00298                         /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
00299                           * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
00300                           * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
00301                           * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
00302                           * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.
00303                           *
00304                           *  - O: OBS_SIZE
00305                           *  - V: VEH_SIZE
00306                           *  - F: FEAT_SIZE
00307                           *
00308                           * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
00309                           */
00310                         void  OnInverseObservationModel(
00311                                 const KFArray_OBS & in_z,
00312                                 KFArray_FEAT  & out_yn,
00313                                 KFMatrix_FxV  & out_dyn_dxv,
00314                                 KFMatrix_FxO  & out_dyn_dhn ) const;
00315 
00316                         /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
00317                           * \param 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.
00318                           * \param 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.
00319                           * \sa OnInverseObservationModel
00320                           */
00321                         void OnNewLandmarkAddedToMap(
00322                                 const size_t    in_obsIdx,
00323                                 const size_t    in_idxNewFeat );
00324 
00325 
00326                         /** 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.
00327                           */
00328                         void OnNormalizeStateVector();
00329 
00330                         /** @}
00331                          */
00332 
00333 
00334                         void getLandmarkIDsFromIndexInStateVector(std::map<unsigned int,CLandmark::TLandmarkID>  &out_id2index) const
00335                         {
00336                                 out_id2index = m_IDs.getInverseMap();
00337                         }
00338 
00339                 protected:
00340 
00341                         /** Set up by processActionObservation
00342                           */
00343                         CActionCollectionPtr    m_action;
00344 
00345                         /** Set up by processActionObservation
00346                           */
00347                         CSensoryFramePtr                m_SF;
00348 
00349                         /** The mapping between landmark IDs and indexes in the Pkk cov. matrix:
00350                           */
00351                         mrpt::utils::bimap<CLandmark::TLandmarkID,unsigned int> m_IDs;
00352 
00353                         /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
00354                           */
00355                         CSimpleMap      m_SFs;
00356 
00357                         TDataAssocInfo m_last_data_association; //!< Last data association
00358 
00359 
00360                 }; // end class
00361         } // End of namespace
00362 } // End of namespace
00363 
00364 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011