Main MRPT website > C++ reference
MRPT logo

utils.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  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/types.h>
00044 #include <mrpt/vision/chessboard_camera_calib.h>
00045 
00046 #include <mrpt/vision/link_pragmas.h>
00047 
00048 namespace mrpt
00049 {
00050         namespace slam
00051         {
00052                 class CObservationStereoImages;
00053                 class CObservationBearingRange;
00054         }
00055 
00056         /** Classes for computer vision, detectors, features, etc.
00057          */
00058         namespace vision
00059         {
00060                 using std::vector;
00061                 using namespace mrpt::slam;
00062                 using namespace mrpt::math;
00063                 using namespace mrpt::utils;
00064 
00065                         /**     Computes the correlation between this image and another one, encapsulating the openCV function cvMatchTemplate
00066                         *   This implementation reduced computation time.
00067                         * \param img            [IN]    The imput image. This function supports gray-scale (1 channel only) images.
00068                         * \param patch_img      [IN]    The "patch" image, which must be equal, or smaller than "this" image. This function supports gray-scale (1 channel only) images.
00069                         * \param x_max          [OUT]   The x coordinate where it was found the maximun cross correlation value.
00070                         * \param y_max          [OUT]   The y coordinate where it was found the maximun cross correlation value.
00071                         * \param max_val        [OUT]   The maximun value of cross correlation which we can find
00072                         * \param x_search_ini   [IN]    The "x" coordinate of the search window.
00073                         * \param y_search_ini   [IN]    The "y" coordinate of the search window.
00074                         * \param x_search_size  [IN]    The width of the search window.
00075                         * \param y_search_size  [IN]    The height of the search window.
00076                         *  Note: By default, the search area is the whole (this) image.
00077                         * \sa cross_correlation
00078                         */
00079                         void VISION_IMPEXP openCV_cross_correlation(
00080                                 const CImage            & img,
00081                                                                 const CImage            & patch_img,
00082                                                                 size_t                              & x_max,
00083                                                                 size_t                              & y_max,
00084                                                                 double                              & max_val,
00085                                                                 int                                         x_search_ini=-1,
00086                                                                 int                                         y_search_ini=-1,
00087                                                                 int                                         x_search_size=-1,
00088                                                                 int                                         y_search_size=-1);
00089 
00090                         /**     Invert an image using OpenCV function
00091                         *
00092                         */
00093                         void VISION_IMPEXP flip(
00094                                 CImage                      & img);
00095 
00096                         /** Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates, and the camera intrinsic coordinates.
00097                           *  \param xy  [IN]   Pixels coordinates, from the top-left corner of the image.
00098                           *  \param A   [IN]   The 3x3 intrinsic parameters matrix for the camera.
00099                           *  \return The TPoint3D containing the output unitary vector.
00100                           * \sa buildIntrinsicParamsMatrix, defaultIntrinsicParamsMatrix, TPixelCoordf
00101                           */
00102                         TPoint3D VISION_IMPEXP pixelTo3D(
00103                                 const TPixelCoordf      & xy,
00104                                 const CMatrixDouble33   & A);
00105 
00106                         /** Builds the intrinsic parameters matrix A from parameters:
00107                           * \param focalLengthX [IN]   The focal length, in X (horizontal) pixels
00108                           * \param focalLengthY [IN]   The focal length, in Y (vertical) pixels
00109                           * \param centerX      [IN]   The image center, horizontal, in pixels
00110                           * \param centerY      [IN]   The image center, vertical, in pixels
00111                           *
00112                           * <br>This method returns the matrix:
00113                           <table>
00114                           <tr><td>f_x</td><td>0</td><td>cX</td> </tr>
00115                           <tr><td>0</td><td>f_y</td><td>cY</td> </tr>
00116                           <tr><td>0</td><td>0</td><td>1</td> </tr>
00117                           </table>
00118                           *  See also the tutorial discussing the <a rhref="http://www.mrpt.org/Camera_Parameters">camera model parameters</a>.
00119                           * \sa defaultIntrinsicParamsMatrix, pixelTo3D
00120                           */
00121                         CMatrixDouble33 VISION_IMPEXP buildIntrinsicParamsMatrix(
00122                                 const double            focalLengthX,
00123                                 const double            focalLengthY,
00124                                 const double            centerX,
00125                                 const double            centerY);
00126 
00127                         /** Returns the stored, default intrinsic params matrix for a given camera:
00128                           * \param camIndex     [IN]   Posible values are listed next.
00129                           * \param resolutionX  [IN]   The number of pixel columns
00130                           * \param resolutionY  [IN]   The number of pixel rows
00131                           *
00132                           * The matrix is generated for the indicated camera resolution configuration.
00133                           * The following table summarizes the current supported cameras and the values as
00134                           *  ratios of the corresponding horz. or vert. resolution:<br>
00135 
00136                           <center><table>
00137                           <tr>
00138                            <td><center><b>camIndex</b></center></td>
00139                            <td><center><b>Manufacturer</b></center></td>
00140                            <td><center><b>Camera model</b></center></td>
00141                            <td><center><b>fx</b></center></td>
00142                            <td><center><b>fy</b></center></td>
00143                            <td><center><b>cx</b></center></td>
00144                            <td><center><b>cy</b></center></td>
00145                           </tr>
00146 
00147                           <tr>
00148                            <td><center>0</center></td>
00149                            <td><center>Point Grey Research</center></td>
00150                            <td><center>Bumblebee</center></td>
00151                            <td><center>0.79345</center></td>
00152                            <td><center>1.05793</center></td>
00153                            <td><center>0.55662</center></td>
00154                            <td><center>0.52692</center></td>
00155                           </tr>
00156 
00157                           <tr>
00158                            <td><center>1</center></td>
00159                            <td><center>Sony</center></td>
00160                            <td><center>???</center></td>
00161                            <td><center>0.95666094</center></td>
00162                            <td><center>1.3983423f</center></td>
00163                            <td><center>0.54626328f</center></td>
00164                            <td><center>0.4939191f</center></td>
00165                           </tr>
00166                           </table>
00167                           </center>
00168 
00169                           * \sa buildIntrinsicParamsMatrix, pixelTo3D
00170                           */
00171                         CMatrixDouble33 VISION_IMPEXP defaultIntrinsicParamsMatrix(
00172                                 unsigned int            camIndex = 0,
00173                                 unsigned int            resolutionX = 320,
00174                                 unsigned int            resolutionY = 240 );
00175 
00176                         /** Explore the feature list and removes features which are in the same coordinates
00177                           * \param list [IN] The list of features.
00178                           */
00179                         void VISION_IMPEXP deleteRepeatedFeats(
00180                                 CFeatureList            & list );
00181 
00182                         /** Search for correspondences which are not in the same row and deletes them
00183                           * \param leftList     [IN/OUT]    The left list of matched features.
00184                           * \param rightList    [IN/OUT]    The right list of matched features.
00185                           * \param threshold    [IN]        The tolerance value for the row checking: valid matched are within this threshold.
00186                           */
00187                         void VISION_IMPEXP rowChecking(
00188                                 CFeatureList            & leftList,
00189                                                                 CFeatureList            & rightList,
00190                                                                 float                   threshold = 1.0);
00191 
00192                         /** Computes the dispersion of the features in the image
00193                           * \param list [IN]    Input list of features
00194                           * \param std  [OUT]   2 element vector containing the standard deviations in the 'x' and 'y' coordinates.
00195                           * \param mean [OUT]   2 element vector containing the mean in the 'x' and 'y' coordinates.
00196                           */
00197                         void VISION_IMPEXP getDispersion(
00198                                 const CFeatureList      & list,
00199                                                                 vector_float            & std,
00200                                                                 vector_float            & mean );
00201 
00202                         /** Returns a new image where distortion has been removed.
00203                           * \param A The 3x3 intrinsic parameters matrix
00204                           * \param dist_coeffs The 1x4 (or 1x5) vector of distortion coefficients
00205                           */
00206                         inline void correctDistortion(
00207                                 const CImage            & in_img,
00208                                 CImage                          & out_img,
00209                                 const CMatrixDouble33   & A,
00210                                 const vector_double     & dist_coeffs )
00211                         {
00212                                 in_img.rectifyImage( out_img, A, dist_coeffs);
00213                         }
00214 
00215 
00216                         /** Computes the mean squared distance between a set of 3D correspondences
00217                           * ...
00218                           */
00219                         double VISION_IMPEXP computeMsd(
00220                                 const TMatchingPairList & list,
00221                                                                 const poses::CPose3D    & Rt );
00222 
00223                         /** Transform two clouds of 3D points into a matched list of points
00224                           * ...
00225                           */
00226                         void VISION_IMPEXP cloudsToMatchedList(
00227                                 const CObservationVisualLandmarks   & cloud1,
00228                                 const CObservationVisualLandmarks   & cloud2,
00229                                                                 TMatchingPairList                   & outList);
00230 
00231                         /** Computes the main orientation of a set of points with an image (for using in SIFT-based algorithms)
00232                           * \param image    [IN] The input image.
00233                           * \param x        [IN] A vector containing the 'x' coordinates of the image points.
00234                           * \param y        [IN] A vector containing the 'y' coordinates of the image points.
00235                           * \return The main orientation of the image point.
00236                           */
00237                         float VISION_IMPEXP computeMainOrientation(
00238                                 const CImage                        & image,
00239                                                                 unsigned int                        x,
00240                                                                 unsigned int                        y );
00241 
00242                         /** Normalizes the brigthness and contrast of an image by setting its mean value to zero and its standard deviation to unit.
00243                           * \param image        [IN]        The input image.
00244                           * \param nimage       [OUTPUT]    The new normalized image.
00245                           */
00246             void VISION_IMPEXP normalizeImage(
00247                                 const CImage                        & image,
00248                                 CImage                              & nimage );
00249 
00250                         /** Find the matches between two lists of features which must be of the same type.
00251                           * \param list1    [IN]    One list.
00252                           * \param list2    [IN]    Other list.
00253                           * \param matches  [OUT]   A vector of pairs of correspondences.
00254                           * \param options  [IN]    A struct containing matching options
00255                           * \return Returns the number of matched pairs of features.
00256                           */
00257                         size_t VISION_IMPEXP matchFeatures(
00258                                 const CFeatureList                  & list1,
00259                                 const CFeatureList                  & list2,
00260                                 CMatchedFeatureList                 & matches,
00261                                 const TMatchingOptions              & options = TMatchingOptions(),
00262                                 const TStereoSystemParams           & params = TStereoSystemParams() );
00263 
00264             /** Calculates the Sum of Absolutes Differences (range [0,1]) between two patches. Both patches must have the same size.
00265                           * \param mList    [IN]  The list of matched features.
00266                           * \param mask1    [OUT] The output mask for left features.
00267                           * \param mask2    [OUT] The output mask for right features.
00268                           * \param wSize    [IN] The value of the masking window for each features.
00269                           * \exception if mList.size() = 0
00270                   */
00271             void VISION_IMPEXP generateMask(
00272                                 const CMatchedFeatureList           & mList,
00273                                 CMatrixBool                         & mask1,
00274                                 CMatrixBool                         & mask2,
00275                                 int                                 wSize = 10 );
00276 
00277             /** Calculates the Sum of Absolutes Differences (range [0,1]) between two patches. Both patches must have the same size.
00278                           * \param patch1 [IN]  One patch.
00279                           * \param patch2 [IN]  The other patch.
00280                           * \return The value of computed SAD normalized to [0,1]
00281                   */
00282                 double VISION_IMPEXP computeSAD(
00283                                 const CImage                        & patch1,
00284                                 const CImage                        & patch2 );
00285 
00286                         /** Draw rectangles around each of the features on a copy of the input image.
00287                           * \param inImg    [IN]    The input image where to draw the features.
00288                           * \param theList  [IN]    The list of features.
00289                           * \param outImg   [OUT]   The copy of the input image with the marked features.
00290                           */
00291                         void VISION_IMPEXP addFeaturesToImage(
00292                                 const CImage                        & inImg,
00293                                 const CFeatureList                  & theList,
00294                                 CImage                              & outImg );
00295 
00296                         /** Computes the 3D position of a set of matched features from their coordinates in the images. The list have to be matched in order, e.g. leftList[0]<->rightList[0]
00297                           * \param leftList     [IN]    The left list of features.
00298                           * \param rightList    [IN]    The right list of features.
00299                           * \param vP3D         [OUT]   A vector of TPoint3D containing the 3D positions of the projected points.
00300                           * \param params       [IN]    The intrinsic and extrinsic parameters of the stereo pair.
00301                           */
00302             void VISION_IMPEXP projectMatchedFeatures(
00303                                 const CFeatureList                              & leftList,
00304                                 const CFeatureList                              & rightList,
00305                                 vector<TPoint3D>                    & vP3D,
00306                                 const TStereoSystemParams           & params = TStereoSystemParams() );
00307 
00308                         /** Computes the 3D position of a particular matched feature.
00309                           * \param leftList     [IN]    The left feature.
00310                           * \param rightList    [IN]    The right feature.
00311                           * \param vP3D         [OUT]   The 3D position of the projected point.
00312                           * \param params       [IN]    The intrinsic and extrinsic parameters of the stereo pair.
00313                           */
00314             void VISION_IMPEXP projectMatchedFeature(
00315                                 const CFeaturePtr                   & leftFeat,
00316                                 const CFeaturePtr                   & rightFeat,
00317                                 TPoint3D                            & p3D,
00318                                 const TStereoSystemParams           & params = TStereoSystemParams() );
00319 
00320                         /** Project a list of matched features into the 3D space, using the provided parameters of the stereo system
00321                           * \param mfList       [IN/OUT]    The list of matched features. Features which yields a 3D point outside the area defined in TStereoSystemParams are removed from the lists.
00322                           * \param param        [IN]        The parameters of the stereo system.
00323                           * \param landmarks    [OUT]       A map containing the projected landmarks.
00324                           * \sa TStereoSystemParams, CLandmarksMap
00325                           */
00326                         void VISION_IMPEXP projectMatchedFeatures(
00327                                 CMatchedFeatureList                             & mfList,
00328                                 const TStereoSystemParams               & param,
00329                                 CLandmarksMap                               & landmarks );
00330 
00331                         /** Project a pair of feature lists into the 3D space, using the provided options for the stereo system. The matches must be in order,
00332                           *     i.e. leftList[0] corresponds to rightList[0] and so on. Features which yields a 3D point outside the area defined in TStereoSystemParams are removed from the lists.
00333                           * \param leftList     [IN/OUT]    The left list of matched features.
00334                           * \param rightList    [IN/OUT]    The right list of matched features.
00335                           * \param param        [IN]        The options of the stereo system.
00336                           * \param landmarks    (OUT]       A map containing the projected landmarks.
00337                           * \sa TStereoSystemParams, CLandmarksMap
00338                           */
00339                         void VISION_IMPEXP projectMatchedFeatures(
00340                                 CFeatureList                                    & leftList,
00341                                 CFeatureList                                        & rightList,
00342                                 const TStereoSystemParams               & param,
00343                                 CLandmarksMap                               & landmarks );
00344 
00345                         /** Converts a stereo images observation into a bearing and range observation.
00346                                 \param inObs    [IN]    The input stereo images observation.
00347                                 \param sg               [IN]    The sigma of the row, col, and disparity variables involved in the feature detection.
00348                                 \param outObs   [OUT]   The output bearing and range observation (including covariances).
00349                         */
00350                         void VISION_IMPEXP StereoObs2BRObs(
00351                                 const CObservationStereoImages      & inObs,
00352                                 const vector<double>                & sg,
00353                                 CObservationBearingRange            & outObs );
00354 
00355                         /** Converts a matched feature list into a bearing and range observation (some of the stereo camera system must be provided).
00356                                 \param inMatches                [IN]    The input list of matched features.
00357                                 \param intrinsicParams  [IN]    The intrisic params of the reference (left) camera of the stereo system.
00358                                 \param baseline                 [IN]    The distance among the X axis of the right camera wrt the reference (left) camera.
00359                                 \param sg                               [IN]    The sigma of the row, col, and disparity variables involved in the feature detection.
00360                                 \param outObs                   [OUT]   The output bearing and range observation (including covariances).
00361                         */
00362                         void VISION_IMPEXP StereoObs2BRObs(
00363                                 const CMatchedFeatureList           & inMatches,
00364                                 const CMatrixDouble33               & intrinsicParams,
00365                                 const double                        & baseline,
00366                                 const CPose3D                       & sensorPose,
00367                                 const vector<double>                & sg,
00368                                 CObservationBearingRange            & outObs );
00369 
00370                         /** Converts a CObservationVisualLandmarks into a bearing and range observation (without any covariances). Fields of view are not computed.
00371                                 \param inObs                    [IN]    The input observation.
00372                                 \param sg                               [IN]    The sigma of the row, col, and disparity variables involved in the feature detection.
00373                                 \param outObs                   [OUT]   The output bearing and range observation.
00374                         */
00375             void VISION_IMPEXP StereoObs2BRObs(
00376                                 const CObservationStereoImages      & inObs,
00377                                 const vector<double>                & sg,
00378                                 CObservationBearingRange            & outObs );
00379 
00380                         /** Converts a CObservationVisualLandmarks into a bearing and range observation (without any covariances). Fields of view are not computed.
00381                                 \param inObs                    [IN]    The input observation.
00382                                 \param outObs                   [OUT]   The output bearing and range observation.
00383                         */
00384                         void VISION_IMPEXP StereoObs2BRObs(
00385                                 const CObservationVisualLandmarks   & inObs,
00386                                 CObservationBearingRange            & outObs );
00387 
00388             /** Computes a pair of x-and-y maps for stereo rectification from a pair of cameras and the relative pose of the second one wrt the first one.
00389                 \param cam1, cam2           [IN]    The pair of involved cameras
00390                 \param rightCameraPose      [IN]    The change in pose of the second camera wrt the first one
00391                 \param outMap1x,outMap1y    [OUT]   The x-and-y maps corresponding to cam1 (should be converted to *cv::Mat)
00392                 \param outMap2x,outMap2y    [OUT]   The x-and-y maps corresponding to cam2 (should be converted to *cv::Mat)
00393             */
00394             void VISION_IMPEXP computeStereoRectificationMaps(
00395                                 const TCamera                       & cam1,
00396                                 const TCamera                       & cam2,
00397                                 const poses::CPose3D                & rightCameraPose,
00398                                 void                                *outMap1x,
00399                                 void                                *outMap1y,
00400                                 void                                *outMap2x,
00401                                 void                                *outMap2y );
00402 
00403         } // end-namespace-vision
00404 } // end-namespace-mrpt
00405 
00406 
00407 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011