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 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 00041 #include <mrpt/slam/CSensoryFrame.h> 00042 #include <mrpt/slam/CActionCollection.h> 00043 #include <mrpt/slam/CObservationBearingRange.h> 00044 #include <mrpt/poses/CPoint2D.h> 00045 #include <mrpt/poses/CPosePDFGaussian.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 using namespace mrpt::poses; 00056 00057 /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks. 00058 * The main method is "processActionObservation" which processes pairs of action/observation. 00059 * 00060 * The following Wiki page describes an front-end application based on this class: 00061 * http://babel.isa.uma.es/mrpt/index.php/Application:2d-slam-demo 00062 * 00063 * \sa CRangeBearingKFSLAM 00064 */ 00065 class MRPTDLLIMPEXP CRangeBearingKFSLAM2D : public utils::CDebugOutputCapable, public bayes::CKalmanFilterCapable 00066 { 00067 public: 00068 CRangeBearingKFSLAM2D( ); //!< Default constructor 00069 virtual ~CRangeBearingKFSLAM2D(); //!< Destructor 00070 void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). 00071 00072 /** 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. 00073 * \param action May contain odometry 00074 * \param SF The set of observations, must contain at least one CObservationBearingRange 00075 */ 00076 void processActionObservation( 00077 CActionCollectionPtr &action, 00078 CSensoryFramePtr &SF ); 00079 00080 /** Returns the complete mean and cov. 00081 * \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D pose 00082 * \param out_landmarksPositions One entry for each of the M landmark positions (2D). 00083 * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. 00084 * \param out_fullState The complete state vector (3+2M). 00085 * \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of the filter. 00086 * \sa getCurrentRobotPose 00087 */ 00088 void getCurrentState( 00089 CPosePDFGaussian &out_robotPose, 00090 std::vector<TPoint2D> &out_landmarksPositions, 00091 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs, 00092 CVectorDouble &out_fullState, 00093 CMatrixDouble &out_fullCovariance 00094 ) const; 00095 00096 /** Returns the mean & 3x3 covariance matrix of the robot 2D pose. 00097 * \sa getCurrentState 00098 */ 00099 void getCurrentRobotPose( 00100 CPosePDFGaussian &out_robotPose ) const; 00101 00102 /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state. 00103 * \param out_objects 00104 */ 00105 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00106 00107 /** Load options from a ini-like file/text 00108 */ 00109 void loadOptions( const mrpt::utils::CConfigFileBase &ini ); 00110 00111 /** The options for the algorithm 00112 */ 00113 struct MRPTDLLIMPEXP TOptions : utils::CLoadableOptions 00114 { 00115 /** Default values 00116 */ 00117 TOptions(); 00118 00119 /** Load from a config file/text 00120 */ 00121 void loadFromConfigFile( 00122 const mrpt::utils::CConfigFileBase &source, 00123 const std::string §ion); 00124 00125 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00126 */ 00127 void dumpToTextStream(CStream &out) const; 00128 00129 00130 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!) 00131 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. 00132 float quantiles_3D_representation; //!< Default = 3 00133 bool create_simplemap; //!< Whether to fill m_SFs (default=false) 00134 00135 }; 00136 00137 TOptions options; //!< The options for the algorithm 00138 00139 00140 /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D 00141 */ 00142 void saveMapAndPath2DRepresentationAsMATLABFile( 00143 const std::string &fil, 00144 float stdCount=3.0f, 00145 const std::string &styleLandmarks = std::string("b"), 00146 const std::string &stylePath = std::string("r"), 00147 const std::string &styleRobot = std::string("r") ) const; 00148 00149 protected: 00150 00151 /** @name Virtual methods for Kalman Filter implementation 00152 @{ 00153 */ 00154 00155 /** Must return the action vector u. 00156 * \param out_u The action vector which will be passed to OnTransitionModel 00157 */ 00158 void OnGetAction( CVectorTemplate<KFTYPE> &out_u ); 00159 00160 /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$ 00161 * \param in_u The vector returned by OnGetAction. 00162 * \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$ . 00163 * \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 00164 */ 00165 void OnTransitionModel( 00166 const CVectorTemplate<KFTYPE>& in_u, 00167 CVectorTemplate<KFTYPE>& inout_x, 00168 bool &out_skipPrediction 00169 ); 00170 00171 /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$ 00172 * \param out_F Must return the Jacobian. 00173 * 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(). 00174 */ 00175 void OnTransitionJacobian( 00176 CMatrixTemplateNumeric<KFTYPE> & out_F 00177 ); 00178 00179 /** Implements the transition noise covariance \f$ Q_k \f$ 00180 * \param out_Q Must return the covariance matrix. 00181 * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian 00182 */ 00183 void OnTransitionNoise( 00184 CMatrixTemplateNumeric<KFTYPE> & out_Q 00185 ); 00186 00187 /** 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. 00188 * \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. 00189 * \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. 00190 * \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. 00191 * This method will be called just once for each complete KF iteration. 00192 */ 00193 void OnGetObservations( 00194 CMatrixTemplateNumeric<KFTYPE> &out_z, 00195 CVectorTemplate<KFTYPE> &out_R2, 00196 vector_int &out_data_association 00197 ); 00198 00199 /** 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. 00200 * \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. 00201 * \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. 00202 * \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. 00203 * This method will be called just once for each complete KF iteration. 00204 * \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. 00205 */ 00206 void OnGetObservations( 00207 CMatrixTemplateNumeric<KFTYPE> &out_z, 00208 CMatrixTemplateNumeric<KFTYPE> &out_R, 00209 vector_int &out_data_association 00210 ) 00211 { 00212 CKalmanFilterCapable::OnGetObservations(out_z,out_R,out_data_association); 00213 } 00214 00215 00216 /** 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$. 00217 * \param in_z This is the same matrix returned by OnGetObservations(), passed here for reference. 00218 * \param in_data_association The vector returned by OnGetObservations(), passed here for reference. 00219 * \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. 00220 * \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. 00221 * \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. 00222 * \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$. 00223 * \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$. 00224 * 00225 * 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: 00226 * - N: number of rows in in_z (number of observed features, or 1 in general). 00227 * - O: get_observation_size() 00228 * - V: get_vehicle_size() 00229 * - F: get_feature_size() 00230 */ 00231 void OnObservationModelAndJacobians( 00232 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00233 const vector_int &in_data_association, 00234 const bool &in_full, 00235 const int &in_obsIdx, 00236 CVectorTemplate<KFTYPE> &out_innov, 00237 CMatrixTemplateNumeric<KFTYPE> &out_Hx, 00238 CMatrixTemplateNumeric<KFTYPE> &out_Hy 00239 ); 00240 00241 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". 00242 * \param in_z This is the same matrix returned by OnGetObservations(). 00243 * \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. 00244 * \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. 00245 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$. 00246 * \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$. 00247 * \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$. 00248 * 00249 * - O: get_observation_size() 00250 * - V: get_vehicle_size() 00251 * - F: get_feature_size() 00252 */ 00253 void OnInverseObservationModel( 00254 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00255 const size_t &in_obsIdx, 00256 const size_t &in_idxNewFeat, 00257 CVectorTemplate<KFTYPE> &out_yn, 00258 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dxv, 00259 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dhn ); 00260 00261 00262 /** 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. 00263 */ 00264 void OnNormalizeStateVector(); 00265 00266 /** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable. 00267 */ 00268 size_t get_vehicle_size() const { return 3; /* x y phi */ } 00269 00270 /** Must return the dimension of the features in the system state (the "map"), or 0 if not applicable. 00271 */ 00272 size_t get_feature_size() const { return 2; /* x y */ } 00273 00274 /** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc). 00275 */ 00276 size_t get_observation_size() const { return 2; /* range yaw */ } 00277 00278 /** @} 00279 */ 00280 00281 /** Set up by processActionObservation 00282 */ 00283 CActionCollectionPtr m_action; 00284 00285 /** Set up by processActionObservation 00286 */ 00287 CSensoryFramePtr m_SF; 00288 00289 /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: 00290 */ 00291 std::map<CLandmark::TLandmarkID,unsigned int> m_IDs; 00292 00293 /** The mapping between indexes in the Pkk cov. matrix and landmark IDs: 00294 */ 00295 std::map<unsigned int,CLandmark::TLandmarkID> m_IDs_inverse; 00296 00297 /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc) 00298 */ 00299 CSensFrameProbSequence m_SFs; 00300 00301 00302 }; // end class 00303 } // End of namespace 00304 } // End of namespace 00305 00306 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |