vision/include/mrpt/vision/utils.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#ifndef mrpt_vision_utils_H
#define mrpt_vision_utils_H
 
#include <mrpt/vision/CFeature.h>
#include <mrpt/img/CImage.h>
#include <mrpt/img/TStereoCamera.h>
#include <mrpt/math/CMatrixTemplate.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/tfest/TMatchingPair.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/vision/types.h>
#include <mrpt/vision/chessboard_camera_calib.h>
#include <mrpt/obs/obs_frwds.h>
 
namespace mrpt
{
namespace maps
{
class CLandmarksMap;
}
namespace obs
{
class CObservationVisualLandmarks;
}
 
/** Classes for computer vision, detectors, features, etc.  \ingroup
 * mrpt_vision_grp
 */
namespace vision
{
/** \addtogroup mrpt_vision_grp
  *  @{ */
 
/**     Computes the correlation between this image and another one, encapsulating
* the openCV function cvMatchTemplate
*   This implementation reduced computation time.
* \param img            [IN]    The imput image. This function supports
* gray-scale (1 channel only) images.
* \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.
* \param x_max          [OUT]   The x coordinate where it was found the maximun
* cross correlation value.
* \param y_max          [OUT]   The y coordinate where it was found the maximun
* cross correlation value.
* \param max_val        [OUT]   The maximun value of cross correlation which we
* can find
* \param x_search_ini   [IN]    The "x" coordinate of the search window.
* \param y_search_ini   [IN]    The "y" coordinate of the search window.
* \param x_search_size  [IN]    The width of the search window.
* \param y_search_size  [IN]    The height of the search window.
*  Note: By default, the search area is the whole (this) image.
* \sa cross_correlation
*/
void openCV_cross_correlation(
        const mrpt::img::CImage& img, const mrpt::img::CImage& patch_img,
        size_t& x_max, size_t& y_max, double& max_val, int x_search_ini = -1,
        int y_search_ini = -1, int x_search_size = -1, int y_search_size = -1);
 
/**     Invert an image using OpenCV function
*
*/
void flip(mrpt::img::CImage& img);
 
/** Extract a UNITARY 3D vector in the direction of a 3D point, given from its
 * (x,y) pixels coordinates, and the camera intrinsic coordinates.
  *  \param xy  [IN]   Pixels coordinates, from the top-left corner of the
 * image.
  *  \param A   [IN]   The 3x3 intrinsic parameters matrix for the camera.
  *  \return The mrpt::math::TPoint3D containing the output unitary vector.
  * \sa buildIntrinsicParamsMatrix, defaultIntrinsicParamsMatrix, TPixelCoordf
  */
mrpt::math::TPoint3D pixelTo3D(
        const mrpt::img::TPixelCoordf& xy, const mrpt::math::CMatrixDouble33& A);
 
/** Builds the intrinsic parameters matrix A from parameters:
  * \param focalLengthX [IN]   The focal length, in X (horizontal) pixels
  * \param focalLengthY [IN]   The focal length, in Y (vertical) pixels
  * \param centerX      [IN]   The image center, horizontal, in pixels
  * \param centerY      [IN]   The image center, vertical, in pixels
  *
  * <br>This method returns the matrix:
  <table>
  <tr><td>f_x</td><td>0</td><td>cX</td> </tr>
  <tr><td>0</td><td>f_y</td><td>cY</td> </tr>
  <tr><td>0</td><td>0</td><td>1</td> </tr>
  </table>
  *  See also the tutorial discussing the <a
  rhref="http://www.mrpt.org/Camera_Parameters">camera model parameters</a>.
  * \sa defaultIntrinsicParamsMatrix, pixelTo3D
  */
mrpt::math::CMatrixDouble33 buildIntrinsicParamsMatrix(
        const double focalLengthX, const double focalLengthY, const double centerX,
        const double centerY);
 
/** Returns the stored, default intrinsic params matrix for a given camera:
  * \param camIndex     [IN]   Posible values are listed next.
  * \param resolutionX  [IN]   The number of pixel columns
  * \param resolutionY  [IN]   The number of pixel rows
  *
  * The matrix is generated for the indicated camera resolution configuration.
  * The following table summarizes the current supported cameras and the values
  as
  *  ratios of the corresponding horz. or vert. resolution:<br>
 
  <center><table>
  <tr>
   <td><center><b>camIndex</b></center></td>
   <td><center><b>Manufacturer</b></center></td>
   <td><center><b>Camera model</b></center></td>
   <td><center><b>fx</b></center></td>
   <td><center><b>fy</b></center></td>
   <td><center><b>cx</b></center></td>
   <td><center><b>cy</b></center></td>
  </tr>
 
  <tr>
   <td><center>0</center></td>
   <td><center>Point Grey Research</center></td>
   <td><center>Bumblebee</center></td>
   <td><center>0.79345</center></td>
   <td><center>1.05793</center></td>
   <td><center>0.55662</center></td>
   <td><center>0.52692</center></td>
  </tr>
 
  <tr>
   <td><center>1</center></td>
   <td><center>Sony</center></td>
   <td><center>???</center></td>
   <td><center>0.95666094</center></td>
   <td><center>1.3983423f</center></td>
   <td><center>0.54626328f</center></td>
   <td><center>0.4939191f</center></td>
  </tr>
  </table>
  </center>
 
  * \sa buildIntrinsicParamsMatrix, pixelTo3D
  */
mrpt::math::CMatrixDouble33 defaultIntrinsicParamsMatrix(
        unsigned int camIndex = 0, unsigned int resolutionX = 320,
        unsigned int resolutionY = 240);
 
/** Explore the feature list and removes features which are in the same
 * coordinates
  * \param list [IN] The list of features.
  */
void deleteRepeatedFeats(CFeatureList& list);
 
/** Search for correspondences which are not in the same row and deletes them
  * \param leftList     [IN/OUT]    The left list of matched features.
  * \param rightList    [IN/OUT]    The right list of matched features.
  * \param threshold    [IN]        The tolerance value for the row checking:
 * valid matched are within this threshold.
  */
void rowChecking(
        CFeatureList& leftList, CFeatureList& rightList, float threshold = 1.0);
 
/** Computes the dispersion of the features in the image
  * \param list [IN]    Input list of features
  * \param std  [OUT]   2 element vector containing the standard deviations in
 * the 'x' and 'y' coordinates.
  * \param mean [OUT]   2 element vector containing the mean in the 'x' and
 * 'y' coordinates.
  */
void getDispersion(
        const CFeatureList& list, mrpt::math::CVectorFloat& std,
        mrpt::math::CVectorFloat& mean);
 
/** Computes the mean squared distance between a set of 3D correspondences
  * ...
  */
double computeMsd(
        const mrpt::tfest::TMatchingPairList& list, const poses::CPose3D& Rt);
 
/** Transform two clouds of 3D points into a matched list of points
  * ...
  */
void cloudsToMatchedList(
        const mrpt::obs::CObservationVisualLandmarks& cloud1,
        const mrpt::obs::CObservationVisualLandmarks& cloud2,
        mrpt::tfest::TMatchingPairList& outList);
 
/** Computes the main orientation of a set of points with an image (for using in
 * SIFT-based algorithms)
  * \param image    [IN] The input image.
  * \param x        [IN] A vector containing the 'x' coordinates of the image
 * points.
  * \param y        [IN] A vector containing the 'y' coordinates of the image
 * points.
  * \return The main orientation of the image point.
  */
float computeMainOrientation(
        const mrpt::img::CImage& image, unsigned int x, unsigned int y);
 
/** Normalizes the brigthness and contrast of an image by setting its mean value
 * to zero and its standard deviation to unit.
  * \param image        [IN]        The input image.
  * \param nimage       [OUTPUT]    The new normalized image.
  */
void normalizeImage(const mrpt::img::CImage& image, mrpt::img::CImage& nimage);
 
/** Find the matches between two lists of features which must be of the same
 * type.
  * \param list1    [IN]    One list.
  * \param list2    [IN]    Other list.
  * \param matches  [OUT]   A vector of pairs of correspondences.
  * \param options  [IN]    A struct containing matching options
  * \return Returns the number of matched pairs of features.
  */
size_t matchFeatures(
        const CFeatureList& list1, const CFeatureList& list2,
        CMatchedFeatureList& matches,
        const TMatchingOptions& options = TMatchingOptions(),
        const TStereoSystemParams& params = TStereoSystemParams());
 
/** Calculates the Sum of Absolutes Differences (range [0,1]) between two
 * patches. Both patches must have the same size.
  * \param mList    [IN]  The list of matched features.
  * \param mask1    [OUT] The output mask for left features.
  * \param mask2    [OUT] The output mask for right features.
  * \param wSize    [IN] The value of the masking window for each features.
  * \exception if mList.size() = 0
  */
void generateMask(
        const CMatchedFeatureList& mList, mrpt::math::CMatrixBool& mask1,
        mrpt::math::CMatrixBool& mask2, int wSize = 10);
 
/** Calculates the Sum of Absolutes Differences (range [0,1]) between two
 * patches. Both patches must have the same size.
  * \param patch1 [IN]  One patch.
  * \param patch2 [IN]  The other patch.
  * \return The value of computed SAD normalized to [0,1]
  */
double computeSAD(
        const mrpt::img::CImage& patch1, const mrpt::img::CImage& patch2);
 
/** Draw rectangles around each of the features on a copy of the input image.
  * \param inImg    [IN]    The input image where to draw the features.
  * \param theList  [IN]    The list of features.
  * \param outImg   [OUT]   The copy of the input image with the marked
 * features.
  */
void addFeaturesToImage(
        const mrpt::img::CImage& inImg, const CFeatureList& theList,
        mrpt::img::CImage& outImg);
 
void projectMatchedFeatures(
        const CMatchedFeatureList& matches,
        const mrpt::img::TStereoCamera& stereo_camera,
        std::vector<mrpt::math::TPoint3D>& out_points);
 
/** 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]
  * \param leftList     [IN]    The left list of features.
  * \param rightList    [IN]    The right list of features.
  * \param vP3D         [OUT]   A vector of mrpt::math::TPoint3D containing the
 * 3D positions of the projected points.
  * \param params       [IN]    The intrinsic and extrinsic parameters of the
 * stereo pair.
  */
void projectMatchedFeatures(
        const CFeatureList& leftList, const CFeatureList& rightList,
        std::vector<mrpt::math::TPoint3D>& vP3D,
        const TStereoSystemParams& params = TStereoSystemParams());
 
/** Computes the 3D position of a particular matched feature.
  * \param leftList     [IN]    The left feature.
  * \param rightList    [IN]    The right feature.
  * \param vP3D         [OUT]   The 3D position of the projected point.
  * \param params       [IN]    The intrinsic and extrinsic parameters of the
 * stereo pair.
  */
void projectMatchedFeature(
        const CFeature::Ptr& leftFeat, const CFeature::Ptr& rightFeat,
        mrpt::math::TPoint3D& p3D,
        const TStereoSystemParams& params = TStereoSystemParams());
 
/** Project a list of matched features into the 3D space, using the provided
 * parameters of the stereo system
  * \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.
  * \param param        [IN]        The parameters of the stereo system.
  * \param landmarks    [OUT]       A map containing the projected landmarks.
  * \sa TStereoSystemParams, CLandmarksMap
  */
void projectMatchedFeatures(
        CMatchedFeatureList& mfList, const TStereoSystemParams& param,
        mrpt::maps::CLandmarksMap& landmarks);
 
/** Project a pair of feature lists into the 3D space, using the provided
  *options for the stereo system. The matches must be in order,
  *     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.
  * \param leftList     [IN/OUT]    The left list of matched features.
  * \param rightList    [IN/OUT]    The right list of matched features.
  * \param param        [IN]        The options of the stereo system.
  * \param landmarks    (OUT]       A map containing the projected landmarks.
  * \sa TStereoSystemParams, CLandmarksMap
  */
void projectMatchedFeatures(
        CFeatureList& leftList, CFeatureList& rightList,
        const TStereoSystemParams& param, mrpt::maps::CLandmarksMap& landmarks);
 
/** Converts a stereo images observation into a bearing and range observation.
        \param inObs    [IN]    The input stereo images observation.
        \param sg               [IN]    The sigma of the row, col, and disparity variables
   involved in the feature detection.
        \param outObs   [OUT]   The output bearing and range observation
   (including covariances).
*/
void StereoObs2BRObs(
        const mrpt::obs::CObservationStereoImages& inObs,
        const std::vector<double>& sg, mrpt::obs::CObservationBearingRange& outObs);
 
/** Converts a matched feature list into a bearing and range observation (some
   of the stereo camera system must be provided).
        \param inMatches                [IN]    The input list of matched features.
        \param intrinsicParams  [IN]    The intrisic params of the reference
   (left) camera of the stereo system.
        \param baseline                 [IN]    The distance among the X axis of the
   right camera wrt the reference (left) camera.
        \param sg                               [IN]    The sigma of the row, col, and disparity
   variables involved in the feature detection.
        \param outObs                   [OUT]   The output bearing and range observation
   (including covariances).
*/
void StereoObs2BRObs(
        const CMatchedFeatureList& inMatches,
        const mrpt::math::CMatrixDouble33& intrinsicParams, const double& baseline,
        const mrpt::poses::CPose3D& sensorPose, const std::vector<double>& sg,
        mrpt::obs::CObservationBearingRange& outObs);
 
/** Converts a CObservationVisualLandmarks into a bearing and range observation
   (without any covariances). Fields of view are not computed.
        \param inObs                    [IN]    The input observation.
        \param sg                               [IN]    The sigma of the row, col, and disparity
   variables involved in the feature detection.
        \param outObs                   [OUT]   The output bearing and range observation.
*/
void StereoObs2BRObs(
        const mrpt::obs::CObservationStereoImages& inObs,
        const std::vector<double>& sg, mrpt::obs::CObservationBearingRange& outObs);
 
/** Converts a CObservationVisualLandmarks into a bearing and range observation
   (without any covariances). Fields of view are not computed.
        \param inObs                    [IN]    The input observation.
        \param outObs                   [OUT]   The output bearing and range observation.
*/
void StereoObs2BRObs(
        const mrpt::obs::CObservationVisualLandmarks& inObs,
        mrpt::obs::CObservationBearingRange& outObs);
 
/** 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.
        \param cam1, cam2           [IN]    The pair of involved cameras
        \param rightCameraPose      [IN]    The change in pose of the second camera
wrt the first one
        \param outMap1x,outMap1y    [OUT]   The x-and-y maps corresponding to cam1
(should be converted to *cv::Mat)
        \param outMap2x,outMap2y    [OUT]   The x-and-y maps corresponding to cam2
(should be converted to *cv::Mat)
* \sa An easier to use class for stereo rectification
mrpt::vision::CStereoRectifyMap
*/
void computeStereoRectificationMaps(
        const mrpt::img::TCamera& cam1, const mrpt::img::TCamera& cam2,
        const mrpt::poses::CPose3D& rightCameraPose, void* outMap1x, void* outMap1y,
        void* outMap2x, void* outMap2y);
 
/** @} */  // end of grouping
 
}  // end-namespace-vision
}  // end-namespace-mrpt
 
#endif