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 Perception and Robotics | 00009 | research group, 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 CMonoSlam_H 00029 #define CMonoSlam_H 00030 00031 #ifndef MONO_SLAM_APP 00032 #define MONO_SLAM_APP 00033 #endif 00034 00035 #include <mrpt/math/CMatrixTemplateNumeric.h> 00036 #include <mrpt/math/CVectorTemplate.h> 00037 #include <mrpt/math/CQuaternion.h> 00038 #include <mrpt/math/lightweight_geom_data.h> 00039 #include <mrpt/vision/CCamModel.h> 00040 #include <mrpt/vision/CFeatureExtraction.h> 00041 #include <mrpt/bayes/CKalmanFilterCapable.h> 00042 #include <mrpt/utils/CConfigFile.h> 00043 #include <mrpt/utils/CImage.h> 00044 #include <mrpt/monoslam/CMonoSlam.h> 00045 #include <mrpt/vision/CFeature.h> 00046 #include <mrpt/hwdrivers/CCameraSensor.h> 00047 00048 #include <mrpt/monoslam/CFeature.h> 00049 #include <mrpt/monoslam/CMonoSlamMotionModel.h> 00050 #include <mrpt/monoslam/link_pragmas.h> 00051 00052 namespace mrpt 00053 { 00054 /** Monocular SLAM classes (experimental!) 00055 */ 00056 namespace monoslam 00057 { 00058 using namespace mrpt::math; 00059 using namespace mrpt::poses; 00060 using namespace mrpt::slam; 00061 00062 /** The main class which implements Monocular SLAM (Mono-SLAM), either from a real camera or offline data. 00063 * This class just contains the algorithm implementation, it's not a GUI. For directly usable programs, see "apps/mono-slam". 00064 * 00065 * The basic usage would be like this: 00066 * - Set all the parameters as desired in "CMonoSlam::options" and "KF_options" (CKalmanFilterCapable::KF_options). This can be done manually or by calling CMonoSlam::loadParameters 00067 * - Setup a video source in CMonoSlam::m_camera 00068 * - Call "go_one_step" to process incoming image frames 00069 * - If desired, use the provided 2D/3D renders of the filter state in "m_progress2D" & "m_progress3D" 00070 * 00071 * The configuration file (.ini-like file) may have these sections: 00072 * \code 00073 * [MONOSLAM] 00074 * match_threshold_up = // Top threshold for feature classification 00075 * match_threshold_down = // Botton threshold for feature classification 00076 * ...// All the variables in mrpt::monoslam::CMonoSlam::TOptions 00077 * 00078 * [MOTION_MODEL] 00079 * sigma_lin_vel = 6.0 // linear velocity noise in m/s^2 00080 * sigma_ang_vel = 6.0 // angular velocity noise in rad/s^2 00081 * 00082 * [KALMAN] 00083 * method = 0 // 0:Full KF, 1:Efficient KF, 2: Full IKF, 3: Efficient IKF 00084 * ... // All the variables in mrpt::bayes::CKalmanFilterCapable::TKF_options 00085 * 00086 * [CAMERA_MODEL] 00087 * .... 00088 * 00089 * \endcode 00090 * 00091 * 00092 * 00093 * History of changes: 00094 * <ul> 00095 * <li>2007:<br> 00096 * The first version of this application, the corresponding classes 00097 * and config files were developed by Antonio J. Ortiz de Galisteo 00098 * as part of his Master Thesis at the University of Malaga (2007), 00099 * based on public sources released by Andrew Davison (Imperial College London). 00100 * </li> 00101 * 00102 * <li>2009:<br> 00103 * mono-slam updated and integrated back into MRPT by M.A. Amiri Atashgah (MAAA) 00104 * at the University of Sharif Technology, Tehran, Iran (2009), and 00105 * Jose Luis Blanco (University of Malaga). 00106 * </li> 00107 * </ul> 00108 */ 00109 class MONOSLAMDLLIMPEXP CMonoSlam :public mrpt::bayes::CKalmanFilterCapable 00110 { 00111 public: 00112 /** This is the source of images. It MUST be filled before starting execution of Mono-SLAM 00113 * This smart pointer can be created from: 00114 * - Manually and loaded from a custom INI file or programatically set parameters. 00115 * - mrpt::hwdrivers::prepareVideoSourceFromUserSelection 00116 * - mrpt::hwdrivers::prepareVideoSourceFromPanel 00117 * Before executing MonoSLAM, call m_camera->initialize() 00118 */ 00119 mrpt::hwdrivers::CCameraSensorPtr m_camera; 00120 00121 /** All the options of Mono-SLAM, in a structure which can be changed manually or saved/loaded to/from a .ini file 00122 * \sa KF_options 00123 */ 00124 struct MONOSLAMDLLIMPEXP TOptions : public mrpt::utils::CLoadableOptions 00125 { 00126 /** Initilization of default parameters */ 00127 TOptions(); 00128 /** See utils::CLoadableOptions */ 00129 void loadFromConfigFile( const mrpt::utils::CConfigFileBase &source, const std::string §ion); 00130 /** See utils::CLoadableOptions */ 00131 void dumpToTextStream(CStream &out) const; 00132 00133 bool verbose; //!< Show extra debug info to console 00134 bool save_data_files; //!< Save debug info to text files 00135 double match_threshold_up; //!< Top threshold for feature classification 00136 double match_threshold_down; //!< Botton threshold for feature classification 00137 double threshold_ini; //!< Minimum quality for a feature in the initialization 00138 double threshold_new; //!< Minimum quality for a feature in the searching of a new mark 00139 int radius; //!< Minimum radius between detected image features 00140 double sigma; //!< Sigma value in the Harris method 00141 unsigned int max_num_feat_to_detect ; //!< max num of features to detect 00142 00143 double lambda_init; //!< Initial Inverse Depth estimation the first time a feature is observed 00144 double std_lambda; //!< Inverse Depth Variance for the first time a feature is observed 00145 double sigma_image_noise; //!< image noise in pixels 00146 }; 00147 00148 TOptions options; //!< All the options of Mono-SLAM, in a structure which can be changed manually or saved/loaded to/from a .ini file \sa KF_options, loadParameters 00149 00150 /** With each step, "go_one_step" will save the 2D image with the detected features, etc to this image so the external program can show it to the user. */ 00151 CImage m_progress2D; 00152 00153 /** With each step, "go_one_step" will save the 3D scene with the filter state to this scene object so the external program can show it to the user. */ 00154 opengl::COpenGLScenePtr m_progress3D; 00155 00156 CMatrixDouble m_robotPath; //!< robot 3D Pose 00157 double m_simTime; //!< 00158 00159 /** Constructor */ 00160 CMonoSlam(); 00161 00162 /** Dtor */ 00163 virtual ~CMonoSlam(); 00164 00165 /** @name The main interface methods for the final user 00166 @{ 00167 */ 00168 00169 /** This is the main method from the user viewpoint in Mono-SLAM: it is passed a pair Action/Observation (action can be empty, observation must contains at least 1 image), and one full Kalman Filter step will be executed. 00170 * This method does: 00171 * - Grab a new image frame from the camera (See m_camera). 00172 * - Search Features if we have less than necessaries 00173 * - EKF Prediction Step 00174 * - Matching/Tracking of Features 00175 * - EKF Update Step 00176 * - Show/Save Results 00177 * 00178 * \param optional_odometry This may contain odometry, if the robot has it. Left as an empty smart pointer to just rely on images (default). 00179 */ 00180 void go_one_step(const CActionCollectionPtr &optional_odometry = CActionCollectionPtr() ); 00181 00182 /** Load all the parameters for Mono-SLAM, Kalman-Filter, and the motion model. 00183 * \sa saveParameters 00184 */ 00185 void loadParameters( const mrpt::utils::CConfigFileBase &source); 00186 00187 /** Save all the parameters for Mono-SLAM, Kalman-Filter, and the motion model. 00188 * 00189 * \sa loadParameters 00190 */ 00191 void saveParameters( mrpt::utils::CConfigFileBase &config); 00192 00193 /** Resets the filter state: the camera pose is resetted and the map is emptied. 00194 */ 00195 void resetFilterState(); 00196 00197 /** @} */ 00198 00199 00200 protected: 00201 int m_iterations_count; 00202 unsigned int m_num_obj_visible; 00203 CMonoSlamMotionModel mmm; //!< MonoSLAMMotionModel 00204 mrpt::vision::CCamModel cam; //!< Camera model 00205 //unsigned int m_min_num_feat ; //!< min num of features to detect 00206 00207 double prev_speed; //speed of previous iteration (for avoid strong movements) 00208 CVectorDouble prev_xv; //state of previous iteration 00209 00210 static const size_t state_size; //!< State Vector Size 00211 static const size_t feature_size; //!< Feature Vector Size 00212 00213 /*Members*/ 00214 00215 //MAAA--------------------------------------- 00216 //unsigned int m_type; // JL: This is "KF_options.type" 00217 //bool m_bScaleHalf,m_bGrayScaleProcessing; // This will be controlled from outside 00218 00219 mrpt::system::TTimeStamp m_dFrameTimeStamp; 00220 mrpt::math::CVectorDouble m_match; 00221 00222 /** Set up by go_one_step, it contains the GRAYSCALE image grabbed from the video or live camera so it can be used in the KF methods. */ 00223 mrpt::utils::CImage m_frame; 00224 00225 /** If given by the user, set up by go_one_step, it may contain odometry */ 00226 mrpt::slam::CActionCollectionPtr m_action; 00227 00228 // JL: Needed in the future?? 00229 /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: */ 00230 //std::map<CLandmark::TLandmarkID,unsigned int> m_IDs; 00231 /** The mapping between indexes in the Pkk cov. matrix and landmark IDs: */ 00232 //std::map<unsigned int,CLandmark::TLandmarkID> m_IDs_inverse; 00233 00234 //--------------------------------------- 00235 //int store_index; //!< Index for data saving files // --> m_iterations_count 00236 00237 mrpt::monoslam::CFeaturePatch m_FeatPatch; 00238 00239 mrpt::system::TTimeStamp m_timeLastFrame; //!< Used to estimate FPS of the video source 00240 00241 // CTicTac m_DeltaTimeCounter; //!< Counter used to estimate FPS of the video source 00242 //int sim_or_rob; // --> Done at CCameraSensor level 00243 //float m_dt; // --> mmm.delta_m 00244 00245 //gui::CDisplayWindowPtr wind2D; // --> Managed by the GUI or console app. 00246 //gui::CDisplayWindow3DPtr wind3D; // --> Managed by the GUI or console app. 00247 00248 /** Show results (features selected and tracked) over actual frame. */ 00249 void plot_results_2D(mrpt::math::CMatrixDouble &zz, 00250 mrpt::math::CMatrixDouble &hh, 00251 mrpt::math::CMatrixDouble &h, 00252 mrpt::math::CMatrixDouble &S, 00253 mrpt::math::CMatrixDouble &XYZ, 00254 mrpt::utils::CImage &framecolor, 00255 unsigned int type, 00256 mrpt::math::CVectorDouble &match, 00257 bool verbose, 00258 bool savedatas); 00259 00260 00261 /** Show the camera and feature estimated position in a 3D world */ 00262 void plot_results_3D(mrpt::math::CMatrixDouble &XYZ); 00263 00264 00265 /** This function is only a short method for plot in 3D windows */ 00266 /* 00267 void plot3D() 00268 { 00269 if (!wind3D) wind3D = new UTILS::CDisplayWindow3D("3D WORLD MONOSLAM",960,590); 00270 wind3D->unlockAccess3DScene(); 00271 world = wind3D->get3DSceneAndLock(); 00272 wind3D->unlockAccess3DScene(); 00273 wind3D->forceRepaint(); 00274 } 00275 */ 00276 00277 //MAAA-------------------------------------------------- 00278 00279 /** Must return the action vector u. 00280 * \param out_u The action vector which will be passed to OnTransitionModel 00281 */ 00282 void OnGetAction( CVectorTemplate<KFTYPE> &out_u ); 00283 00284 /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$ 00285 * \param in_u The vector returned by OnGetAction. 00286 * \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$ . 00287 * \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 00288 */ 00289 void OnTransitionModel( 00290 const CVectorTemplate<KFTYPE>& in_u, 00291 CVectorTemplate<KFTYPE>& inout_x, 00292 bool &out_skipPrediction 00293 ); 00294 00295 /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$ 00296 * \param out_F Must return the Jacobian. 00297 * 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(). 00298 */ 00299 void OnTransitionJacobian( 00300 CMatrixTemplateNumeric<KFTYPE> & out_F 00301 ); 00302 00303 /** Implements the transition noise covariance \f$ Q_k \f$ 00304 * \param out_Q Must return the covariance matrix. 00305 * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian 00306 */ 00307 void OnTransitionNoise( 00308 CMatrixTemplateNumeric<KFTYPE> & out_Q 00309 ); 00310 00311 /** 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. 00312 * \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. 00313 * \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. 00314 * \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. 00315 * This method will be called just once for each complete KF iteration. 00316 * \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. 00317 */ 00318 void OnGetObservations( 00319 CMatrixTemplateNumeric<KFTYPE> &out_z, 00320 CMatrixTemplateNumeric<KFTYPE> &out_R, 00321 vector_int &out_data_association 00322 ); 00323 00324 00325 /** 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$. 00326 * \param in_z This is the same matrix returned by OnGetObservations(), passed here for reference. 00327 * \param in_data_association The vector returned by OnGetObservations(), passed here for reference. 00328 * \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. 00329 * \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. 00330 * \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. 00331 * \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$. 00332 * \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$. 00333 * 00334 * 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: 00335 * - N: number of rows in in_z (number of observed features, or 1 in general). 00336 * - O: get_observation_size() 00337 * - V: get_vehicle_size() 00338 * - F: get_feature_size() 00339 */ 00340 00341 void OnObservationModelAndJacobians( 00342 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00343 const vector_int &data_association, 00344 const bool &in_full, 00345 const int &in_obsIdx, 00346 CVectorTemplate<KFTYPE> &ytilde, 00347 CMatrixTemplateNumeric<KFTYPE> &Hx, 00348 CMatrixTemplateNumeric<KFTYPE> &Hy ) ; 00349 00350 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". 00351 * \param in_z This is the same matrix returned by OnGetObservations(). 00352 * \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. 00353 * \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. 00354 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$. 00355 * \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$. 00356 * \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$. 00357 * 00358 * - O: get_observation_size() 00359 * - V: get_vehicle_size() 00360 * - F: get_feature_size() 00361 */ 00362 virtual void OnInverseObservationModel( 00363 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00364 const size_t &in_obsIdx, 00365 const size_t &in_idxNewFeat, 00366 CVectorTemplate<KFTYPE> &out_yn, 00367 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dxv, 00368 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dhn ); 00369 00370 00371 /** 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. 00372 */ 00373 virtual void OnNormalizeStateVector() {normalize();}; 00374 00375 virtual void OnPostIteration(); 00376 00377 /** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable. 00378 */ 00379 virtual size_t get_vehicle_size() const 00380 { 00381 return state_size;// 00382 } 00383 00384 00385 /** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc). 00386 */ 00387 virtual size_t get_observation_size() const 00388 { 00389 return 2; 00390 } 00391 //------------------------------------------------------------------------------------------ 00392 00393 00394 CVectorDouble m_hrl_i; //!< Position of the ith feature relative to the camera expressed in the image uv coordinate system 00395 00396 mrpt::vision::CFeatureList m_featList; //!< List of all features 00397 mrpt::vision::CFeatureList m_ObservedfeatList; //!< List of observed features 00398 00399 CMatrixDouble m_h; //!< coordinate of all features the image uv coordinate system. Note: unobserved features has -1 uv values 00400 CMatrixDouble m_z; //!< coordinate of Observed features in the image uv coordinate system 00401 CMatrixDouble m_zz; //!< coordinate of matched features in the image uv coordinate system 00402 CMatrixDouble m_hh; //!< coordinate of precdicted features(includes just matched ones) in the image uv system 00403 CMatrixDouble m_MatchCoord; //!< coordinate of matched features in the image uv coordinate system based on correlation value 00404 00405 CMatrixDouble m_Hx_Hy; //!< Full Jacobian matrix of all features 00406 CMatrixDouble m_HH; //!< Full Jacobian matrix of all matched features 00407 CMatrixDouble m_RR; //!< Full R matrix of all matched features 00408 CMatrixDouble m_SS; //!< Full S matrix of all matched features 00409 CPose3D m_robotPose; //!< robot 3D Pose 00410 00411 /** Calculate the Jacobian dh_dxv 00412 */ 00413 void dh_dxv(CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dxv); 00414 00415 /** Calculate the Jacobian dh_dy 00416 */ 00417 void dh_dyi(CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dy); 00418 00419 /** Calculate the Jacobian dh_drw 00420 */ 00421 void dh_drw(CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_drw); 00422 00423 /** Calculate the Jacobian dh_dqwr 00424 */ 00425 void dh_dqwr(CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dqwr); 00426 00427 /** Calculate the Jacobian dh_dhrl 00428 */ 00429 void dh_dhrl(CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dhrl); 00430 00431 /** Calculate the Jacobian dhrl_drw 00432 */ 00433 void dhrl_drw(CVectorDouble &yi, CMatrixDouble &m_dhrl_drw); 00434 00435 /** Calculate the Jacobian dhd_dhu 00436 */ 00437 void dhd_dhu(CMatrixDouble &zi, CMatrixDouble &m_dhd_dhu); 00438 00439 /** Calculate the Jacobian dhu_dhrl 00440 */ 00441 void dhu_dhrl(CVectorDouble &yi, CMatrixDouble &m_dhu_dhrl); 00442 00443 /** Calculate the Jacobian dhrl_dqwr 00444 */ 00445 void dhrl_dqwr(CVectorDouble &yi, CMatrixDouble &m_dhrl_dqwr); 00446 00447 /** Calculate the Jacobian dhrl_dy 00448 */ 00449 void dhrl_dy(CVectorDouble &yi, CMatrixDouble &m_dhrl_dy); 00450 00451 00452 /** Calculate and return the feature coordinate in Unified Inverse Depth Parametrization. Where h_LR_rot 00453 * is the vector which join the camera optic center to the feature. 00454 */ 00455 CVectorDouble hinv(CMatrixDouble h_LR_rot); 00456 00457 /** Calculate and return the vector which join the camera optic center to the feature. 00458 */ 00459 CMatrixDouble calculate_ray(const float &col,const float &row); 00460 00461 /** This function increment the covariance matrix with a new feature. 00462 */ 00463 void addAFeatureCov_newPar(CMatrixDouble &XYZ_w, float &col, float &row); 00464 00465 /** Calculate the mahalanobis distance in feature innovation (prediction - measurement) 00466 */ 00467 void mahalanob_dist(int j_k, int i_k, int halfW_x, int halfW_y, CMatrixDouble &inv_S, CMatrixDouble &correlationScore); 00468 00469 00470 protected: 00471 00472 /** This function insert a new feature to the map (state vector and covariance matrix). 00473 * \param col is the feature column position in pixels over the image 00474 * \param row is the feature row position in pixels over the image 00475 */ 00476 void addNewFeatures_newPar(float &col,float &row); 00477 00478 /** Calculate the feature's position and the transition matrix H = [dh_dxv dh_dy] 00479 * Measurement, derivatives and texture prediction 00480 * \param h, output. This vector contain the feature position estimation. 00481 * \param H, output. This matrix is the Jacovian of measurement prediction. 00482 */ 00483 void h_calculate(CMatrixDouble &h_Out, CMatrixDouble &H_Out); 00484 00485 /** This function return the position,zi (in pixels) of a feature yi 00486 */ 00487 00488 //returns distorted pixel coord 00489 void hi(CVectorDouble &yinit, CMatrixDouble &zi, size_t indx ); 00490 00491 00492 /** % Calculate the projection of yi (in the camera reference) 00493 */ 00494 00495 //returns undistorted pixel coord 00496 //Project a 3D point into undistorted pixel coordinates 00497 00498 void hu(const TPoint3D &XYZ, mrpt::vision::TPixelCoordf &out_pixel ); 00499 00500 /** % Calculate h derivates (h = feature's position in pixels). Hi = [dh_dxv dh_dyi] 00501 * \param yi --> feature in Unified Inverse Depth Parametrization 00502 * \param zi --> measurement in pixels coordinates 00503 * \param i --> feature index 00504 * \param Hi --> measurement jacobian 00505 */ 00506 void calculate_Hi(CVectorDouble &yi, CMatrixDouble &zi, unsigned int i, CMatrixDouble &Hi); 00507 00508 /** Matching between actual image and feature patch (FOR EKF) 00509 \code 00510 input parameters 00511 h_predicted --> (u,v) coordinates of predicted features (is a column vector (u0,v0,u1,v1,.,un,vn)) 00512 S_predicted --> predicted covariance innovation matrix 00513 H_predicted --> predicted transition matrix 00514 im --> current frame 00515 00516 output parameters 00517 h --> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00518 z --> (u,v) coordinates of measured features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00519 H_matched --> measured transition matrix 00520 R_matched --> measured process noise matrix 00521 00522 inout parameter 00523 features --> (As input) features struct to get feature's patch 00524 (As output)update hits for erase bad features later 00525 \endcode 00526 */ 00527 void MSMatching(CMatrixDouble &h_predicted, const CMatrixDouble &S_predicted, 00528 CMatrixDouble &H_predicted, mrpt::utils::CImage &im, 00529 CMatrixDouble &hh, CMatrixDouble &zz, 00530 CMatrixDouble &H_matched, CMatrixDouble &R_matched, 00531 CFeaturePatch &features); 00532 00533 /** Matching between actual image and feature patch (FOR UKF and IEKF) 00534 \code 00535 input parameters 00536 h_predicted --> (u,v) coordinates of predicted features (is a column vector (u0,v0,u1,v1,.,un,vn)) 00537 S_predicted --> predicted covariance innovation matrix 00538 im --> current frame 00539 00540 output parameters 00541 z --> (u,v) coordinates of measured features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00542 R_matched --> measured process noise matrix 00543 00544 inout parameter 00545 features --> (As input) features struct to get feature's patch 00546 (As output)update hits for erase bad features later 00547 \endcode 00548 */ 00549 void MSMatching(CMatrixDouble &h_predicted, CMatrixDouble &S_predicted, 00550 mrpt::utils::CImage &im, CMatrixDouble &hh, 00551 CMatrixDouble &zz, CMatrixDouble &R_matched, 00552 CVectorDouble &match, CFeaturePatch &features); 00553 00554 00555 /** Initialize feature employing the Harris method adding neccesary information to x(k|k) and p(k|k) 00556 * \param features --> Structure to store the visual features 00557 * \param im --> Initial image in the system 00558 */ 00559 void InitializeNewFeature(CFeaturePatch &features, mrpt::utils::CImage &im); 00560 00561 /** Try to find a image area without features and search it employing the Harris method adding neccesary information to x(k|k) and p(k|k) 00562 * \param features --> Structure to store the visual features 00563 * \param im --> Image where search more features 00564 * \param h --> Predicted features over the image 00565 */ 00566 void SearchMoreFeature(CFeaturePatch &features, mrpt::utils::CImage &im, CMatrixDouble &h); 00567 00568 00569 /** This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the EKF. 00570 * \param HH --> Measured transition matrix 00571 * \param RR --> Measured process noise matrix 00572 * \param hh --> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00573 * \param zz --> (u,v) coordinates of measured features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00574 */ 00575 void Ambiguity(CMatrixDouble & HH,CMatrixDouble &RR,CMatrixDouble &hh,CMatrixDouble &zz); 00576 00577 /** This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the IEKF or UKF. 00578 * \param RR --> Measured process noise matrix 00579 * \param hh --> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00580 * \param zz --> (u,v) coordinates of measured features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn)) 00581 * \param match --> This vector contain the information about which features has been correctly measured and which not. 00582 */ 00583 void Ambiguity(CMatrixDouble & RR,CMatrixDouble &hh,CMatrixDouble &zz,CVectorDouble &match); 00584 00585 /******************************************** FUNCTIONS FOR EKF ********************************************/ 00586 /** Generic function to acces to the prediction model: fv (it must be implemented in MonoSLAMMotionModel) 00587 * \param u --> odometry information (unused parameter for MonoSLAM) 00588 * \param xv --> camera state 00589 */ 00590 void function_fv(const CMatrixDouble &u, CVectorDouble &xv); 00591 00592 /** Generic function to acces to the prediction model Jacobian, F due to the linealization process 00593 * (it must be implemented in MonoSLAMMotionModel) 00594 */ 00595 void function_F(CMatrixDouble &F_parcial); 00596 00597 /** Generic function to acces to the matrix process noise, Q 00598 * (it must be implemented in MonoSLAMMotionModel) 00599 */ 00600 void function_Q(CMatrixDouble &Q); 00601 00602 /** Measurement model 00603 * \param h --> Vector to return the measurement prediction coordinates 00604 * \param Hx_Hy --> Jacobian [Hx Hy1...Hyn] 00605 */ 00606 void function_h(CMatrixDouble &h, CMatrixDouble &Hx_Hy); 00607 00608 /** Generic function to acces to the measurement process noise, R 00609 * (it must be implemented in MonoSLAMMotionModel) 00610 */ 00611 void function_R(CMatrixDouble &h, CMatrixDouble &R); 00612 00613 /** This function normalize the state vector and covariance matrix due to the quaternion employ for orientation. 00614 * If the orientation is not represented by a quaternion, this function is empty. 00615 */ 00616 void normalize(); 00617 00618 /** This function delete from the state vector and covariance matrix the feature in the position i. 00619 */ 00620 void delete_feature(const unsigned int &index); 00621 00622 /** Return the state size. In Monocular SLAM this value is 13 00623 */ 00624 unsigned int get_state_size() const {return state_size;} 00625 00626 /** Return the feature size. In Monocular SLAM this value is 6 due to the employ of Unified Inverse Depth Parametrization. 00627 */ 00628 size_t get_feature_size() const {return feature_size;} 00629 00630 /** Transform the Unified Inverse Depth Parametrization coordinates to XYZ cartesian coordinates. 00631 */ 00632 CMatrixDouble XYZ6D_TO_XYZ3D(){return mmm.XYZTPL2XYZ(m_xkk);} 00633 00634 /** Transform the Unified Inverse Depth Parametrization covariance matrix (6x6) to XYZ covariance (3x3). 00635 */ 00636 CMatrixDouble pxyztpl2pxyz(){return mmm.pxyztpl2pxyz(m_pkk, m_xkk);} 00637 00638 00639 /******************************************** FUNCTIONS FOR uKF ********************************************/ 00640 /** Measurement model 00641 * \param h --> Vector to return the measurement prediction coordinates 00642 */ 00643 void h_UKF(CMatrixDouble &h); 00644 00645 /** Visibility test for determine which features are shown in the image 00646 * \param h --> Vector to return the measurement prediction coordinates 00647 */ 00648 void visibility(CMatrixDouble &h); 00649 00650 00651 /************************************************ NEWBITS ***************************************************/ 00652 /** Jacobian of quaternion inverse: this is a constant matrix that doesn't 00653 * depend on the quaternion 00654 */ 00655 CMatrixDouble dqbar_by_dq(); //matrix(4x4) 00656 00657 /** Component Jacobians: each returns a 3x3 matrix which is the derivative 00658 * of the rotation matrix derived from a quaternion w.r.t. return the real element 00659 */ 00660 CMatrixDouble dR_by_dq0(CQuaternionDouble &q); //matrix(3x3) 00661 00662 /** Component Jacobians: each returns a 3x3 matrix which is the derivative 00663 * of the rotation matrix derived from a quaternion w.r.t. the imaginary element x 00664 */ 00665 CMatrixDouble dR_by_dqx(CQuaternionDouble &q); //matrix(3x3) 00666 00667 /** Component Jacobians: each returns a 3x3 matrix which is the derivative 00668 * of the rotation matrix derived from a quaternion w.r.t. the imaginary element y 00669 */ 00670 CMatrixDouble dR_by_dqy(CQuaternionDouble &q); //matrix(3x3) 00671 00672 /** Component Jacobians: each returns a 3x3 matrix which is the derivative 00673 * of the rotation matrix derived from a quaternion w.r.t. the imaginary element z 00674 */ 00675 CMatrixDouble dR_by_dqz(CQuaternionDouble &q); //matrix(3x3) 00676 00677 00678 /** Auxiliary functions used by dqnorm_by_dq(). Value of diagonal element of Jacobian 00679 */ 00680 float dqi_by_dqi(float qi, float qq); 00681 00682 /** Value of off-diagonal element of Jacobian 00683 */ 00684 float dqi_by_dqj(float qi, float qj, float qq); 00685 00686 00687 /** Normalising Jacobian 00688 */ 00689 CMatrixDouble dqnorm_by_dq(CQuaternionDouble &q); //matrix (4x4) 00690 00691 /** Normalising a 3D vector 00692 */ 00693 CMatrixDouble dvnorm_by_dv(CVectorDouble &v); //matrix(3x3) 00694 00695 /** Auxiliary function. User must not be used. 00696 * \param q --> state vector quaternion. 00697 * \param a --> (hLhatRi) Not rotate. 00698 */ 00699 CMatrixDouble dRq_times_a_by_dq (CQuaternionDouble &q, CMatrixDouble &aMat); //matrix(3x4) 00700 00701 /***********************************************************************/ 00702 //ESTA FUNCION SOLO SIRVE PARA PROBAR LA CLASE DESDE MATLAB, BORRAR CND TERMINE LA DEPURACION 00703 00704 /** This function set the state vector value. (Employed for debug the program) 00705 */ 00706 00707 void set_xkk(const CVectorDouble &xkk_in){m_xkk = xkk_in;} 00708 00709 /** This function set the covariance matrix value. (Employed for debug the program) 00710 */ 00711 void set_pkk(const CMatrixDouble &pkk_in){m_pkk = pkk_in;} 00712 00713 /** This function read the state vector value. (Employed for debug the program) 00714 */ 00715 void get_xkk(CVectorDouble &xkk_out) 00716 { 00717 xkk_out.resize(m_xkk.size()); 00718 xkk_out = m_xkk; 00719 } 00720 00721 /** This function read the covariance matrix value. (Employed for debug the program) 00722 */ 00723 void get_pkk(CMatrixDouble &pkk_out){ 00724 pkk_out.resize(m_pkk.getRowCount(),m_pkk.getColCount()); 00725 pkk_out = m_pkk; 00726 } 00727 00728 }; // end class 00729 00730 } // end namespace 00731 } // end namespace 00732 00733 #endif //__CCMonoSlam_H
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009 |