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