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 CRangeBearingKFSLAM_H 00029 #define CRangeBearingKFSLAM_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/CPoint3D.h> 00046 #include <mrpt/poses/CPose3DPDFGaussian.h> 00047 #include <mrpt/poses/CPose3DQuatPDFGaussian.h> 00048 #include <mrpt/slam/CLandmark.h> 00049 #include <mrpt/slam/CSimpleMap.h> 00050 #include <mrpt/slam/CIncrementalMapPartitioner.h> 00051 #include <mrpt/slam/data_association.h> 00052 00053 #include <mrpt/slam/link_pragmas.h> 00054 00055 namespace mrpt 00056 { 00057 namespace slam 00058 { 00059 using namespace mrpt::bayes; 00060 00061 /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks. 00062 * The main method is "processActionObservation" which processes pairs of action/observation. 00063 * The state vector comprises: 3D robot position, a quaternion for its attitude, and the 3D landmarks in the map. 00064 * 00065 * The following Wiki page describes an front-end application based on this class: 00066 * http://www.mrpt.org/Application:kf-slam 00067 * 00068 * For the theory behind this implementation, see the technical report in: 00069 * http://www.mrpt.org/6D-SLAM 00070 * 00071 * \sa An implementation for 2D only: CRangeBearingKFSLAM2D 00072 */ 00073 class SLAM_IMPEXP CRangeBearingKFSLAM : 00074 public bayes::CKalmanFilterCapable<7 /* x y z qr qx qy qz*/,3 /* range yaw pitch */, 3 /* x y z */, 7 /* Ax Ay Az Aqr Aqx Aqy Aqz */ > 00075 // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double> 00076 { 00077 public: 00078 /** Constructor. 00079 */ 00080 CRangeBearingKFSLAM( ); 00081 00082 /** Destructor: 00083 */ 00084 virtual ~CRangeBearingKFSLAM(); 00085 00086 void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). 00087 00088 /** 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. 00089 * \param action May contain odometry 00090 * \param SF The set of observations, must contain at least one CObservationBearingRange 00091 */ 00092 void processActionObservation( 00093 CActionCollectionPtr &action, 00094 CSensoryFramePtr &SF ); 00095 00096 /** Returns the complete mean and cov. 00097 * \param out_robotPose The mean and the 7x7 covariance matrix of the robot 6D pose 00098 * \param out_landmarksPositions One entry for each of the M landmark positions (3D). 00099 * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. 00100 * \param out_fullState The complete state vector (7+3M). 00101 * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of the filter. 00102 * \sa getCurrentRobotPose 00103 */ 00104 void getCurrentState( 00105 CPose3DQuatPDFGaussian &out_robotPose, 00106 std::vector<CPoint3D> &out_landmarksPositions, 00107 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs, 00108 CVectorDouble &out_fullState, 00109 CMatrixDouble &out_fullCovariance 00110 ) const; 00111 00112 /** Returns the complete mean and cov. 00113 * \param out_robotPose The mean and the 7x7 covariance matrix of the robot 6D pose 00114 * \param out_landmarksPositions One entry for each of the M landmark positions (3D). 00115 * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. 00116 * \param out_fullState The complete state vector (7+3M). 00117 * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of the filter. 00118 * \sa getCurrentRobotPose 00119 */ 00120 inline void getCurrentState( 00121 CPose3DPDFGaussian &out_robotPose, 00122 std::vector<CPoint3D> &out_landmarksPositions, 00123 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs, 00124 CVectorDouble &out_fullState, 00125 CMatrixDouble &out_fullCovariance 00126 ) const 00127 { 00128 CPose3DQuatPDFGaussian q(UNINITIALIZED_QUATERNION); 00129 this->getCurrentState(q,out_landmarksPositions,out_landmarkIDs,out_fullState,out_fullCovariance); 00130 out_robotPose = CPose3DPDFGaussian(q); 00131 } 00132 00133 /** Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion). 00134 * \sa getCurrentState, getCurrentRobotPoseMean 00135 */ 00136 void getCurrentRobotPose( CPose3DQuatPDFGaussian &out_robotPose ) const; 00137 00138 /** Get the current robot pose mean, as a 3D+quaternion pose. 00139 * \sa getCurrentRobotPose 00140 */ 00141 mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const; 00142 00143 /** Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles). 00144 * \sa getCurrentState 00145 */ 00146 inline void getCurrentRobotPose( CPose3DPDFGaussian &out_robotPose ) const 00147 { 00148 CPose3DQuatPDFGaussian q(UNINITIALIZED_QUATERNION); 00149 this->getCurrentRobotPose(q); 00150 out_robotPose = CPose3DPDFGaussian(q); 00151 } 00152 00153 /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state. 00154 * \param out_objects 00155 */ 00156 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00157 00158 /** Load options from a ini-like file/text 00159 */ 00160 void loadOptions( const mrpt::utils::CConfigFileBase &ini ); 00161 00162 /** The options for the algorithm 00163 */ 00164 struct SLAM_IMPEXP TOptions : utils::CLoadableOptions 00165 { 00166 /** Default values 00167 */ 00168 TOptions(); 00169 00170 /** Load from a config file/text 00171 */ 00172 void loadFromConfigFile( 00173 const mrpt::utils::CConfigFileBase &source, 00174 const std::string §ion); 00175 00176 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00177 */ 00178 void dumpToTextStream(CStream &out) const; 00179 00180 /** A 7-length vector with the std. deviation of the transition model in (x,y,z, qr,qx,qy,qz) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y z: In meters. 00181 */ 00182 vector_float stds_Q_no_odo; 00183 00184 /** The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians. 00185 */ 00186 float std_sensor_range, std_sensor_yaw, std_sensor_pitch; 00187 00188 /** Additional std. dev. to sum to the motion model in the z axis (useful when there is only 2D odometry and we want to put things hard to the algorithm) (default=0) 00189 */ 00190 float std_odo_z_additional; 00191 00192 /** If set to true (default=false), map will be partitioned using the method stated by partitioningMethod 00193 */ 00194 bool doPartitioningExperiment; 00195 00196 /** Default = 3 00197 */ 00198 float quantiles_3D_representation; 00199 00200 /** Applicable only if "doPartitioningExperiment=true". 00201 * 0: Automatically detect partition through graph-cut. 00202 * N>=1: Cut every "N" observations. 00203 */ 00204 int partitioningMethod; 00205 00206 // Data association: 00207 TDataAssociationMethod data_assoc_method; 00208 TDataAssociationMetric data_assoc_metric; 00209 double data_assoc_IC_chi2_thres; //!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99) 00210 TDataAssociationMetric data_assoc_IC_metric; //!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood. 00211 double data_assoc_IC_ml_threshold;//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0) 00212 00213 bool create_simplemap; //!< Whether to fill m_SFs (default=false) 00214 00215 bool force_ignore_odometry; //!< Whether to ignore the input odometry and behave as if there was no odometry at all (default: false) 00216 } options; 00217 00218 /** Information for data-association: 00219 * \sa getLastDataAssociation 00220 */ 00221 struct SLAM_IMPEXP TDataAssocInfo 00222 { 00223 TDataAssocInfo() : 00224 Y_pred_means(0,0), 00225 Y_pred_covs(0,0) 00226 { 00227 } 00228 00229 void clear() { 00230 results.clear(); 00231 predictions_IDs.clear(); 00232 newly_inserted_landmarks.clear(); 00233 } 00234 00235 // Predictions from the map: 00236 CMatrixTemplateNumeric<kftype> Y_pred_means,Y_pred_covs; 00237 mrpt::vector_size_t predictions_IDs; 00238 00239 /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector) 00240 Only used for stats and so. */ 00241 std::map<size_t,size_t> newly_inserted_landmarks; 00242 00243 // DA results: 00244 TDataAssociationResults results; 00245 }; 00246 00247 /** Returns a read-only reference to the information on the last data-association */ 00248 const TDataAssocInfo & getLastDataAssociation() const { 00249 return m_last_data_association; 00250 } 00251 00252 00253 /** Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) 00254 * Only if options.doPartitioningExperiment = true 00255 * \sa getLastPartitionLandmarks 00256 */ 00257 void getLastPartition( std::vector<vector_uint> &parts ) 00258 { 00259 parts = m_lastPartitionSet; 00260 } 00261 00262 /** Return the partitioning of the landmarks in clusters accoring to the last partition. 00263 * Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks) 00264 * Only if options.doPartitioningExperiment = true 00265 * \param 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 !!). 00266 * \sa getLastPartition 00267 */ 00268 void getLastPartitionLandmarks( std::vector<vector_uint> &landmarksMembership ) const; 00269 00270 /** For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used. 00271 */ 00272 void getLastPartitionLandmarksAsIfFixedSubmaps( size_t K, std::vector<vector_uint> &landmarksMembership ); 00273 00274 00275 /** Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks. 00276 * \sa getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps 00277 */ 00278 double computeOffDiagonalBlocksApproximationError( const std::vector<vector_uint> &landmarksMembership ) const; 00279 00280 00281 /** The partitioning of the entire map is recomputed again. 00282 * Only when options.doPartitioningExperiment = true. 00283 * This can be used after changing the parameters of the partitioning method. 00284 * After this method, you can call getLastPartitionLandmarks. 00285 * \sa getLastPartitionLandmarks 00286 */ 00287 void reconsiderPartitionsNow(); 00288 00289 00290 /** Provides access to the parameters of the map partitioning algorithm. 00291 */ 00292 CIncrementalMapPartitioner::TOptions * mapPartitionOptions() 00293 { 00294 return &mapPartitioner.options; 00295 } 00296 00297 /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D 00298 */ 00299 void saveMapAndPath2DRepresentationAsMATLABFile( 00300 const std::string &fil, 00301 float stdCount=3.0f, 00302 const std::string &styleLandmarks = std::string("b"), 00303 const std::string &stylePath = std::string("r"), 00304 const std::string &styleRobot = std::string("r") ) const; 00305 00306 00307 00308 protected: 00309 00310 /** @name Virtual methods for Kalman Filter implementation 00311 @{ 00312 */ 00313 00314 /** Must return the action vector u. 00315 * \param out_u The action vector which will be passed to OnTransitionModel 00316 */ 00317 void OnGetAction( KFArray_ACT &out_u ) const; 00318 00319 /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$ 00320 * \param in_u The vector returned by OnGetAction. 00321 * \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$ . 00322 * \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 00323 */ 00324 void OnTransitionModel( 00325 const KFArray_ACT &in_u, 00326 KFArray_VEH &inout_x, 00327 bool &out_skipPrediction 00328 ) const; 00329 00330 /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$ 00331 * \param out_F Must return the Jacobian. 00332 * 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). 00333 */ 00334 void OnTransitionJacobian( KFMatrix_VxV &out_F ) const; 00335 00336 /** Implements the transition noise covariance \f$ Q_k \f$ 00337 * \param out_Q Must return the covariance matrix. 00338 * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian 00339 */ 00340 void OnTransitionNoise( KFMatrix_VxV &out_Q ) const; 00341 00342 /** 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. 00343 * 00344 * \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. 00345 * \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. 00346 * \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". 00347 * \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. 00348 * 00349 * This method will be called just once for each complete KF iteration. 00350 * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them. 00351 */ 00352 void OnGetObservationsAndDataAssociation( 00353 vector_KFArray_OBS &out_z, 00354 vector_int &out_data_association, 00355 const vector_KFArray_OBS &in_all_predictions, 00356 const KFMatrix &in_S, 00357 const vector_size_t &in_lm_indices_in_S, 00358 const KFMatrix_OxO &in_R 00359 ); 00360 00361 void OnObservationModel( 00362 const vector_size_t &idx_landmarks_to_predict, 00363 vector_KFArray_OBS &out_predictions 00364 ) const; 00365 00366 /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$. 00367 * \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. 00368 * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$. 00369 * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$. 00370 */ 00371 void OnObservationJacobians( 00372 const size_t &idx_landmark_to_predict, 00373 KFMatrix_OxV &Hx, 00374 KFMatrix_OxF &Hy 00375 ) const; 00376 00377 /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). 00378 */ 00379 void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const; 00380 00381 /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. 00382 * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be. 00383 */ 00384 void OnGetObservationNoise(KFMatrix_OxO &out_R) const; 00385 00386 /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. 00387 * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations. 00388 * \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. 00389 * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted. 00390 * \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. 00391 * \sa OnGetObservations, OnDataAssociation 00392 */ 00393 void OnPreComputingPredictions( 00394 const vector_KFArray_OBS &in_all_prediction_means, 00395 vector_size_t &out_LM_indices_to_predict ) const; 00396 00397 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". 00398 * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations(). 00399 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$. 00400 * \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$. 00401 * \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$. 00402 * 00403 * - O: OBS_SIZE 00404 * - V: VEH_SIZE 00405 * - F: FEAT_SIZE 00406 * 00407 * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map. 00408 */ 00409 void OnInverseObservationModel( 00410 const KFArray_OBS & in_z, 00411 KFArray_FEAT & out_yn, 00412 KFMatrix_FxV & out_dyn_dxv, 00413 KFMatrix_FxO & out_dyn_dhn ) const; 00414 00415 /** If applicable to the given problem, do here any special handling of adding a new landmark to the map. 00416 * \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. 00417 * \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. 00418 * \sa OnInverseObservationModel 00419 */ 00420 void OnNewLandmarkAddedToMap( 00421 const size_t in_obsIdx, 00422 const size_t in_idxNewFeat ); 00423 00424 00425 /** 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. 00426 */ 00427 void OnNormalizeStateVector(); 00428 00429 /** @} 00430 */ 00431 00432 /** Set up by processActionObservation 00433 */ 00434 CActionCollectionPtr m_action; 00435 00436 /** Set up by processActionObservation 00437 */ 00438 CSensoryFramePtr m_SF; 00439 00440 /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: 00441 */ 00442 mrpt::utils::bimap<CLandmark::TLandmarkID,unsigned int> m_IDs; 00443 00444 00445 /** Used for map partitioning experiments 00446 */ 00447 CIncrementalMapPartitioner mapPartitioner; 00448 00449 /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc) 00450 */ 00451 CSimpleMap m_SFs; 00452 00453 std::vector<vector_uint> m_lastPartitionSet; 00454 00455 TDataAssocInfo m_last_data_association; //!< Last data association 00456 00457 /** Return the last odometry, as a pose increment. */ 00458 mrpt::poses::CPose3DQuat getIncrementFromOdometry() const; 00459 00460 00461 }; // end class 00462 } // End of namespace 00463 } // End of namespace 00464 00465 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |