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