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 00029 #ifndef mrpt_vision_utils_H 00030 #define mrpt_vision_utils_H 00031 00032 #include <mrpt/vision/CFeature.h> 00033 #include <mrpt/utils/CImage.h> 00034 #include <mrpt/math/utils.h> 00035 #include <mrpt/utils/CLoadableOptions.h> 00036 #include <mrpt/slam/CMetricMap.h> 00037 #include <mrpt/poses/CPose3D.h> 00038 #include <mrpt/poses/CPoint2D.h> 00039 #include <mrpt/poses/CPoint3D.h> 00040 00041 namespace mrpt 00042 { 00043 namespace slam 00044 { 00045 class CLandmarksMap; 00046 class CObservationVisualLandmarks; 00047 } 00048 00049 /** Classes for computer vision, detectors, features, etc. 00050 */ 00051 namespace vision 00052 { 00053 using namespace mrpt::math; 00054 using namespace mrpt::utils; 00055 00056 // Here follow utility declarations, methods, etc. (Old "VisionUtils" namespace). 00057 // Implementations are in "vision.cpp" 00058 00059 /** Landmark ID 00060 */ 00061 typedef uint64_t TLandmarkID; 00062 00063 /** Parameters associated to a stereo system 00064 */ 00065 struct MRPTDLLIMPEXP TStereoSystemParams : public mrpt::utils::CLoadableOptions 00066 { 00067 /** Initilization of default parameters 00068 */ 00069 TStereoSystemParams( ); 00070 00071 /** See utils::CLoadableOptions 00072 */ 00073 void loadFromConfigFile( 00074 const mrpt::utils::CConfigFileBase &source, 00075 const std::string §ion); 00076 00077 /** See utils::CLoadableOptions 00078 */ 00079 void dumpToTextStream(CStream &out) const; 00080 00081 /** Method for propagating the feature's image coordinate uncertainty into 3D space. Default value: Prop_Linear 00082 */ 00083 enum TUnc_Prop_Method 00084 { 00085 /** Linear propagation of the uncertainty 00086 */ 00087 Prop_Linear = -1, 00088 /** Uncertainty propagation through the Unscented Transformation 00089 */ 00090 Prop_UT, 00091 /** Uncertainty propagation through the Scaled Unscented Transformation 00092 */ 00093 Prop_SUT 00094 }; 00095 00096 TUnc_Prop_Method uncPropagation; 00097 00098 /** Intrinsic parameters 00099 */ 00100 CMatrix K; 00101 /** Baseline. Default value: baseline = 0.119f; [Bumblebee] 00102 */ 00103 float baseline; 00104 /** Standard deviation of the error in feature detection. Default value: stdPixel = 1 00105 */ 00106 float stdPixel; 00107 /** Standard deviation of the error in disparity computation. Default value: stdDisp = 1 00108 */ 00109 float stdDisp; 00110 /** Maximum allowed distance. Default value: maxZ = 20.0f 00111 */ 00112 float maxZ; 00113 /** Maximum allowed distance. Default value: minZ = 0.5f 00114 */ 00115 float minZ; 00116 /** Maximum allowed height. Default value: maxY = 3.0f 00117 */ 00118 float maxY; 00119 /** K factor for the UT. Default value: k = 1.5f 00120 */ 00121 float factor_k; 00122 /** Alpha factor for SUT. Default value: a = 1e-3 00123 */ 00124 float factor_a; 00125 /** Beta factor for the SUT. Default value: b = 2.0f 00126 */ 00127 float factor_b; 00128 00129 /** Parameters initialization 00130 */ 00131 //TStereoSystemParams(); 00132 00133 }; // End struct TStereoSystemParams 00134 00135 /** A structure for storing a 3D ROI 00136 */ 00137 struct MRPTDLLIMPEXP TROI 00138 { 00139 // Constructors 00140 TROI(); 00141 TROI(float x1, float x2, float y1, float y2, float z1, float z2); 00142 00143 // Members 00144 float xMin; 00145 float xMax; 00146 float yMin; 00147 float yMax; 00148 float zMin; 00149 float zMax; 00150 }; // end struct TROI 00151 00152 /** A structure for defining a ROI within an image 00153 */ 00154 struct MRPTDLLIMPEXP TImageROI 00155 { 00156 // Constructors 00157 TImageROI(); 00158 TImageROI( float x1, float x2, float y1, float y2 ); 00159 00160 // Members 00161 /** X coordinate limits [0,imageWidth) 00162 */ 00163 float xMin, xMax; 00164 /** Y coordinate limits [0,imageHeight) 00165 */ 00166 float yMin, yMax; 00167 }; // end struct TImageROI 00168 00169 /** A structure containing options for the matching 00170 */ 00171 struct MRPTDLLIMPEXP TMatchingOptions : public mrpt::utils::CLoadableOptions 00172 { 00173 00174 /** Method for propagating the feature's image coordinate uncertainty into 3D space. Default value: Prop_Linear 00175 */ 00176 enum TMatchingMethod 00177 { 00178 /** Matching by cross correlation of the image patches 00179 */ 00180 mmCorrelation = 0, 00181 /** Matching by Euclidean distance between SIFT descriptors 00182 */ 00183 mmDescriptorSIFT, 00184 /** Matching by Euclidean distance between SURF descriptors 00185 */ 00186 mmDescriptorSURF 00187 }; 00188 00189 // General 00190 TMatchingMethod matching_method; 00191 float epipolar_TH; 00192 float rowCheck_TH; 00193 float ccCheck_TH; 00194 00195 // SIFT 00196 float maxEDD_TH; 00197 float EDD_RATIO; 00198 00199 // KLT 00200 float minCC_TH; 00201 float minDCC_TH; 00202 float rCC_TH; 00203 00204 // SURF 00205 00206 /** Constructor 00207 */ 00208 TMatchingOptions( ); 00209 00210 /** See utils::CLoadableOptions 00211 */ 00212 void loadFromConfigFile( 00213 const mrpt::utils::CConfigFileBase &source, 00214 const std::string §ion); 00215 00216 /** See utils::CLoadableOptions 00217 */ 00218 void dumpToTextStream(CStream &out) const; 00219 00220 }; // end struct TMatchingOptions 00221 00222 /** Computes the correlation between this image and another one, encapsulating the openCV function cvMatchTemplate 00223 * This implementation reduced computation time. 00224 * \param patch_img The "patch" image, which must be equal, or smaller than "this" image. This function supports gray-scale (1 channel only) images. 00225 * \param x_search_ini The "x" coordinate of the search window. 00226 * \param y_search_ini The "y" coordinate of the search window. 00227 * \param x_search_size The width of the search window. 00228 * \param y_search_size The height of the search window. 00229 * \param x_max The x coordinate where found the maximun cross correlation value. 00230 * \param y_max The y coordinate where found the maximun cross correlation value 00231 * \param max_val The maximun value of cross correlation which we can find 00232 * Note: By default, the search area is the whole (this) image. 00233 * \sa cross_correlation 00234 */ 00235 void MRPTDLLIMPEXP openCV_cross_correlation( 00236 const CImage &img, 00237 const CImage &patch_img, 00238 size_t &x_max, 00239 size_t &y_max, 00240 double &max_val, 00241 int x_search_ini=-1, 00242 int y_search_ini=-1, 00243 int x_search_size=-1, 00244 int y_search_size=-1); 00245 00246 /** Invert an image using OpenCV function 00247 * 00248 */ 00249 void MRPTDLLIMPEXP flip(CImage &img); 00250 00251 /** Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates, and the camera intrinsic coordinates. 00252 * \param x Pixels coordinates, from the top-left corner of the image. 00253 * \param y Pixels coordinates, from the top-left corner of the image. 00254 * \param A The 3x3 intrinsic parameters matrix for the camera. 00255 * 00256 * \sa buildIntrinsicParamsMatrix, defaultIntrinsicParamsMatrix 00257 */ 00258 TPoint3D MRPTDLLIMPEXP pixelTo3D( const vision::TPixelCoordf &xy, const CMatrixFloat &A); 00259 00260 /** Builds the intrinsic parameters matrix A from parameters: 00261 * \param focalLengthX The focal length, in X (horizontal) pixels 00262 * \param focalLengthY The focal length, in Y (vertical) pixels 00263 * \param centerX The image center, horizontal, in pixels 00264 * \param centerY The image center, vertical, in pixels 00265 * 00266 * <br>This method returns the matrix: 00267 <table> 00268 <tr><td>f_x</td><td>0</td><td>cX</td> </tr> 00269 <tr><td>0</td><td>f_y</td><td>cY</td> </tr> 00270 <tr><td>0</td><td>0</td><td>1</td> </tr> 00271 </table> 00272 * See also the tutorial discussing the <a rhref="http://babel.isa.uma.es/mrpt/index.php/Camera_Parameters">camera model parameters</a>. 00273 * \sa defaultIntrinsicParamsMatrix, pixelTo3D 00274 */ 00275 CMatrix MRPTDLLIMPEXP buildIntrinsicParamsMatrix( 00276 float focalLengthX, 00277 float focalLengthY, 00278 float centerX, 00279 float centerY); 00280 00281 /** Returns the stored, default intrinsic params matrix for a given camera: 00282 * \param camIndex Posible values are listed next. 00283 * \param resolutionX The number of pixel columns 00284 * \param resolutionY The number of pixel rows 00285 * 00286 * The matrix is generated for the indicated camera resolution configuration. 00287 * The following table summarizes the current supported cameras and the values as 00288 * ratios of the corresponding horz. or vert. resolution:<br> 00289 00290 <center><table> 00291 <tr> 00292 <td><center><b>camIndex</b></center></td> 00293 <td><center><b>Manufacturer</b></center></td> 00294 <td><center><b>Camera model</b></center></td> 00295 <td><center><b>fx</b></center></td> 00296 <td><center><b>fy</b></center></td> 00297 <td><center><b>cx</b></center></td> 00298 <td><center><b>cy</b></center></td> 00299 </tr> 00300 00301 <tr> 00302 <td><center>0</center></td> 00303 <td><center>Point Grey Research</center></td> 00304 <td><center>Bumblebee</center></td> 00305 <td><center>0.79345</center></td> 00306 <td><center>1.05793</center></td> 00307 <td><center>0.55662</center></td> 00308 <td><center>0.52692</center></td> 00309 </tr> 00310 00311 <tr> 00312 <td><center>1</center></td> 00313 <td><center>Sony</center></td> 00314 <td><center>???</center></td> 00315 <td><center>0.95666094</center></td> 00316 <td><center>1.3983423f</center></td> 00317 <td><center>0.54626328f</center></td> 00318 <td><center>0.4939191f</center></td> 00319 </tr> 00320 </table> 00321 </center> 00322 00323 * \sa buildIntrinsicParamsMatrix, pixelTo3D 00324 */ 00325 CMatrix MRPTDLLIMPEXP defaultIntrinsicParamsMatrix( 00326 unsigned int camIndex = 0, 00327 unsigned int resolutionX = 320, 00328 unsigned int resolutionY = 240 ); 00329 00330 /** Explore the feature list and removes features which are in the same coordinates 00331 * \param list (Input). The list of features. 00332 */ 00333 void MRPTDLLIMPEXP deleteRepeatedFeats( CFeatureList &list ); 00334 00335 /** Computes ONLY the SIFT descriptors of a set of features ... 00336 * ... 00337 */ 00338 /** / 00339 void computeSiftDescriptors( const CImage &in_img, 00340 CKanadeLucasTomasi::TKLTFeatureList &in_features, 00341 TSIFTFeatureList &out_features); 00342 / ** Computes a set of features with the SIFT algorithm... 00343 * ... 00344 * / 00345 void computeSiftFeatures( const CImage &in_img, 00346 TSIFTFeatureList &out_features, 00347 int wantedNumberOfFeatures = 150 ); 00348 00349 / ** Search for correspondences which are not in the same row and deletes them 00350 * ... 00351 */ 00352 void MRPTDLLIMPEXP rowChecking( CFeatureList &leftList, 00353 CFeatureList &rightList, 00354 float threshold = 0.0); 00355 00356 /** Search for correspondences which are not in the same row and deletes them 00357 * ... 00358 */ 00359 void MRPTDLLIMPEXP checkTrackedFeatures( CFeatureList &leftList, 00360 CFeatureList &rightList, 00361 vision::TMatchingOptions options); 00362 00363 00364 /** Computes the dispersion of the features in the image 00365 * \param list (IN) Inpute list of features 00366 * \param std (OUT) 2 element vector containing the standard deviations in the 'x' and 'y' coordinates. 00367 * \param mean (OUT) 2 element vector containing the mean in the 'x' and 'y' coordinates. 00368 */ 00369 void MRPTDLLIMPEXP getDispersion( const CFeatureList &list, 00370 vector_float &std, 00371 vector_float &mean ); 00372 00373 /** Tracks a set of features in an image. 00374 */ 00375 void MRPTDLLIMPEXP trackFeatures( const CImage &inImg1, 00376 const CImage &inImg2, 00377 vision::CFeatureList &featureList, 00378 const unsigned int &window_width = 7, 00379 const unsigned int &window_height = 7); 00380 00381 /** Filter bad correspondences by distance 00382 * ... 00383 */ 00384 void MRPTDLLIMPEXP filterBadCorrsByDistance( mrpt::slam::CMetricMap::TMatchingPairList &list, // The list of correspondences 00385 unsigned int numberOfSigmas ); // Threshold 00386 00387 00388 /** Returns a new image where distortion has been removed. 00389 * \param A The 3x3 intrinsic parameters matrix 00390 * \param dist_coeffs The 1x4 vector of distortion coefficients 00391 */ 00392 void MRPTDLLIMPEXP correctDistortion( 00393 CImage &in_img, 00394 CImage &out_img, 00395 CMatrix A, 00396 CMatrix dist_coeffs ); 00397 00398 /** Transform HSV color components to RGB, all of them in the range [0,1] 00399 * \sa rgb2hsv 00400 */ 00401 void MRPTDLLIMPEXP hsv2rgb( 00402 float h, 00403 float s, 00404 float v, 00405 float &r, 00406 float &g, 00407 float &b); 00408 00409 /** Transform RGB color components to HSV, all of them in the range [0,1] 00410 * \sa hsv2rgb 00411 */ 00412 void MRPTDLLIMPEXP rgb2hsv( 00413 float r, 00414 float g, 00415 float b, 00416 float &h, 00417 float &s, 00418 float &v ); 00419 00420 /** Different colormaps 00421 * \sa mrpt::vision::colormap 00422 */ 00423 enum TColormap 00424 { 00425 cmGRAYSCALE = 0, 00426 cmJET 00427 }; 00428 00429 /** Transform a float number in the range [0,1] into RGB components. Different colormaps are available. 00430 */ 00431 void MRPTDLLIMPEXP colormap( 00432 const TColormap &color_map, 00433 const float &color_index, 00434 float &r, 00435 float &g, 00436 float &b); 00437 00438 /** Computes the RGB color components (range [0,1]) for the corresponding color index in the range [0,1] using the MATLAB 'jet' colormap. 00439 * \sa colormap 00440 */ 00441 void MRPTDLLIMPEXP jet2rgb( 00442 const float &color_index, 00443 float &r, 00444 float &g, 00445 float &b); 00446 00447 /** Interpolation methods (as defined in OpenCV) 00448 * Used for OpenCV related operations with images, but also with MRPT native classes. 00449 * \sa mrpt::utils::CMappedImage 00450 */ 00451 enum TInterpolationMethod 00452 { 00453 imNEAREST = 0, //!< No interpolation 00454 imBILINEAR, //!< Bilinear interpolation 00455 imBICUBIC, //!< Bilinear interpolation 00456 imAREA //!< Resampling using pixel-area relation 00457 }; 00458 00459 00460 /** Computes the mean squared distance between a set of 3D correspondences 00461 * ... 00462 */ 00463 double MRPTDLLIMPEXP computeMsd( const mrpt::slam::CMetricMap::TMatchingPairList &list, 00464 const mrpt::poses::CPose3D &Rt ); 00465 00466 /** Transform two clouds of 3D points into a matched list of points 00467 * ... 00468 */ 00469 void MRPTDLLIMPEXP cloudsToMatchedList( const mrpt::slam::CObservationVisualLandmarks &cloud1, 00470 const mrpt::slam::CObservationVisualLandmarks &cloud2, 00471 mrpt::slam::CMetricMap::TMatchingPairList &outList); 00472 00473 /** Computes the main orientation of a set of points with an image (for using in SIFT-based algorithms) 00474 * \param image (Input). The input image. 00475 * \param x (Input). A vector containing the 'x' coordinates of the image points. 00476 * \param y (Input). A vector containing the 'y' coordinates of the image points. 00477 * \param orientation (Output). A vector containing the main orientation of the image points. 00478 */ 00479 float MRPTDLLIMPEXP computeMainOrientation( const CImage &image, 00480 const unsigned int &x, 00481 const unsigned int &y ); 00482 00483 /** Find the matches between two lists of features. They must be of the same type. Return value: the number of matched pairs of features 00484 * \param list1 (Input). One list. 00485 * \param list2 (Input). Other list. 00486 * \param matches (Output). A vector of pairs of correspondences. 00487 * \param options (Optional Input). A struct containing matching options 00488 */ 00489 size_t MRPTDLLIMPEXP matchFeatures( const CFeatureList &list1, 00490 const CFeatureList &list2, 00491 CMatchedFeatureList &matches, 00492 const TMatchingOptions &options = TMatchingOptions() ); 00493 00494 /** Project a list of matched features into the 3D space, using the provided options for the stereo system 00495 * \param matches (Input). The list of matched features. 00496 * \param options (Input). The options of the stereo system. 00497 * \param landmarks (Output). A map containing the projected landmarks. 00498 */ 00499 void MRPTDLLIMPEXP projectMatchedFeatures( 00500 CMatchedFeatureList &mfList, // The set of matched features 00501 const vision::TStereoSystemParams ¶m, // Parameters for the stereo system 00502 mrpt::slam::CLandmarksMap &landmarks ); // Output map of 3D landmarks 00503 00504 00505 /** Data associated to each image in the calibration process mrpt::vision::checkerBoardCameraCalibration (All the information can be left empty and will be filled up in the calibration method). 00506 */ 00507 struct TImageCalibData 00508 { 00509 CImage img_original; //!< This image will be automatically loaded from the file name passed to checkerBoardCameraCalibration 00510 CImage img_checkboard; //!< At output, this will contain the detected checkerboard overprinted to the image. 00511 CImage img_rectified; //!< At output, this will be the rectified image 00512 std::vector<mrpt::poses::CPoint2D> detected_corners; //!< At output, the detected corners (x,y) in pixel units. 00513 mrpt::poses::CPose3D reconstructed_camera_pose; //!< At output, the reconstructed pose of the camera. 00514 std::vector<TPixelCoordf> projectedPoints_distorted; //!< At output, only will have an empty vector if the checkerboard was not found in this image, or the predicted (reprojected) corners, which were used to estimate the average square error. 00515 std::vector<TPixelCoordf> projectedPoints_undistorted; //!< At output, like projectedPoints_distorted but for the undistorted image. 00516 }; 00517 00518 /** A list of images, used in checkerBoardCameraCalibration 00519 * \sa checkerBoardCameraCalibration 00520 */ 00521 typedef std::map<std::string,TImageCalibData> TCalibrationImageList; 00522 00523 /** Performs a camera calibration (computation of projection and distortion parameters) from a sequence of captured images of a checkerboard. 00524 * \param input_images [IN/OUT] At input, this list must have one entry for each image to process. At output the original, detected checkboard and rectified images can be found here. See TImageCalibData. 00525 * \param check_size_x [IN] The number of squares in the checkerboard in the X direction. 00526 * \param check_size_y [IN] The number of squares in the checkerboard in the Y direction. 00527 * \param check_squares_length_X_meters [IN] The size of each square in the checkerboard, in meters, in the X axis. 00528 * \param check_squares_length_Y_meters [IN] This will typically be equal to check_squares_length_X_meters. 00529 * \param intrinsicParams [OUT] The 3x3 intrinsic parameters matrix. See http://babel.isa.uma.es/mrpt/index.php/Camera_Parameters 00530 * \param distortionParams [OUT] The 1x4 vector of distortion parameters: k1 k2 p1 p2. See http://babel.isa.uma.es/mrpt/index.php/Camera_Parameters 00531 * \param normalize_image [IN] Select OpenCV flag 00532 * \param out_MSE [OUT] If set to !=NULL, the mean square error of the reprojection will be stored here (in pixel units). 00533 * \param skipDrawDetectedImgs [IN] Whether to skip the generation of the undistorted and detected images in each TImageCalibData 00534 * \sa The <a href="http://babel.isa.uma.es/mrpt/index.php/Application:camera-calib-gui" >camera-calib-gui application</a> is a user-friendly GUI to this class. 00535 * \return false on any error (more info will be dumped to cout), or true on success. 00536 * \sa CImage::findChessboardCorners 00537 */ 00538 bool MRPTDLLIMPEXP checkerBoardCameraCalibration( 00539 TCalibrationImageList &images, 00540 unsigned int check_size_x, 00541 unsigned int check_size_y, 00542 double check_squares_length_X_meters, 00543 double check_squares_length_Y_meters, 00544 CMatrixDouble &intrinsicParams, 00545 std::vector<double> &distortionParams, 00546 bool normalize_image = true, 00547 double *out_MSE = NULL, 00548 bool skipDrawDetectedImgs = false 00549 ); 00550 00551 } 00552 } 00553 00554 00555 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:32:05 EDT 2009 |