00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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/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 00041 #include <mrpt/slam/CSensoryFrame.h> 00042 #include <mrpt/slam/CActionCollection.h> 00043 #include <mrpt/slam/CObservationBearingRange.h> 00044 #include <mrpt/poses/CPoint3D.h> 00045 #include <mrpt/poses/CPose3DPDFGaussian.h> 00046 #include <mrpt/slam/CLandmark.h> 00047 #include <mrpt/slam/CSensFrameProbSequence.h> 00048 #include <mrpt/slam/CIncrementalMapPartitioner.h> 00049 00050 namespace mrpt 00051 { 00052 namespace slam 00053 { 00054 using namespace mrpt::bayes; 00055 00056 /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks. 00057 * The main method is "processActionObservation" which processes pairs of action/observation. 00058 * 00059 * The following Wiki page describes an front-end application based on this class: 00060 * http://babel.isa.uma.es/mrpt/index.php/Application:kf-slam 00061 * 00062 * \sa An implementation for 2D only: CRangeBearingKFSLAM2D 00063 */ 00064 class MRPTDLLIMPEXP CRangeBearingKFSLAM : public utils::CDebugOutputCapable, public bayes::CKalmanFilterCapable 00065 { 00066 public: 00067 /** Constructor. 00068 */ 00069 CRangeBearingKFSLAM( ); 00070 00071 /** Destructor: 00072 */ 00073 virtual ~CRangeBearingKFSLAM(); 00074 00075 void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). 00076 00077 /** 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. 00078 * \param action May contain odometry 00079 * \param SF The set of observations, must contain at least one CObservationBearingRange 00080 */ 00081 void processActionObservation( 00082 CActionCollectionPtr &action, 00083 CSensoryFramePtr &SF ); 00084 00085 /** Returns the complete mean and cov. 00086 * \param out_robotPose The mean & 6x6 covariance matrix of the robot 6D pose 00087 * \param out_landmarksPositions One entry for each of the M landmark positions (3D). 00088 * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. 00089 * \param out_fullState The complete state vector (6+3M). 00090 * \param out_fullCovariance The full (6+3M)x(6+3M) covariance matrix of the filter. 00091 * \sa getCurrentRobotPose 00092 */ 00093 void getCurrentState( 00094 CPose3DPDFGaussian &out_robotPose, 00095 std::vector<CPoint3D> &out_landmarksPositions, 00096 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs, 00097 CVectorDouble &out_fullState, 00098 CMatrixDouble &out_fullCovariance 00099 ) const; 00100 00101 /** Returns the mean & 6x6 covariance matrix of the robot 6D pose. 00102 * \sa getCurrentState 00103 */ 00104 void getCurrentRobotPose( 00105 CPose3DPDFGaussian &out_robotPose ) const; 00106 00107 /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state. 00108 * \param out_objects 00109 */ 00110 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00111 00112 /** Load options from a ini-like file/text 00113 */ 00114 void loadOptions( const mrpt::utils::CConfigFileBase &ini ); 00115 00116 /** The options for the algorithm 00117 */ 00118 struct MRPTDLLIMPEXP TOptions : utils::CLoadableOptions 00119 { 00120 /** Default values 00121 */ 00122 TOptions(); 00123 00124 /** Load from a config file/text 00125 */ 00126 void loadFromConfigFile( 00127 const mrpt::utils::CConfigFileBase &source, 00128 const std::string §ion); 00129 00130 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00131 */ 00132 void dumpToTextStream(CStream &out) const; 00133 00134 /** A 6-length vector with the std. deviation of the transition model in (x,y,z,yaw,pitch,roll) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y z: In meters, yaw pitch roll: radians (but in degrees when loading from a configuration ini-file!) 00135 */ 00136 vector_float stds_Q_no_odo; 00137 00138 /** The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians. 00139 */ 00140 float std_sensor_range, std_sensor_yaw, std_sensor_pitch; 00141 00142 /** 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) 00143 */ 00144 float std_odo_z_additional; 00145 00146 /** If set to true (default=false), map will be partitioned using the method stated by partitioningMethod 00147 */ 00148 bool doPartitioningExperiment; 00149 00150 /** Default = 3 00151 */ 00152 float quantiles_3D_representation; 00153 00154 /** Applicable only if "doPartitioningExperiment=true". 00155 * 0: Automatically detect partition through graph-cut. 00156 * N>=1: Cut every "N" observations. 00157 */ 00158 int partitioningMethod; 00159 00160 bool create_simplemap; //!< Whether to fill m_SFs (default=false) 00161 } options; 00162 00163 /** Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) 00164 * Only if options.doPartitioningExperiment = true 00165 * \sa getLastPartitionLandmarks 00166 */ 00167 void getLastPartition( std::vector<vector_uint> &parts ) 00168 { 00169 parts = m_lastPartitionSet; 00170 } 00171 00172 /** Return the partitioning of the landmarks in clusters accoring to the last partition. 00173 * Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks) 00174 * Only if options.doPartitioningExperiment = true 00175 * \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 !!). 00176 * \sa getLastPartition 00177 */ 00178 void getLastPartitionLandmarks( std::vector<vector_uint> &landmarksMembership ) const; 00179 00180 /** For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used. 00181 */ 00182 void getLastPartitionLandmarksAsIfFixedSubmaps( size_t K, std::vector<vector_uint> &landmarksMembership ); 00183 00184 00185 /** Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks. 00186 * \sa getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps 00187 */ 00188 double computeOffDiagonalBlocksApproximationError( const std::vector<vector_uint> &landmarksMembership ) const; 00189 00190 00191 /** The partitioning of the entire map is recomputed again. 00192 * Only when options.doPartitioningExperiment = true. 00193 * This can be used after changing the parameters of the partitioning method. 00194 * After this method, you can call getLastPartitionLandmarks. 00195 * \sa getLastPartitionLandmarks 00196 */ 00197 void reconsiderPartitionsNow(); 00198 00199 00200 /** Provides access to the parameters of the map partitioning algorithm. 00201 */ 00202 CIncrementalMapPartitioner::TOptions * mapPartitionOptions() 00203 { 00204 return &mapPartitioner.options; 00205 } 00206 00207 /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D 00208 */ 00209 void saveMapAndPath2DRepresentationAsMATLABFile( 00210 const std::string &fil, 00211 float stdCount=3.0f, 00212 const std::string &styleLandmarks = std::string("b"), 00213 const std::string &stylePath = std::string("r"), 00214 const std::string &styleRobot = std::string("r") ) const; 00215 00216 00217 00218 protected: 00219 00220 /** @name Virtual methods for Kalman Filter implementation 00221 @{ 00222 */ 00223 00224 /** Must return the action vector u. 00225 * \param out_u The action vector which will be passed to OnTransitionModel 00226 */ 00227 void OnGetAction( CVectorTemplate<KFTYPE> &out_u ); 00228 00229 /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$ 00230 * \param in_u The vector returned by OnGetAction. 00231 * \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$ . 00232 * \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 00233 */ 00234 void OnTransitionModel( 00235 const CVectorTemplate<KFTYPE>& in_u, 00236 CVectorTemplate<KFTYPE>& inout_x, 00237 bool &out_skipPrediction 00238 ); 00239 00240 /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$ 00241 * \param out_F Must return the Jacobian. 00242 * The returned matrix must be \f$N \times N\f$ with N being either the size of the whole state vector or get_vehicle_size(). 00243 */ 00244 void OnTransitionJacobian( 00245 CMatrixTemplateNumeric<KFTYPE> & out_F 00246 ); 00247 00248 /** Implements the transition noise covariance \f$ Q_k \f$ 00249 * \param out_Q Must return the covariance matrix. 00250 * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian 00251 */ 00252 void OnTransitionNoise( 00253 CMatrixTemplateNumeric<KFTYPE> & out_Q 00254 ); 00255 00256 /** 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. 00257 * \param out_z A \f$N \times O\f$ 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. 00258 * \param 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. 00259 * \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, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. 00260 * This method will be called just once for each complete KF iteration. 00261 */ 00262 void OnGetObservations( 00263 CMatrixTemplateNumeric<KFTYPE> &out_z, 00264 CVectorTemplate<KFTYPE> &out_R2, 00265 vector_int &out_data_association 00266 ); 00267 00268 /** 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. 00269 * \param out_z A \f$N \times O\f$ 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. 00270 * \param 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. 00271 * \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, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. 00272 * This method will be called just once for each complete KF iteration. 00273 * \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. 00274 */ 00275 void OnGetObservations( 00276 CMatrixTemplateNumeric<KFTYPE> &out_z, 00277 CMatrixTemplateNumeric<KFTYPE> &out_R, 00278 vector_int &out_data_association 00279 ) 00280 { 00281 CKalmanFilterCapable::OnGetObservations(out_z,out_R,out_data_association); 00282 } 00283 00284 00285 /** Implements the observation prediction \f$ h_i(x) \f$ and the Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$. 00286 * \param in_z This is the same matrix returned by OnGetObservations(), passed here for reference. 00287 * \param in_data_association The vector returned by OnGetObservations(), passed here for reference. 00288 * \param in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx. 00289 * \param 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. 00290 * \param out_innov The difference between the expected observation and the real one: \f$ \tilde{y} = z - h(x) \f$. 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. 00291 * \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$. 00292 * \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$. 00293 * 00294 * out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked \f$O \times V\f$ matrices, and out_Hy must consist of 1 or N \f$O \times F\f$ matrices, with: 00295 * - N: number of rows in in_z (number of observed features, or 1 in general). 00296 * - O: get_observation_size() 00297 * - V: get_vehicle_size() 00298 * - F: get_feature_size() 00299 */ 00300 void OnObservationModelAndJacobians( 00301 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00302 const vector_int &in_data_association, 00303 const bool &in_full, 00304 const int &in_obsIdx, 00305 CVectorTemplate<KFTYPE> &out_innov, 00306 CMatrixTemplateNumeric<KFTYPE> &out_Hx, 00307 CMatrixTemplateNumeric<KFTYPE> &out_Hy 00308 ); 00309 00310 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". 00311 * \param in_z This is the same matrix returned by OnGetObservations(). 00312 * \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. 00313 * \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. 00314 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$. 00315 * \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$. 00316 * \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$. 00317 * 00318 * - O: get_observation_size() 00319 * - V: get_vehicle_size() 00320 * - F: get_feature_size() 00321 */ 00322 void OnInverseObservationModel( 00323 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00324 const size_t &in_obsIdx, 00325 const size_t &in_idxNewFeat, 00326 CVectorTemplate<KFTYPE> &out_yn, 00327 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dxv, 00328 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dhn ); 00329 00330 00331 /** 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. 00332 */ 00333 void OnNormalizeStateVector(); 00334 00335 /** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable. 00336 */ 00337 size_t get_vehicle_size() const { return 6; /* x y z yaw pitch roll*/ } 00338 00339 /** Must return the dimension of the features in the system state (the "map"), or 0 if not applicable. 00340 */ 00341 size_t get_feature_size() const { return 3; /* x y z */ } 00342 00343 /** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc). 00344 */ 00345 size_t get_observation_size() const { return 3; /* range yaw pitch */ } 00346 00347 /** @} 00348 */ 00349 00350 /** Set up by processActionObservation 00351 */ 00352 CActionCollectionPtr m_action; 00353 00354 /** Set up by processActionObservation 00355 */ 00356 CSensoryFramePtr m_SF; 00357 00358 /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: 00359 */ 00360 std::map<CLandmark::TLandmarkID,unsigned int> m_IDs; 00361 00362 /** The mapping between indexes in the Pkk cov. matrix and landmark IDs: 00363 */ 00364 std::map<unsigned int,CLandmark::TLandmarkID> m_IDs_inverse; 00365 00366 00367 /** Used for map partitioning experiments 00368 */ 00369 CIncrementalMapPartitioner mapPartitioner; 00370 00371 /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc) 00372 */ 00373 CSensFrameProbSequence m_SFs; 00374 00375 std::vector<vector_uint> m_lastPartitionSet; 00376 00377 00378 }; // end class 00379 } // End of namespace 00380 } // End of namespace 00381 00382 #endif
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |