From: Roman Donchenko Date: Tue, 8 Jul 2014 10:33:56 +0000 (+0400) Subject: Merge remote-tracking branch 'origin/2.4' into merge-2.4 X-Git-Tag: accepted/tizen/6.0/unified/20201030.111113~3036^2~7 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=a3bde36c84d0ff2fe818ec409900115b2013eff6;p=platform%2Fupstream%2Fopencv.git Merge remote-tracking branch 'origin/2.4' into merge-2.4 Conflicts: modules/calib3d/include/opencv2/calib3d/calib3d.hpp modules/contrib/doc/facerec/facerec_api.rst modules/contrib/include/opencv2/contrib/contrib.hpp modules/contrib/src/facerec.cpp modules/core/include/opencv2/core/mat.hpp modules/features2d/include/opencv2/features2d/features2d.hpp modules/highgui/src/loadsave.cpp modules/imgproc/src/pyramids.cpp modules/ocl/include/opencv2/ocl/cl_runtime/cl_runtime.hpp modules/python/src2/gen.py modules/python/test/test.py modules/superres/test/test_superres.cpp samples/cpp/facerec_demo.cpp --- a3bde36c84d0ff2fe818ec409900115b2013eff6 diff --cc modules/calib3d/include/opencv2/calib3d.hpp index 8b9b69c,0000000..c7b5388 mode 100644,000000..100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@@ -1,416 -1,0 +1,477 @@@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#ifndef __OPENCV_CALIB3D_HPP__ +#define __OPENCV_CALIB3D_HPP__ + +#include "opencv2/core.hpp" +#include "opencv2/features2d.hpp" ++#include "opencv2/core/affine.hpp" + +namespace cv +{ + +//! type of the robust estimation algorithm +enum { LMEDS = 4, //!< least-median algorithm + RANSAC = 8 //!< RANSAC algorithm + }; + +enum { ITERATIVE = 0, + EPNP = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" + P3P = 2 // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" + }; + +enum { CALIB_CB_ADAPTIVE_THRESH = 1, + CALIB_CB_NORMALIZE_IMAGE = 2, + CALIB_CB_FILTER_QUADS = 4, + CALIB_CB_FAST_CHECK = 8 + }; + +enum { CALIB_CB_SYMMETRIC_GRID = 1, + CALIB_CB_ASYMMETRIC_GRID = 2, + CALIB_CB_CLUSTERING = 4 + }; + +enum { CALIB_USE_INTRINSIC_GUESS = 0x00001, + CALIB_FIX_ASPECT_RATIO = 0x00002, + CALIB_FIX_PRINCIPAL_POINT = 0x00004, + CALIB_ZERO_TANGENT_DIST = 0x00008, + CALIB_FIX_FOCAL_LENGTH = 0x00010, + CALIB_FIX_K1 = 0x00020, + CALIB_FIX_K2 = 0x00040, + CALIB_FIX_K3 = 0x00080, + CALIB_FIX_K4 = 0x00800, + CALIB_FIX_K5 = 0x01000, + CALIB_FIX_K6 = 0x02000, + CALIB_RATIONAL_MODEL = 0x04000, + CALIB_THIN_PRISM_MODEL = 0x08000, + CALIB_FIX_S1_S2_S3_S4 = 0x10000, + // only for stereo + CALIB_FIX_INTRINSIC = 0x00100, + CALIB_SAME_FOCAL_LENGTH = 0x00200, + // for stereo rectification + CALIB_ZERO_DISPARITY = 0x00400 + }; + +//! the algorithm for finding fundamental matrix +enum { FM_7POINT = 1, //!< 7-point algorithm + FM_8POINT = 2, //!< 8-point algorithm + FM_LMEDS = 4, //!< least-median algorithm + FM_RANSAC = 8 //!< RANSAC algorithm + }; + + + +//! converts rotation vector to rotation matrix or vice versa using Rodrigues transformation +CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() ); + +//! computes the best-fit perspective transformation mapping srcPoints to dstPoints. +CV_EXPORTS_W Mat findHomography( InputArray srcPoints, InputArray dstPoints, + int method = 0, double ransacReprojThreshold = 3, + OutputArray mask=noArray()); + +//! variant of findHomography for backward compatibility +CV_EXPORTS Mat findHomography( InputArray srcPoints, InputArray dstPoints, + OutputArray mask, int method = 0, double ransacReprojThreshold = 3 ); + +//! Computes RQ decomposition of 3x3 matrix +CV_EXPORTS_W Vec3d RQDecomp3x3( InputArray src, OutputArray mtxR, OutputArray mtxQ, + OutputArray Qx = noArray(), + OutputArray Qy = noArray(), + OutputArray Qz = noArray()); + +//! Decomposes the projection matrix into camera matrix and the rotation martix and the translation vector +CV_EXPORTS_W void decomposeProjectionMatrix( InputArray projMatrix, OutputArray cameraMatrix, + OutputArray rotMatrix, OutputArray transVect, + OutputArray rotMatrixX = noArray(), + OutputArray rotMatrixY = noArray(), + OutputArray rotMatrixZ = noArray(), + OutputArray eulerAngles =noArray() ); + +//! computes derivatives of the matrix product w.r.t each of the multiplied matrix coefficients +CV_EXPORTS_W void matMulDeriv( InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB ); + +//! composes 2 [R|t] transformations together. Also computes the derivatives of the result w.r.t the arguments +CV_EXPORTS_W void composeRT( InputArray rvec1, InputArray tvec1, + InputArray rvec2, InputArray tvec2, + OutputArray rvec3, OutputArray tvec3, + OutputArray dr3dr1 = noArray(), OutputArray dr3dt1 = noArray(), + OutputArray dr3dr2 = noArray(), OutputArray dr3dt2 = noArray(), + OutputArray dt3dr1 = noArray(), OutputArray dt3dt1 = noArray(), + OutputArray dt3dr2 = noArray(), OutputArray dt3dt2 = noArray() ); + +//! projects points from the model coordinate space to the image coordinates. Also computes derivatives of the image coordinates w.r.t the intrinsic and extrinsic camera parameters +CV_EXPORTS_W void projectPoints( InputArray objectPoints, + InputArray rvec, InputArray tvec, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray imagePoints, + OutputArray jacobian = noArray(), + double aspectRatio = 0 ); + +//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are not handled. +CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, + bool useExtrinsicGuess = false, int flags = ITERATIVE ); + +//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible. +CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, InputArray imagePoints, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, + bool useExtrinsicGuess = false, int iterationsCount = 100, + float reprojectionError = 8.0, int minInliersCount = 100, + OutputArray inliers = noArray(), int flags = ITERATIVE ); + +//! initializes camera matrix from a few 3D points and the corresponding projections. +CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, + InputArrayOfArrays imagePoints, + Size imageSize, double aspectRatio = 1.0 ); + +//! finds checkerboard pattern of the specified size in the image +CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, + int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE ); + +//! finds subpixel-accurate positions of the chessboard corners +CV_EXPORTS bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size ); + +//! draws the checkerboard pattern (found or partly found) in the image +CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, + InputArray corners, bool patternWasFound ); + +//! finds circles' grid pattern of the specified size in the image +CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize, + OutputArray centers, int flags = CALIB_CB_SYMMETRIC_GRID, + const Ptr &blobDetector = makePtr()); + +//! finds intrinsic and extrinsic camera parameters from several fews of a known calibration pattern. +CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints, + InputArrayOfArrays imagePoints, Size imageSize, + InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + int flags = 0, TermCriteria criteria = TermCriteria( + TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); + +//! computes several useful camera characteristics from the camera matrix, camera frame resolution and the physical sensor size. +CV_EXPORTS_W void calibrationMatrixValues( InputArray cameraMatrix, Size imageSize, + double apertureWidth, double apertureHeight, + CV_OUT double& fovx, CV_OUT double& fovy, + CV_OUT double& focalLength, CV_OUT Point2d& principalPoint, + CV_OUT double& aspectRatio ); + +//! finds intrinsic and extrinsic parameters of a stereo camera +CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, + InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, + InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, + Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F, + int flags = CALIB_FIX_INTRINSIC, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); + + +//! computes the rectification transformation for a stereo camera from its intrinsic and extrinsic parameters +CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1, + InputArray cameraMatrix2, InputArray distCoeffs2, + Size imageSize, InputArray R, InputArray T, + OutputArray R1, OutputArray R2, + OutputArray P1, OutputArray P2, + OutputArray Q, int flags = CALIB_ZERO_DISPARITY, + double alpha = -1, Size newImageSize = Size(), + CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 ); + +//! computes the rectification transformation for an uncalibrated stereo camera (zero distortion is assumed) +CV_EXPORTS_W bool stereoRectifyUncalibrated( InputArray points1, InputArray points2, + InputArray F, Size imgSize, + OutputArray H1, OutputArray H2, + double threshold = 5 ); + +//! computes the rectification transformations for 3-head camera, where all the heads are on the same line. +CV_EXPORTS_W float rectify3Collinear( InputArray cameraMatrix1, InputArray distCoeffs1, + InputArray cameraMatrix2, InputArray distCoeffs2, + InputArray cameraMatrix3, InputArray distCoeffs3, + InputArrayOfArrays imgpt1, InputArrayOfArrays imgpt3, + Size imageSize, InputArray R12, InputArray T12, + InputArray R13, InputArray T13, + OutputArray R1, OutputArray R2, OutputArray R3, + OutputArray P1, OutputArray P2, OutputArray P3, + OutputArray Q, double alpha, Size newImgSize, + CV_OUT Rect* roi1, CV_OUT Rect* roi2, int flags ); + +//! returns the optimal new camera matrix +CV_EXPORTS_W Mat getOptimalNewCameraMatrix( InputArray cameraMatrix, InputArray distCoeffs, + Size imageSize, double alpha, Size newImgSize = Size(), + CV_OUT Rect* validPixROI = 0, + bool centerPrincipalPoint = false); + +//! converts point coordinates from normal pixel coordinates to homogeneous coordinates ((x,y)->(x,y,1)) +CV_EXPORTS_W void convertPointsToHomogeneous( InputArray src, OutputArray dst ); + +//! converts point coordinates from homogeneous to normal pixel coordinates ((x,y,z)->(x/z, y/z)) +CV_EXPORTS_W void convertPointsFromHomogeneous( InputArray src, OutputArray dst ); + +//! for backward compatibility +CV_EXPORTS void convertPointsHomogeneous( InputArray src, OutputArray dst ); + +//! finds fundamental matrix from a set of corresponding 2D points +CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2, + int method = FM_RANSAC, + double param1 = 3., double param2 = 0.99, + OutputArray mask = noArray() ); + +//! variant of findFundamentalMat for backward compatibility +CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2, + OutputArray mask, int method = FM_RANSAC, + double param1 = 3., double param2 = 0.99 ); + +//! finds essential matrix from a set of corresponding 2D points using five-point algorithm +CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2, + double focal = 1.0, Point2d pp = Point2d(0, 0), + int method = RANSAC, double prob = 0.999, + double threshold = 1.0, OutputArray mask = noArray() ); + +//! decompose essential matrix to possible rotation matrix and one translation vector +CV_EXPORTS_W void decomposeEssentialMat( InputArray E, OutputArray R1, OutputArray R2, OutputArray t ); + +//! recover relative camera pose from a set of corresponding 2D points +CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2, + OutputArray R, OutputArray t, + double focal = 1.0, Point2d pp = Point2d(0, 0), + InputOutputArray mask = noArray() ); + + +//! finds coordinates of epipolar lines corresponding the specified points +CV_EXPORTS_W void computeCorrespondEpilines( InputArray points, int whichImage, + InputArray F, OutputArray lines ); + +CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2, + InputArray projPoints1, InputArray projPoints2, + OutputArray points4D ); + +CV_EXPORTS_W void correctMatches( InputArray F, InputArray points1, InputArray points2, + OutputArray newPoints1, OutputArray newPoints2 ); + +//! filters off speckles (small regions of incorrectly computed disparity) +CV_EXPORTS_W void filterSpeckles( InputOutputArray img, double newVal, + int maxSpeckleSize, double maxDiff, + InputOutputArray buf = noArray() ); + +//! computes valid disparity ROI from the valid ROIs of the rectified images (that are returned by cv::stereoRectify()) +CV_EXPORTS_W Rect getValidDisparityROI( Rect roi1, Rect roi2, + int minDisparity, int numberOfDisparities, + int SADWindowSize ); + +//! validates disparity using the left-right check. The matrix "cost" should be computed by the stereo correspondence algorithm +CV_EXPORTS_W void validateDisparity( InputOutputArray disparity, InputArray cost, + int minDisparity, int numberOfDisparities, + int disp12MaxDisp = 1 ); + +//! reprojects disparity image to 3D: (x,y,d)->(X,Y,Z) using the matrix Q returned by cv::stereoRectify +CV_EXPORTS_W void reprojectImageTo3D( InputArray disparity, + OutputArray _3dImage, InputArray Q, + bool handleMissingValues = false, + int ddepth = -1 ); + +CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst, + OutputArray out, OutputArray inliers, + double ransacThreshold = 3, double confidence = 0.99); + + + +class CV_EXPORTS_W StereoMatcher : public Algorithm +{ +public: + enum { DISP_SHIFT = 4, + DISP_SCALE = (1 << DISP_SHIFT) + }; + + CV_WRAP virtual void compute( InputArray left, InputArray right, + OutputArray disparity ) = 0; + + CV_WRAP virtual int getMinDisparity() const = 0; + CV_WRAP virtual void setMinDisparity(int minDisparity) = 0; + + CV_WRAP virtual int getNumDisparities() const = 0; + CV_WRAP virtual void setNumDisparities(int numDisparities) = 0; + + CV_WRAP virtual int getBlockSize() const = 0; + CV_WRAP virtual void setBlockSize(int blockSize) = 0; + + CV_WRAP virtual int getSpeckleWindowSize() const = 0; + CV_WRAP virtual void setSpeckleWindowSize(int speckleWindowSize) = 0; + + CV_WRAP virtual int getSpeckleRange() const = 0; + CV_WRAP virtual void setSpeckleRange(int speckleRange) = 0; + + CV_WRAP virtual int getDisp12MaxDiff() const = 0; + CV_WRAP virtual void setDisp12MaxDiff(int disp12MaxDiff) = 0; +}; + + + +class CV_EXPORTS_W StereoBM : public StereoMatcher +{ +public: + enum { PREFILTER_NORMALIZED_RESPONSE = 0, + PREFILTER_XSOBEL = 1 + }; + + CV_WRAP virtual int getPreFilterType() const = 0; + CV_WRAP virtual void setPreFilterType(int preFilterType) = 0; + + CV_WRAP virtual int getPreFilterSize() const = 0; + CV_WRAP virtual void setPreFilterSize(int preFilterSize) = 0; + + CV_WRAP virtual int getPreFilterCap() const = 0; + CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0; + + CV_WRAP virtual int getTextureThreshold() const = 0; + CV_WRAP virtual void setTextureThreshold(int textureThreshold) = 0; + + CV_WRAP virtual int getUniquenessRatio() const = 0; + CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0; + + CV_WRAP virtual int getSmallerBlockSize() const = 0; + CV_WRAP virtual void setSmallerBlockSize(int blockSize) = 0; + + CV_WRAP virtual Rect getROI1() const = 0; + CV_WRAP virtual void setROI1(Rect roi1) = 0; + + CV_WRAP virtual Rect getROI2() const = 0; + CV_WRAP virtual void setROI2(Rect roi2) = 0; +}; + +CV_EXPORTS_W Ptr createStereoBM(int numDisparities = 0, int blockSize = 21); + + +class CV_EXPORTS_W StereoSGBM : public StereoMatcher +{ +public: + enum { MODE_SGBM = 0, + MODE_HH = 1 + }; + + CV_WRAP virtual int getPreFilterCap() const = 0; + CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0; + + CV_WRAP virtual int getUniquenessRatio() const = 0; + CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0; + + CV_WRAP virtual int getP1() const = 0; + CV_WRAP virtual void setP1(int P1) = 0; + + CV_WRAP virtual int getP2() const = 0; + CV_WRAP virtual void setP2(int P2) = 0; + + CV_WRAP virtual int getMode() const = 0; + CV_WRAP virtual void setMode(int mode) = 0; +}; + + +CV_EXPORTS_W Ptr createStereoSGBM(int minDisparity, int numDisparities, int blockSize, + int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, + int preFilterCap = 0, int uniquenessRatio = 0, + int speckleWindowSize = 0, int speckleRange = 0, + int mode = StereoSGBM::MODE_SGBM); + ++namespace fisheye ++{ ++ enum{ ++ CALIB_USE_INTRINSIC_GUESS = 1, ++ CALIB_RECOMPUTE_EXTRINSIC = 2, ++ CALIB_CHECK_COND = 4, ++ CALIB_FIX_SKEW = 8, ++ CALIB_FIX_K1 = 16, ++ CALIB_FIX_K2 = 32, ++ CALIB_FIX_K3 = 64, ++ CALIB_FIX_K4 = 128, ++ CALIB_FIX_INTRINSIC = 256 ++ }; ++ ++ //! projects 3D points using fisheye model ++ CV_EXPORTS void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, ++ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); ++ ++ //! projects points using fisheye model ++ CV_EXPORTS void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec, ++ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); ++ ++ //! distorts 2D points using fisheye model ++ CV_EXPORTS void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0); ++ ++ //! undistorts 2D points using fisheye model ++ CV_EXPORTS void undistortPoints(InputArray distorted, OutputArray undistorted, ++ InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray()); ++ ++ //! computing undistortion and rectification maps for image transform by cv::remap() ++ //! If D is empty zero distortion is used, if R or P is empty identity matrixes are used ++ CV_EXPORTS void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P, ++ const cv::Size& size, int m1type, OutputArray map1, OutputArray map2); ++ ++ //! undistorts image, optionally changes resolution and camera matrix. If Knew zero identity matrix is used ++ CV_EXPORTS void undistortImage(InputArray distorted, OutputArray undistorted, ++ InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size()); ++ ++ //! estimates new camera matrix for undistortion or rectification ++ CV_EXPORTS void estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R, ++ OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0); ++ ++ //! performs camera calibaration ++ CV_EXPORTS double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size, ++ InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0, ++ TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); ++ ++ //! stereo rectification estimation ++ CV_EXPORTS void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec, ++ OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(), ++ double balance = 0.0, double fov_scale = 1.0); ++ ++ //! performs stereo calibaration ++ CV_EXPORTS double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, ++ InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, ++ OutputArray R, OutputArray T, int flags = CALIB_FIX_INTRINSIC, ++ TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); ++ ++} ++ +} // cv + +#endif diff --cc modules/calib3d/src/fisheye.cpp index 0000000,77a0e84..ce4144b mode 000000,100644..100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@@ -1,0 -1,1612 +1,1612 @@@ + /*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. + // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and/or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + + #include "precomp.hpp" + #include "fisheye.hpp" + + namespace cv { namespace + { + struct JacobianRow + { + Vec2d df, dc; + Vec4d dk; + Vec3d dom, dT; + double dalpha; + }; + - void subMatrix(const Mat& src, Mat& dst, const vector& cols, const vector& rows); ++ void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows); + }} + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::projectPoints + + void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, + InputArray K, InputArray D, double alpha, OutputArray jacobian) + { + projectPoints(objectPoints, imagePoints, affine.rvec(), affine.translation(), K, D, alpha, jacobian); + } + + void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray _rvec, + InputArray _tvec, InputArray _K, InputArray _D, double alpha, OutputArray jacobian) + { + // will support only 3-channel data now for points + CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3); + imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2)); + size_t n = objectPoints.total(); + + CV_Assert(_rvec.total() * _rvec.channels() == 3 && (_rvec.depth() == CV_32F || _rvec.depth() == CV_64F)); + CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F)); + CV_Assert(_tvec.getMat().isContinuous() && _rvec.getMat().isContinuous()); + + Vec3d om = _rvec.depth() == CV_32F ? (Vec3d)*_rvec.getMat().ptr() : *_rvec.getMat().ptr(); + Vec3d T = _tvec.depth() == CV_32F ? (Vec3d)*_tvec.getMat().ptr() : *_tvec.getMat().ptr(); + + CV_Assert(_K.size() == Size(3,3) && (_K.type() == CV_32F || _K.type() == CV_64F) && _D.type() == _K.type() && _D.total() == 4); + + cv::Vec2d f, c; + if (_K.depth() == CV_32F) + { + + Matx33f K = _K.getMat(); + f = Vec2f(K(0, 0), K(1, 1)); + c = Vec2f(K(0, 2), K(1, 2)); + } + else + { + Matx33d K = _K.getMat(); + f = Vec2d(K(0, 0), K(1, 1)); + c = Vec2d(K(0, 2), K(1, 2)); + } + + Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr(): *_D.getMat().ptr(); + + JacobianRow *Jn = 0; + if (jacobian.needed()) + { + int nvars = 2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T, + jacobian.create(2*(int)n, nvars, CV_64F); + Jn = jacobian.getMat().ptr(0); + } + + Matx33d R; + Matx dRdom; + Rodrigues(om, R, dRdom); + Affine3d aff(om, T); + + const Vec3f* Xf = objectPoints.getMat().ptr(); + const Vec3d* Xd = objectPoints.getMat().ptr(); + Vec2f *xpf = imagePoints.getMat().ptr(); + Vec2d *xpd = imagePoints.getMat().ptr(); + + for(size_t i = 0; i < n; ++i) + { + Vec3d Xi = objectPoints.depth() == CV_32F ? (Vec3d)Xf[i] : Xd[i]; + Vec3d Y = aff*Xi; + + Vec2d x(Y[0]/Y[2], Y[1]/Y[2]); + + double r2 = x.dot(x); + double r = std::sqrt(r2); + + // Angle of the incoming ray: + double theta = atan(r); + + double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta, + theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta; + + double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9; + + double inv_r = r > 1e-8 ? 1.0/r : 1; + double cdist = r > 1e-8 ? theta_d * inv_r : 1; + + Vec2d xd1 = x * cdist; + Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]); + Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]); + + if (objectPoints.depth() == CV_32F) + xpf[i] = final_point; + else + xpd[i] = final_point; + + if (jacobian.needed()) + { + //Vec3d Xi = pdepth == CV_32F ? (Vec3d)Xf[i] : Xd[i]; + //Vec3d Y = aff*Xi; + double dYdR[] = { Xi[0], Xi[1], Xi[2], 0, 0, 0, 0, 0, 0, + 0, 0, 0, Xi[0], Xi[1], Xi[2], 0, 0, 0, + 0, 0, 0, 0, 0, 0, Xi[0], Xi[1], Xi[2] }; + + Matx33d dYdom_data = Matx(dYdR) * dRdom.t(); + const Vec3d *dYdom = (Vec3d*)dYdom_data.val; + + Matx33d dYdT_data = Matx33d::eye(); + const Vec3d *dYdT = (Vec3d*)dYdT_data.val; + + //Vec2d x(Y[0]/Y[2], Y[1]/Y[2]); + Vec3d dxdom[2]; + dxdom[0] = (1.0/Y[2]) * dYdom[0] - x[0]/Y[2] * dYdom[2]; + dxdom[1] = (1.0/Y[2]) * dYdom[1] - x[1]/Y[2] * dYdom[2]; + + Vec3d dxdT[2]; + dxdT[0] = (1.0/Y[2]) * dYdT[0] - x[0]/Y[2] * dYdT[2]; + dxdT[1] = (1.0/Y[2]) * dYdT[1] - x[1]/Y[2] * dYdT[2]; + + //double r2 = x.dot(x); + Vec3d dr2dom = 2 * x[0] * dxdom[0] + 2 * x[1] * dxdom[1]; + Vec3d dr2dT = 2 * x[0] * dxdT[0] + 2 * x[1] * dxdT[1]; + + //double r = std::sqrt(r2); + double drdr2 = r > 1e-8 ? 1.0/(2*r) : 1; + Vec3d drdom = drdr2 * dr2dom; + Vec3d drdT = drdr2 * dr2dT; + + // Angle of the incoming ray: + //double theta = atan(r); + double dthetadr = 1.0/(1+r2); + Vec3d dthetadom = dthetadr * drdom; + Vec3d dthetadT = dthetadr * drdT; + + //double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9; + double dtheta_ddtheta = 1 + 3*k[0]*theta2 + 5*k[1]*theta4 + 7*k[2]*theta6 + 9*k[3]*theta8; + Vec3d dtheta_ddom = dtheta_ddtheta * dthetadom; + Vec3d dtheta_ddT = dtheta_ddtheta * dthetadT; + Vec4d dtheta_ddk = Vec4d(theta3, theta5, theta7, theta9); + + //double inv_r = r > 1e-8 ? 1.0/r : 1; + //double cdist = r > 1e-8 ? theta_d / r : 1; + Vec3d dcdistdom = inv_r * (dtheta_ddom - cdist*drdom); + Vec3d dcdistdT = inv_r * (dtheta_ddT - cdist*drdT); + Vec4d dcdistdk = inv_r * dtheta_ddk; + + //Vec2d xd1 = x * cdist; + Vec4d dxd1dk[2]; + Vec3d dxd1dom[2], dxd1dT[2]; + dxd1dom[0] = x[0] * dcdistdom + cdist * dxdom[0]; + dxd1dom[1] = x[1] * dcdistdom + cdist * dxdom[1]; + dxd1dT[0] = x[0] * dcdistdT + cdist * dxdT[0]; + dxd1dT[1] = x[1] * dcdistdT + cdist * dxdT[1]; + dxd1dk[0] = x[0] * dcdistdk; + dxd1dk[1] = x[1] * dcdistdk; + + //Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]); + Vec4d dxd3dk[2]; + Vec3d dxd3dom[2], dxd3dT[2]; + dxd3dom[0] = dxd1dom[0] + alpha * dxd1dom[1]; + dxd3dom[1] = dxd1dom[1]; + dxd3dT[0] = dxd1dT[0] + alpha * dxd1dT[1]; + dxd3dT[1] = dxd1dT[1]; + dxd3dk[0] = dxd1dk[0] + alpha * dxd1dk[1]; + dxd3dk[1] = dxd1dk[1]; + + Vec2d dxd3dalpha(xd1[1], 0); + + //final jacobian + Jn[0].dom = f[0] * dxd3dom[0]; + Jn[1].dom = f[1] * dxd3dom[1]; + + Jn[0].dT = f[0] * dxd3dT[0]; + Jn[1].dT = f[1] * dxd3dT[1]; + + Jn[0].dk = f[0] * dxd3dk[0]; + Jn[1].dk = f[1] * dxd3dk[1]; + + Jn[0].dalpha = f[0] * dxd3dalpha[0]; + Jn[1].dalpha = 0; //f[1] * dxd3dalpha[1]; + + Jn[0].df = Vec2d(xd3[0], 0); + Jn[1].df = Vec2d(0, xd3[1]); + + Jn[0].dc = Vec2d(1, 0); + Jn[1].dc = Vec2d(0, 1); + + //step to jacobian rows for next point + Jn += 2; + } + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::distortPoints + + void cv::fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha) + { + // will support only 2-channel data now for points + CV_Assert(undistorted.type() == CV_32FC2 || undistorted.type() == CV_64FC2); + distorted.create(undistorted.size(), undistorted.type()); + size_t n = undistorted.total(); + + CV_Assert(K.size() == Size(3,3) && (K.type() == CV_32F || K.type() == CV_64F) && D.total() == 4); + + cv::Vec2d f, c; + if (K.depth() == CV_32F) + { + Matx33f camMat = K.getMat(); + f = Vec2f(camMat(0, 0), camMat(1, 1)); + c = Vec2f(camMat(0, 2), camMat(1, 2)); + } + else + { + Matx33d camMat = K.getMat(); + f = Vec2d(camMat(0, 0), camMat(1, 1)); + c = Vec2d(camMat(0 ,2), camMat(1, 2)); + } + + Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr(): *D.getMat().ptr(); + + const Vec2f* Xf = undistorted.getMat().ptr(); + const Vec2d* Xd = undistorted.getMat().ptr(); + Vec2f *xpf = distorted.getMat().ptr(); + Vec2d *xpd = distorted.getMat().ptr(); + + for(size_t i = 0; i < n; ++i) + { + Vec2d x = undistorted.depth() == CV_32F ? (Vec2d)Xf[i] : Xd[i]; + + double r2 = x.dot(x); + double r = std::sqrt(r2); + + // Angle of the incoming ray: + double theta = atan(r); + + double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta, + theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta; + + double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9; + + double inv_r = r > 1e-8 ? 1.0/r : 1; + double cdist = r > 1e-8 ? theta_d * inv_r : 1; + + Vec2d xd1 = x * cdist; + Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]); + Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]); + + if (undistorted.depth() == CV_32F) + xpf[i] = final_point; + else + xpd[i] = final_point; + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::undistortPoints + + void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray R, InputArray P) + { + // will support only 2-channel data now for points + CV_Assert(distorted.type() == CV_32FC2 || distorted.type() == CV_64FC2); + undistorted.create(distorted.size(), distorted.type()); + + CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3)); + CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3); + CV_Assert(D.total() == 4 && K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F)); + + cv::Vec2d f, c; + if (K.depth() == CV_32F) + { + Matx33f camMat = K.getMat(); + f = Vec2f(camMat(0, 0), camMat(1, 1)); + c = Vec2f(camMat(0, 2), camMat(1, 2)); + } + else + { + Matx33d camMat = K.getMat(); + f = Vec2d(camMat(0, 0), camMat(1, 1)); + c = Vec2d(camMat(0, 2), camMat(1, 2)); + } + + Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr(): *D.getMat().ptr(); + + cv::Matx33d RR = cv::Matx33d::eye(); + if (!R.empty() && R.total() * R.channels() == 3) + { + cv::Vec3d rvec; + R.getMat().convertTo(rvec, CV_64F); + RR = cv::Affine3d(rvec).rotation(); + } + else if (!R.empty() && R.size() == Size(3, 3)) + R.getMat().convertTo(RR, CV_64F); + + if(!P.empty()) + { + cv::Matx33d PP; + P.getMat().colRange(0, 3).convertTo(PP, CV_64F); + RR = PP * RR; + } + + // start undistorting + const cv::Vec2f* srcf = distorted.getMat().ptr(); + const cv::Vec2d* srcd = distorted.getMat().ptr(); + cv::Vec2f* dstf = undistorted.getMat().ptr(); + cv::Vec2d* dstd = undistorted.getMat().ptr(); + + size_t n = distorted.total(); + int sdepth = distorted.depth(); + + for(size_t i = 0; i < n; i++ ) + { + Vec2d pi = sdepth == CV_32F ? (Vec2d)srcf[i] : srcd[i]; // image point + Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point + + double scale = 1.0; + + double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]); + if (theta_d > 1e-8) + { + // compensate distortion iteratively + double theta = theta_d; + for(int j = 0; j < 10; j++ ) + { + double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2; + theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8); + } + + scale = std::tan(theta) / theta_d; + } + + Vec2d pu = pw * scale; //undistorted point + + // reproject + Vec3d pr = RR * Vec3d(pu[0], pu[1], 1.0); // rotated point optionally multiplied by new camera matrix + Vec2d fi(pr[0]/pr[2], pr[1]/pr[2]); // final + + if( sdepth == CV_32F ) + dstf[i] = fi; + else + dstd[i] = fi; + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::undistortPoints + + void cv::fisheye::initUndistortRectifyMap( InputArray K, InputArray D, InputArray R, InputArray P, + const cv::Size& size, int m1type, OutputArray map1, OutputArray map2 ) + { + CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 ); + map1.create( size, m1type <= 0 ? CV_16SC2 : m1type ); + map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F ); + + CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F)); + CV_Assert((P.depth() == CV_32F || P.depth() == CV_64F) && (R.depth() == CV_32F || R.depth() == CV_64F)); + CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4)); + CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3); + CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3)); + + cv::Vec2d f, c; + if (K.depth() == CV_32F) + { + Matx33f camMat = K.getMat(); + f = Vec2f(camMat(0, 0), camMat(1, 1)); + c = Vec2f(camMat(0, 2), camMat(1, 2)); + } + else + { + Matx33d camMat = K.getMat(); + f = Vec2d(camMat(0, 0), camMat(1, 1)); + c = Vec2d(camMat(0, 2), camMat(1, 2)); + } + + Vec4d k = Vec4d::all(0); + if (!D.empty()) + k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr(): *D.getMat().ptr(); + + cv::Matx33d RR = cv::Matx33d::eye(); + if (!R.empty() && R.total() * R.channels() == 3) + { + cv::Vec3d rvec; + R.getMat().convertTo(rvec, CV_64F); + RR = Affine3d(rvec).rotation(); + } + else if (!R.empty() && R.size() == Size(3, 3)) + R.getMat().convertTo(RR, CV_64F); + + cv::Matx33d PP = cv::Matx33d::eye(); + if (!P.empty()) + P.getMat().colRange(0, 3).convertTo(PP, CV_64F); + + cv::Matx33d iR = (PP * RR).inv(cv::DECOMP_SVD); + + for( int i = 0; i < size.height; ++i) + { + float* m1f = map1.getMat().ptr(i); + float* m2f = map2.getMat().ptr(i); + short* m1 = (short*)m1f; + ushort* m2 = (ushort*)m2f; + + double _x = i*iR(0, 1) + iR(0, 2), + _y = i*iR(1, 1) + iR(1, 2), + _w = i*iR(2, 1) + iR(2, 2); + + for( int j = 0; j < size.width; ++j) + { + double x = _x/_w, y = _y/_w; + + double r = sqrt(x*x + y*y); + double theta = atan(r); + + double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta4*theta4; + double theta_d = theta * (1 + k[0]*theta2 + k[1]*theta4 + k[2]*theta6 + k[3]*theta8); + + double scale = (r == 0) ? 1.0 : theta_d / r; + double u = f[0]*x*scale + c[0]; + double v = f[1]*y*scale + c[1]; + + if( m1type == CV_16SC2 ) + { + int iu = cv::saturate_cast(u*cv::INTER_TAB_SIZE); + int iv = cv::saturate_cast(v*cv::INTER_TAB_SIZE); + m1[j*2+0] = (short)(iu >> cv::INTER_BITS); + m1[j*2+1] = (short)(iv >> cv::INTER_BITS); + m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1))); + } + else if( m1type == CV_32FC1 ) + { + m1f[j] = (float)u; + m2f[j] = (float)v; + } + + _x += iR(0, 0); + _y += iR(1, 0); + _w += iR(2, 0); + } + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::undistortImage + + void cv::fisheye::undistortImage(InputArray distorted, OutputArray undistorted, + InputArray K, InputArray D, InputArray Knew, const Size& new_size) + { + Size size = new_size.area() != 0 ? new_size : distorted.size(); + + cv::Mat map1, map2; + fisheye::initUndistortRectifyMap(K, D, cv::Matx33d::eye(), Knew, size, CV_16SC2, map1, map2 ); + cv::remap(distorted, undistorted, map1, map2, INTER_LINEAR, BORDER_CONSTANT); + } + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::estimateNewCameraMatrixForUndistortRectify + + void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R, + OutputArray P, double balance, const Size& new_size, double fov_scale) + { + CV_Assert( K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F)); + CV_Assert((D.empty() || D.total() == 4) && (D.depth() == CV_32F || D.depth() == CV_64F || D.empty())); + + int w = image_size.width, h = image_size.height; + balance = std::min(std::max(balance, 0.0), 1.0); + + cv::Mat points(1, 4, CV_64FC2); + Vec2d* pptr = points.ptr(); + pptr[0] = Vec2d(w/2, 0); + pptr[1] = Vec2d(w, h/2); + pptr[2] = Vec2d(w/2, h); + pptr[3] = Vec2d(0, h/2); + + #if 0 + const int N = 10; + cv::Mat points(1, N * 4, CV_64FC2); + Vec2d* pptr = points.ptr(); + for(int i = 0, k = 0; i < 10; ++i) + { + pptr[k++] = Vec2d(w/2, 0) - Vec2d(w/8, 0) + Vec2d(w/4/N*i, 0); + pptr[k++] = Vec2d(w/2, h-1) - Vec2d(w/8, h-1) + Vec2d(w/4/N*i, h-1); + + pptr[k++] = Vec2d(0, h/2) - Vec2d(0, h/8) + Vec2d(0, h/4/N*i); + pptr[k++] = Vec2d(w-1, h/2) - Vec2d(w-1, h/8) + Vec2d(w-1, h/4/N*i); + } + #endif + + fisheye::undistortPoints(points, points, K, D, R); + cv::Scalar center_mass = mean(points); + cv::Vec2d cn(center_mass.val); + + double aspect_ratio = (K.depth() == CV_32F) ? K.getMat().at(0,0)/K.getMat().at (1,1) + : K.getMat().at(0,0)/K.getMat().at(1,1); + + // convert to identity ratio + cn[0] *= aspect_ratio; + for(size_t i = 0; i < points.total(); ++i) + pptr[i][1] *= aspect_ratio; + + double minx = DBL_MAX, miny = DBL_MAX, maxx = -DBL_MAX, maxy = -DBL_MAX; + for(size_t i = 0; i < points.total(); ++i) + { + miny = std::min(miny, pptr[i][1]); + maxy = std::max(maxy, pptr[i][1]); + minx = std::min(minx, pptr[i][0]); + maxx = std::max(maxx, pptr[i][0]); + } + + #if 0 + double minx = -DBL_MAX, miny = -DBL_MAX, maxx = DBL_MAX, maxy = DBL_MAX; + for(size_t i = 0; i < points.total(); ++i) + { + if (i % 4 == 0) miny = std::max(miny, pptr[i][1]); + if (i % 4 == 1) maxy = std::min(maxy, pptr[i][1]); + if (i % 4 == 2) minx = std::max(minx, pptr[i][0]); + if (i % 4 == 3) maxx = std::min(maxx, pptr[i][0]); + } + #endif + + double f1 = w * 0.5/(cn[0] - minx); + double f2 = w * 0.5/(maxx - cn[0]); + double f3 = h * 0.5 * aspect_ratio/(cn[1] - miny); + double f4 = h * 0.5 * aspect_ratio/(maxy - cn[1]); + + double fmin = std::min(f1, std::min(f2, std::min(f3, f4))); + double fmax = std::max(f1, std::max(f2, std::max(f3, f4))); + + double f = balance * fmin + (1.0 - balance) * fmax; + f *= fov_scale > 0 ? 1.0/fov_scale : 1.0; + + cv::Vec2d new_f(f, f), new_c = -cn * f + Vec2d(w, h * aspect_ratio) * 0.5; + + // restore aspect ratio + new_f[1] /= aspect_ratio; + new_c[1] /= aspect_ratio; + + if (new_size.area() > 0) + { + double rx = new_size.width /(double)image_size.width; + double ry = new_size.height/(double)image_size.height; + + new_f[0] *= rx; new_f[1] *= ry; + new_c[0] *= rx; new_c[1] *= ry; + } + + Mat(Matx33d(new_f[0], 0, new_c[0], + 0, new_f[1], new_c[1], + 0, 0, 1)).convertTo(P, P.empty() ? K.type() : P.type()); + } + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::stereoRectify + + void cv::fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size& imageSize, + InputArray _R, InputArray _tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, + OutputArray Q, int flags, const Size& newImageSize, double balance, double fov_scale) + { + CV_Assert((_R.size() == Size(3, 3) || _R.total() * _R.channels() == 3) && (_R.depth() == CV_32F || _R.depth() == CV_64F)); + CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F)); + + + cv::Mat aaa = _tvec.getMat().reshape(3, 1); + + Vec3d rvec; // Rodrigues vector + if (_R.size() == Size(3, 3)) + { + cv::Matx33d rmat; + _R.getMat().convertTo(rmat, CV_64F); + rvec = Affine3d(rmat).rvec(); + } + else if (_R.total() * _R.channels() == 3) + _R.getMat().convertTo(rvec, CV_64F); + + Vec3d tvec; + _tvec.getMat().convertTo(tvec, CV_64F); + + // rectification algorithm + rvec *= -0.5; // get average rotation + + Matx33d r_r; + Rodrigues(rvec, r_r); // rotate cameras to same orientation by averaging + + Vec3d t = r_r * tvec; + Vec3d uu(t[0] > 0 ? 1 : -1, 0, 0); + + // calculate global Z rotation + Vec3d ww = t.cross(uu); + double nw = norm(ww); + if (nw > 0.0) + ww *= acos(fabs(t[0])/cv::norm(t))/nw; + + Matx33d wr; + Rodrigues(ww, wr); + + // apply to both views + Matx33d ri1 = wr * r_r.t(); + Mat(ri1, false).convertTo(R1, R1.empty() ? CV_64F : R1.type()); + Matx33d ri2 = wr * r_r; + Mat(ri2, false).convertTo(R2, R2.empty() ? CV_64F : R2.type()); + Vec3d tnew = ri2 * tvec; + + // calculate projection/camera matrices. these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) + Matx33d newK1, newK2; + estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, R1, newK1, balance, newImageSize, fov_scale); + estimateNewCameraMatrixForUndistortRectify(K2, D2, imageSize, R2, newK2, balance, newImageSize, fov_scale); + + double fc_new = std::min(newK1(1,1), newK2(1,1)); + Point2d cc_new[2] = { Vec2d(newK1(0, 2), newK1(1, 2)), Vec2d(newK2(0, 2), newK2(1, 2)) }; + + // Vertical focal length must be the same for both images to keep the epipolar constraint use fy for fx also. + // For simplicity, set the principal points for both cameras to be the average + // of the two principal points (either one of or both x- and y- coordinates) + if( flags & cv::CALIB_ZERO_DISPARITY ) + cc_new[0] = cc_new[1] = (cc_new[0] + cc_new[1]) * 0.5; + else + cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + + Mat(Matx34d(fc_new, 0, cc_new[0].x, 0, + 0, fc_new, cc_new[0].y, 0, + 0, 0, 1, 0), false).convertTo(P1, P1.empty() ? CV_64F : P1.type()); + + Mat(Matx34d(fc_new, 0, cc_new[1].x, tnew[0]*fc_new, // baseline * focal length;, + 0, fc_new, cc_new[1].y, 0, + 0, 0, 1, 0), false).convertTo(P2, P2.empty() ? CV_64F : P2.type()); + + if (Q.needed()) + Mat(Matx44d(1, 0, 0, -cc_new[0].x, + 0, 1, 0, -cc_new[0].y, + 0, 0, 0, fc_new, + 0, 0, -1./tnew[0], (cc_new[0].x - cc_new[1].x)/tnew[0]), false).convertTo(Q, Q.empty() ? CV_64F : Q.depth()); + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::calibrate + + double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size, + InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + int flags , cv::TermCriteria criteria) + { + CV_Assert(!objectPoints.empty() && !imagePoints.empty() && objectPoints.total() == imagePoints.total()); + CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3); + CV_Assert(imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2); + CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty()); + CV_Assert((!D.empty() && D.total() == 4) || D.empty()); + CV_Assert((!rvecs.empty() && rvecs.channels() == 3) || rvecs.empty()); + CV_Assert((!tvecs.empty() && tvecs.channels() == 3) || tvecs.empty()); + + CV_Assert(((flags & CALIB_USE_INTRINSIC_GUESS) && !K.empty() && !D.empty()) || !(flags & CALIB_USE_INTRINSIC_GUESS)); + + using namespace cv::internal; + //-------------------------------Initialization + IntrinsicParams finalParam; + IntrinsicParams currentParam; + IntrinsicParams errors; + + finalParam.isEstimate[0] = 1; + finalParam.isEstimate[1] = 1; + finalParam.isEstimate[2] = 1; + finalParam.isEstimate[3] = 1; + finalParam.isEstimate[4] = flags & CALIB_FIX_SKEW ? 0 : 1; + finalParam.isEstimate[5] = flags & CALIB_FIX_K1 ? 0 : 1; + finalParam.isEstimate[6] = flags & CALIB_FIX_K2 ? 0 : 1; + finalParam.isEstimate[7] = flags & CALIB_FIX_K3 ? 0 : 1; + finalParam.isEstimate[8] = flags & CALIB_FIX_K4 ? 0 : 1; + + const int recompute_extrinsic = flags & CALIB_RECOMPUTE_EXTRINSIC ? 1: 0; + const int check_cond = flags & CALIB_CHECK_COND ? 1 : 0; + + const double alpha_smooth = 0.4; + const double thresh_cond = 1e6; + double change = 1; + Vec2d err_std; + + Matx33d _K; + Vec4d _D; + if (flags & CALIB_USE_INTRINSIC_GUESS) + { + K.getMat().convertTo(_K, CV_64FC1); + D.getMat().convertTo(_D, CV_64FC1); + finalParam.Init(Vec2d(_K(0,0), _K(1, 1)), + Vec2d(_K(0,2), _K(1, 2)), + Vec4d(flags & CALIB_FIX_K1 ? 0 : _D[0], + flags & CALIB_FIX_K2 ? 0 : _D[1], + flags & CALIB_FIX_K3 ? 0 : _D[2], + flags & CALIB_FIX_K4 ? 0 : _D[3]), + _K(0, 1) / _K(0, 0)); + } + else + { + finalParam.Init(Vec2d(max(image_size.width, image_size.height) / CV_PI, max(image_size.width, image_size.height) / CV_PI), + Vec2d(image_size.width / 2.0 - 0.5, image_size.height / 2.0 - 0.5)); + } + + errors.isEstimate = finalParam.isEstimate; + + std::vector omc(objectPoints.total()), Tc(objectPoints.total()); + + CalibrateExtrinsics(objectPoints, imagePoints, finalParam, check_cond, thresh_cond, omc, Tc); + + + //-------------------------------Optimization + for(int iter = 0; ; ++iter) + { + if ((criteria.type == 1 && iter >= criteria.maxCount) || + (criteria.type == 2 && change <= criteria.epsilon) || + (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount))) + break; + + double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0); + + Mat JJ2_inv, ex3; + ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2_inv, ex3); + + Mat G = alpha_smooth2 * JJ2_inv * ex3; + + currentParam = finalParam + G; + + change = norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]) - + Vec4d(finalParam.f[0], finalParam.f[1], finalParam.c[0], finalParam.c[1])) + / norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1])); + + finalParam = currentParam; + + if (recompute_extrinsic) + { + CalibrateExtrinsics(objectPoints, imagePoints, finalParam, check_cond, + thresh_cond, omc, Tc); + } + } + + //-------------------------------Validation + double rms; + EstimateUncertainties(objectPoints, imagePoints, finalParam, omc, Tc, errors, err_std, thresh_cond, + check_cond, rms); + + //------------------------------- + _K = Matx33d(finalParam.f[0], finalParam.f[0] * finalParam.alpha, finalParam.c[0], + 0, finalParam.f[1], finalParam.c[1], + 0, 0, 1); + + if (K.needed()) cv::Mat(_K).convertTo(K, K.empty() ? CV_64FC1 : K.type()); + if (D.needed()) cv::Mat(finalParam.k).convertTo(D, D.empty() ? CV_64FC1 : D.type()); + if (rvecs.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type()); + if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type()); + + return rms; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// cv::fisheye::stereoCalibrate + + double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, int flags, TermCriteria criteria) + { + CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty()); + CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total()); + CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3); + CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2); + CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2); + + CV_Assert((!K1.empty() && K1.size() == Size(3,3)) || K1.empty()); + CV_Assert((!D1.empty() && D1.total() == 4) || D1.empty()); + CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty()); + CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty()); + + CV_Assert(((flags & CALIB_FIX_INTRINSIC) && !K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC)); + + //-------------------------------Initialization + + const int threshold = 50; + const double thresh_cond = 1e6; + const int check_cond = 1; + + int n_points = (int)objectPoints.getMat(0).total(); + int n_images = (int)objectPoints.total(); + + double change = 1; + + cv::internal::IntrinsicParams intrinsicLeft; + cv::internal::IntrinsicParams intrinsicRight; + + cv::internal::IntrinsicParams intrinsicLeft_errors; + cv::internal::IntrinsicParams intrinsicRight_errors; + + Matx33d _K1, _K2; + Vec4d _D1, _D2; + if (!K1.empty()) K1.getMat().convertTo(_K1, CV_64FC1); + if (!D1.empty()) D1.getMat().convertTo(_D1, CV_64FC1); + if (!K2.empty()) K2.getMat().convertTo(_K2, CV_64FC1); + if (!D2.empty()) D2.getMat().convertTo(_D2, CV_64FC1); + + std::vector rvecs1(n_images), tvecs1(n_images), rvecs2(n_images), tvecs2(n_images); + + if (!(flags & CALIB_FIX_INTRINSIC)) + { + calibrate(objectPoints, imagePoints1, imageSize, _K1, _D1, rvecs1, tvecs1, flags, TermCriteria(3, 20, 1e-6)); + calibrate(objectPoints, imagePoints2, imageSize, _K2, _D2, rvecs2, tvecs2, flags, TermCriteria(3, 20, 1e-6)); + } + + intrinsicLeft.Init(Vec2d(_K1(0,0), _K1(1, 1)), Vec2d(_K1(0,2), _K1(1, 2)), + Vec4d(_D1[0], _D1[1], _D1[2], _D1[3]), _K1(0, 1) / _K1(0, 0)); + + intrinsicRight.Init(Vec2d(_K2(0,0), _K2(1, 1)), Vec2d(_K2(0,2), _K2(1, 2)), + Vec4d(_D2[0], _D2[1], _D2[2], _D2[3]), _K2(0, 1) / _K2(0, 0)); + + if ((flags & CALIB_FIX_INTRINSIC)) + { + internal::CalibrateExtrinsics(objectPoints, imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1); + internal::CalibrateExtrinsics(objectPoints, imagePoints2, intrinsicRight, check_cond, thresh_cond, rvecs2, tvecs2); + } + + intrinsicLeft.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1; + + intrinsicRight.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1; + + intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate; + intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate; + + std::vector selectedParams; + std::vector tmp(6 * (n_images + 1), 1); + selectedParams.insert(selectedParams.end(), intrinsicLeft.isEstimate.begin(), intrinsicLeft.isEstimate.end()); + selectedParams.insert(selectedParams.end(), intrinsicRight.isEstimate.begin(), intrinsicRight.isEstimate.end()); + selectedParams.insert(selectedParams.end(), tmp.begin(), tmp.end()); + + //Init values for rotation and translation between two views + cv::Mat om_list(1, n_images, CV_64FC3), T_list(1, n_images, CV_64FC3); + cv::Mat om_ref, R_ref, T_ref, R1, R2; + for (int image_idx = 0; image_idx < n_images; ++image_idx) + { + cv::Rodrigues(rvecs1[image_idx], R1); + cv::Rodrigues(rvecs2[image_idx], R2); + R_ref = R2 * R1.t(); + T_ref = cv::Mat(tvecs2[image_idx]) - R_ref * cv::Mat(tvecs1[image_idx]); + cv::Rodrigues(R_ref, om_ref); + om_ref.reshape(3, 1).copyTo(om_list.col(image_idx)); + T_ref.reshape(3, 1).copyTo(T_list.col(image_idx)); + } + cv::Vec3d omcur = internal::median3d(om_list); + cv::Vec3d Tcur = internal::median3d(T_list); + + cv::Mat J = cv::Mat::zeros(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1), + e = cv::Mat::zeros(4 * n_points * n_images, 1, CV_64FC1), Jkk, ekk; + cv::Mat J2_inv; + + for(int iter = 0; ; ++iter) + { + if ((criteria.type == 1 && iter >= criteria.maxCount) || + (criteria.type == 2 && change <= criteria.epsilon) || + (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount))) + break; + + J.create(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1); + e.create(4 * n_points * n_images, 1, CV_64FC1); + Jkk.create(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1); + ekk.create(4 * n_points, 1, CV_64FC1); + + cv::Mat omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT; + + for (int image_idx = 0; image_idx < n_images; ++image_idx) + { + Jkk = cv::Mat::zeros(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1); + + cv::Mat object = objectPoints.getMat(image_idx).clone(); + cv::Mat imageLeft = imagePoints1.getMat(image_idx).clone(); + cv::Mat imageRight = imagePoints2.getMat(image_idx).clone(); + cv::Mat jacobians, projected; + + //left camera jacobian + cv::Mat rvec = cv::Mat(rvecs1[image_idx]); + cv::Mat tvec = cv::Mat(tvecs1[image_idx]); + cv::internal::projectPoints(object, projected, rvec, tvec, intrinsicLeft, jacobians); + cv::Mat(cv::Mat((imageLeft - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(0, 2 * n_points)); + jacobians.colRange(8, 11).copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(0, 2 * n_points)); + jacobians.colRange(11, 14).copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(0, 2 * n_points)); + jacobians.colRange(0, 2).copyTo(Jkk.colRange(0, 2).rowRange(0, 2 * n_points)); + jacobians.colRange(2, 4).copyTo(Jkk.colRange(2, 4).rowRange(0, 2 * n_points)); + jacobians.colRange(4, 8).copyTo(Jkk.colRange(5, 9).rowRange(0, 2 * n_points)); + jacobians.col(14).copyTo(Jkk.col(4).rowRange(0, 2 * n_points)); + + //right camera jacobian + internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT); + rvec = cv::Mat(rvecs2[image_idx]); + tvec = cv::Mat(tvecs2[image_idx]); + + cv::internal::projectPoints(object, projected, omr, Tr, intrinsicRight, jacobians); + cv::Mat(cv::Mat((imageRight - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(2 * n_points, 4 * n_points)); + cv::Mat dxrdom = jacobians.colRange(8, 11) * domrdom + jacobians.colRange(11, 14) * dTrdom; + cv::Mat dxrdT = jacobians.colRange(8, 11) * domrdT + jacobians.colRange(11, 14)* dTrdT; + cv::Mat dxrdomckk = jacobians.colRange(8, 11) * domrdomckk + jacobians.colRange(11, 14) * dTrdomckk; + cv::Mat dxrdTckk = jacobians.colRange(8, 11) * domrdTckk + jacobians.colRange(11, 14) * dTrdTckk; + + dxrdom.copyTo(Jkk.colRange(18, 21).rowRange(2 * n_points, 4 * n_points)); + dxrdT.copyTo(Jkk.colRange(21, 24).rowRange(2 * n_points, 4 * n_points)); + dxrdomckk.copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(2 * n_points, 4 * n_points)); + dxrdTckk.copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(2 * n_points, 4 * n_points)); + jacobians.colRange(0, 2).copyTo(Jkk.colRange(9 + 0, 9 + 2).rowRange(2 * n_points, 4 * n_points)); + jacobians.colRange(2, 4).copyTo(Jkk.colRange(9 + 2, 9 + 4).rowRange(2 * n_points, 4 * n_points)); + jacobians.colRange(4, 8).copyTo(Jkk.colRange(9 + 5, 9 + 9).rowRange(2 * n_points, 4 * n_points)); + jacobians.col(14).copyTo(Jkk.col(9 + 4).rowRange(2 * n_points, 4 * n_points)); + + //check goodness of sterepair + double abs_max = 0; + for (int i = 0; i < 4 * n_points; i++) + { + if (fabs(ekk.at(i)) > abs_max) + { + abs_max = fabs(ekk.at(i)); + } + } + + CV_Assert(abs_max < threshold); // bad stereo pair + + Jkk.copyTo(J.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points)); + ekk.copyTo(e.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points)); + } + + cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]); + + //update all parameters + cv::subMatrix(J, J, selectedParams, std::vector(J.rows, 1)); + cv::Mat J2 = J.t() * J; + J2_inv = J2.inv(); + int a = cv::countNonZero(intrinsicLeft.isEstimate); + int b = cv::countNonZero(intrinsicRight.isEstimate); + cv::Mat deltas = J2_inv * J.t() * e; + intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a); + intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b); + omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3)); + Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6)); + for (int image_idx = 0; image_idx < n_images; ++image_idx) + { + rvecs1[image_idx] = cv::Mat(cv::Mat(rvecs1[image_idx]) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6)); + tvecs1[image_idx] = cv::Mat(cv::Mat(tvecs1[image_idx]) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6)); + } + + cv::Vec6d newTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]); + change = cv::norm(newTom - oldTom) / cv::norm(newTom); + } + + double rms = 0; + const Vec2d* ptr_e = e.ptr(); + for (size_t i = 0; i < e.total() / 2; i++) + { + rms += ptr_e[i][0] * ptr_e[i][0] + ptr_e[i][1] * ptr_e[i][1]; + } + + rms /= ((double)e.total() / 2.0); + rms = sqrt(rms); + + _K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0], + 0, intrinsicLeft.f[1], intrinsicLeft.c[1], + 0, 0, 1); + + _K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0], + 0, intrinsicRight.f[1], intrinsicRight.c[1], + 0, 0, 1); + + Mat _R; + Rodrigues(omcur, _R); + + if (K1.needed()) cv::Mat(_K1).convertTo(K1, K1.empty() ? CV_64FC1 : K1.type()); + if (K2.needed()) cv::Mat(_K2).convertTo(K2, K2.empty() ? CV_64FC1 : K2.type()); + if (D1.needed()) cv::Mat(intrinsicLeft.k).convertTo(D1, D1.empty() ? CV_64FC1 : D1.type()); + if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type()); + if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type()); + if (T.needed()) cv::Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.type()); + + return rms; + } + + namespace cv{ namespace { -void subMatrix(const Mat& src, Mat& dst, const vector& cols, const vector& rows) ++void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows) + { + CV_Assert(src.type() == CV_64FC1); + + int nonzeros_cols = cv::countNonZero(cols); + Mat tmp(src.rows, nonzeros_cols, CV_64FC1); + + for (int i = 0, j = 0; i < (int)cols.size(); i++) + { + if (cols[i]) + { + src.col(i).copyTo(tmp.col(j++)); + } + } + + int nonzeros_rows = cv::countNonZero(rows); + Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1); + for (int i = 0, j = 0; i < (int)rows.size(); i++) + { + if (rows[i]) + { + tmp.row(i).copyTo(tmp1.row(j++)); + } + } + + dst = tmp1.clone(); + } + + }} + + cv::internal::IntrinsicParams::IntrinsicParams(): + f(Vec2d::all(0)), c(Vec2d::all(0)), k(Vec4d::all(0)), alpha(0), isEstimate(9,0) + { + } + + cv::internal::IntrinsicParams::IntrinsicParams(Vec2d _f, Vec2d _c, Vec4d _k, double _alpha): + f(_f), c(_c), k(_k), alpha(_alpha), isEstimate(9,0) + { + } + + cv::internal::IntrinsicParams cv::internal::IntrinsicParams::operator+(const Mat& a) + { + CV_Assert(a.type() == CV_64FC1); + IntrinsicParams tmp; + const double* ptr = a.ptr(); + + int j = 0; + tmp.f[0] = this->f[0] + (isEstimate[0] ? ptr[j++] : 0); + tmp.f[1] = this->f[1] + (isEstimate[1] ? ptr[j++] : 0); + tmp.c[0] = this->c[0] + (isEstimate[2] ? ptr[j++] : 0); + tmp.alpha = this->alpha + (isEstimate[4] ? ptr[j++] : 0); + tmp.c[1] = this->c[1] + (isEstimate[3] ? ptr[j++] : 0); + tmp.k[0] = this->k[0] + (isEstimate[5] ? ptr[j++] : 0); + tmp.k[1] = this->k[1] + (isEstimate[6] ? ptr[j++] : 0); + tmp.k[2] = this->k[2] + (isEstimate[7] ? ptr[j++] : 0); + tmp.k[3] = this->k[3] + (isEstimate[8] ? ptr[j++] : 0); + + tmp.isEstimate = isEstimate; + return tmp; + } + + cv::internal::IntrinsicParams& cv::internal::IntrinsicParams::operator =(const Mat& a) + { + CV_Assert(a.type() == CV_64FC1); + const double* ptr = a.ptr(); + + int j = 0; + + this->f[0] = isEstimate[0] ? ptr[j++] : 0; + this->f[1] = isEstimate[1] ? ptr[j++] : 0; + this->c[0] = isEstimate[2] ? ptr[j++] : 0; + this->c[1] = isEstimate[3] ? ptr[j++] : 0; + this->alpha = isEstimate[4] ? ptr[j++] : 0; + this->k[0] = isEstimate[5] ? ptr[j++] : 0; + this->k[1] = isEstimate[6] ? ptr[j++] : 0; + this->k[2] = isEstimate[7] ? ptr[j++] : 0; + this->k[3] = isEstimate[8] ? ptr[j++] : 0; + + return *this; + } + + void cv::internal::IntrinsicParams::Init(const cv::Vec2d& _f, const cv::Vec2d& _c, const cv::Vec4d& _k, const double& _alpha) + { + this->c = _c; + this->f = _f; + this->k = _k; + this->alpha = _alpha; + } + + void cv::internal::projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints, + cv::InputArray _rvec,cv::InputArray _tvec, + const IntrinsicParams& param, cv::OutputArray jacobian) + { + CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3); + Matx33d K(param.f[0], param.f[0] * param.alpha, param.c[0], + 0, param.f[1], param.c[1], + 0, 0, 1); + fisheye::projectPoints(objectPoints, imagePoints, _rvec, _tvec, K, param.k, param.alpha, jacobian); + } + + void cv::internal::ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec, + Mat& tvec, Mat& J, const int MaxIter, + const IntrinsicParams& param, const double thresh_cond) + { + CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3); + CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2); + Vec6d extrinsics(rvec.at(0), rvec.at(1), rvec.at(2), + tvec.at(0), tvec.at(1), tvec.at(2)); + double change = 1; + int iter = 0; + + while (change > 1e-10 && iter < MaxIter) + { + std::vector x; + Mat jacobians; + projectPoints(objectPoints, x, rvec, tvec, param, jacobians); + + Mat ex = imagePoints - Mat(x).t(); + ex = ex.reshape(1, 2); + + J = jacobians.colRange(8, 14).clone(); + + SVD svd(J, SVD::NO_UV); + double condJJ = svd.w.at(0)/svd.w.at(5); + + if (condJJ > thresh_cond) + change = 0; + else + { + Vec6d param_innov; + solve(J, ex.reshape(1, (int)ex.total()), param_innov, DECOMP_SVD + DECOMP_NORMAL); + + Vec6d param_up = extrinsics + param_innov; + change = norm(param_innov)/norm(param_up); + extrinsics = param_up; + iter = iter + 1; + + rvec = Mat(Vec3d(extrinsics.val)); + tvec = Mat(Vec3d(extrinsics.val+3)); + } + } + } + + cv::Mat cv::internal::ComputeHomography(Mat m, Mat M) + { + int Np = m.cols; + + if (m.rows < 3) + { + vconcat(m, Mat::ones(1, Np, CV_64FC1), m); + } + if (M.rows < 3) + { + vconcat(M, Mat::ones(1, Np, CV_64FC1), M); + } + + divide(m, Mat::ones(3, 1, CV_64FC1) * m.row(2), m); + divide(M, Mat::ones(3, 1, CV_64FC1) * M.row(2), M); + + Mat ax = m.row(0).clone(); + Mat ay = m.row(1).clone(); + + double mxx = mean(ax)[0]; + double myy = mean(ay)[0]; + + ax = ax - mxx; + ay = ay - myy; + + double scxx = mean(abs(ax))[0]; + double scyy = mean(abs(ay))[0]; + + Mat Hnorm (Matx33d( 1/scxx, 0.0, -mxx/scxx, + 0.0, 1/scyy, -myy/scyy, + 0.0, 0.0, 1.0 )); + + Mat inv_Hnorm (Matx33d( scxx, 0, mxx, + 0, scyy, myy, + 0, 0, 1 )); + Mat mn = Hnorm * m; + + Mat L = Mat::zeros(2*Np, 9, CV_64FC1); + + for (int i = 0; i < Np; ++i) + { + for (int j = 0; j < 3; j++) + { + L.at(2 * i, j) = M.at(j, i); + L.at(2 * i + 1, j + 3) = M.at(j, i); + L.at(2 * i, j + 6) = -mn.at(0,i) * M.at(j, i); + L.at(2 * i + 1, j + 6) = -mn.at(1,i) * M.at(j, i); + } + } + + if (Np > 4) L = L.t() * L; + SVD svd(L); + Mat hh = svd.vt.row(8) / svd.vt.row(8).at(8); + Mat Hrem = hh.reshape(1, 3); + Mat H = inv_Hnorm * Hrem; + + if (Np > 4) + { + Mat hhv = H.reshape(1, 9)(Rect(0, 0, 1, 8)).clone(); + for (int iter = 0; iter < 10; iter++) + { + Mat mrep = H * M; + Mat J = Mat::zeros(2 * Np, 8, CV_64FC1); + Mat MMM; + divide(M, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), MMM); + divide(mrep, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), mrep); + Mat m_err = m(Rect(0,0, m.cols, 2)) - mrep(Rect(0,0, mrep.cols, 2)); + m_err = Mat(m_err.t()).reshape(1, m_err.cols * m_err.rows); + Mat MMM2, MMM3; + multiply(Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 0, mrep.cols, 1)), MMM, MMM2); + multiply(Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 1, mrep.cols, 1)), MMM, MMM3); + + for (int i = 0; i < Np; ++i) + { + for (int j = 0; j < 3; ++j) + { + J.at(2 * i, j) = -MMM.at(j, i); + J.at(2 * i + 1, j + 3) = -MMM.at(j, i); + } + + for (int j = 0; j < 2; ++j) + { + J.at(2 * i, j + 6) = MMM2.at(j, i); + J.at(2 * i + 1, j + 6) = MMM3.at(j, i); + } + } + divide(M, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0,2,mrep.cols,1)), MMM); + Mat hh_innov = (J.t() * J).inv() * (J.t()) * m_err; + Mat hhv_up = hhv - hh_innov; + Mat tmp; + vconcat(hhv_up, Mat::ones(1,1,CV_64FC1), tmp); + Mat H_up = tmp.reshape(1,3); + hhv = hhv_up; + H = H_up; + } + } + return H; + } + + cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param) + { + CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2); + + Mat distorted((int)imagePoints.total(), 1, CV_64FC2), undistorted; + const Vec2d* ptr = imagePoints.ptr(0); + Vec2d* ptr_d = distorted.ptr(0); + for (size_t i = 0; i < imagePoints.total(); ++i) + { + ptr_d[i] = (ptr[i] - param.c).mul(Vec2d(1.0 / param.f[0], 1.0 / param.f[1])); + ptr_d[i][0] = ptr_d[i][0] - param.alpha * ptr_d[i][1]; + } + cv::fisheye::undistortPoints(distorted, undistorted, Matx33d::eye(), param.k); + return undistorted; + } + + void cv::internal::InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk) + { + + CV_Assert(!_objectPoints.empty() && _objectPoints.type() == CV_64FC3); + CV_Assert(!_imagePoints.empty() && _imagePoints.type() == CV_64FC2); + + Mat imagePointsNormalized = NormalizePixels(_imagePoints.t(), param).reshape(1).t(); + Mat objectPoints = Mat(_objectPoints.t()).reshape(1).t(); + Mat objectPointsMean, covObjectPoints; + Mat Rckk; + int Np = imagePointsNormalized.cols; - calcCovarMatrix(objectPoints, covObjectPoints, objectPointsMean, CV_COVAR_NORMAL | CV_COVAR_COLS); ++ calcCovarMatrix(objectPoints, covObjectPoints, objectPointsMean, COVAR_NORMAL | COVAR_COLS); + SVD svd(covObjectPoints); + Mat R(svd.vt); + if (norm(R(Rect(2, 0, 1, 2))) < 1e-6) + R = Mat::eye(3,3, CV_64FC1); + if (determinant(R) < 0) + R = -R; + Mat T = -R * objectPointsMean; + Mat X_new = R * objectPoints + T * Mat::ones(1, Np, CV_64FC1); + Mat H = ComputeHomography(imagePointsNormalized, X_new(Rect(0,0,X_new.cols,2))); + double sc = .5 * (norm(H.col(0)) + norm(H.col(1))); + H = H / sc; + Mat u1 = H.col(0).clone(); + u1 = u1 / norm(u1); + Mat u2 = H.col(1).clone() - u1.dot(H.col(1).clone()) * u1; + u2 = u2 / norm(u2); + Mat u3 = u1.cross(u2); + Mat RRR; + hconcat(u1, u2, RRR); + hconcat(RRR, u3, RRR); + Rodrigues(RRR, omckk); + Rodrigues(omckk, Rckk); + Tckk = H.col(2).clone(); + Tckk = Tckk + Rckk * T; + Rckk = Rckk * R; + Rodrigues(Rckk, omckk); + } + + void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + const IntrinsicParams& param, const int check_cond, + const double thresh_cond, InputOutputArray omc, InputOutputArray Tc) + { + CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3)); + CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2)); + CV_Assert(omc.type() == CV_64FC3 || Tc.type() == CV_64FC3); + + if (omc.empty()) omc.create(1, (int)objectPoints.total(), CV_64FC3); + if (Tc.empty()) Tc.create(1, (int)objectPoints.total(), CV_64FC3); + + const int maxIter = 20; + + for(int image_idx = 0; image_idx < (int)imagePoints.total(); ++image_idx) + { + Mat omckk, Tckk, JJ_kk; + Mat image, object; + + objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); + imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); + + InitExtrinsics(image, object, param, omckk, Tckk); + + ComputeExtrinsicRefine(image, object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond); + if (check_cond) + { + SVD svd(JJ_kk, SVD::NO_UV); + CV_Assert(svd.w.at(0) / svd.w.at((int)svd.w.total() - 1) < thresh_cond); + } + omckk.reshape(3,1).copyTo(omc.getMat().col(image_idx)); + Tckk.reshape(3,1).copyTo(Tc.getMat().col(image_idx)); + } + } + + + void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + const IntrinsicParams& param, InputArray omc, InputArray Tc, + const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3) + { + CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3)); + CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2)); + + CV_Assert(!omc.empty() && omc.type() == CV_64FC3); + CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3); + + int n = (int)objectPoints.total(); + + Mat JJ3 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1); + ex3 = Mat::zeros(9 + 6 * n, 1, CV_64FC1 ); + + for (int image_idx = 0; image_idx < n; ++image_idx) + { + Mat image, object; + objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); + imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); + + Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx)); + + std::vector x; + Mat jacobians; + projectPoints(object, x, om, T, param, jacobians); + Mat exkk = image.t() - Mat(x); + + Mat A(jacobians.rows, 9, CV_64FC1); + jacobians.colRange(0, 4).copyTo(A.colRange(0, 4)); + jacobians.col(14).copyTo(A.col(4)); + jacobians.colRange(4, 8).copyTo(A.colRange(5, 9)); + + A = A.t(); + + Mat B = jacobians.colRange(8, 14).clone(); + B = B.t(); + + JJ3(Rect(0, 0, 9, 9)) = JJ3(Rect(0, 0, 9, 9)) + A * A.t(); + JJ3(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t(); + + Mat AB = A * B.t(); + AB.copyTo(JJ3(Rect(9 + 6 * image_idx, 0, 6, 9))); + + JJ3(Rect(0, 9 + 6 * image_idx, 9, 6)) = AB.t(); + ex3(Rect(0,0,1,9)) = ex3(Rect(0,0,1,9)) + A * exkk.reshape(1, 2 * exkk.rows); + + ex3(Rect(0, 9 + 6 * image_idx, 1, 6)) = B * exkk.reshape(1, 2 * exkk.rows); + + if (check_cond) + { + Mat JJ_kk = B.t(); + SVD svd(JJ_kk, SVD::NO_UV); + CV_Assert(svd.w.at(0) / svd.w.at(svd.w.rows - 1) < thresh_cond); + } + } + - vector idxs(param.isEstimate); ++ std::vector idxs(param.isEstimate); + idxs.insert(idxs.end(), 6 * n, 1); + + subMatrix(JJ3, JJ3, idxs, idxs); + subMatrix(ex3, ex3, std::vector(1, 1), idxs); + JJ2_inv = JJ3.inv(); + } + + void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + const IntrinsicParams& params, InputArray omc, InputArray Tc, + IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms) + { + CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3)); + CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2)); + + CV_Assert(!omc.empty() && omc.type() == CV_64FC3); + CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3); + + Mat ex((int)(objectPoints.getMat(0).total() * objectPoints.total()), 1, CV_64FC2); + + for (int image_idx = 0; image_idx < (int)objectPoints.total(); ++image_idx) + { + Mat image, object; + objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); + imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); + + Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx)); + + std::vector x; + projectPoints(object, x, om, T, params, noArray()); + Mat ex_ = image.t() - Mat(x); + ex_.copyTo(ex.rowRange(ex_.rows * image_idx, ex_.rows * (image_idx + 1))); + } + + meanStdDev(ex, noArray(), std_err); + std_err *= sqrt((double)ex.total()/((double)ex.total() - 1.0)); + + Mat sigma_x; + meanStdDev(ex.reshape(1, 1), noArray(), sigma_x); + sigma_x *= sqrt(2.0 * (double)ex.total()/(2.0 * (double)ex.total() - 1.0)); + + Mat _JJ2_inv, ex3; + ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, _JJ2_inv, ex3); + + Mat_& JJ2_inv = (Mat_&)_JJ2_inv; + + sqrt(JJ2_inv, JJ2_inv); + + double s = sigma_x.at(0); + Mat r = 3 * s * JJ2_inv.diag(); + errors = r; + + rms = 0; + const Vec2d* ptr_ex = ex.ptr(); + for (size_t i = 0; i < ex.total(); i++) + { + rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1]; + } + + rms /= (double)ex.total(); + rms = sqrt(rms); + } + + void cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB) + { + CV_Assert(A.getMat().cols == B.getMat().rows); + CV_Assert(A.type() == CV_64FC1 && B.type() == CV_64FC1); + + int p = A.getMat().rows; + int n = A.getMat().cols; + int q = B.getMat().cols; + + dABdA.create(p * q, p * n, CV_64FC1); + dABdB.create(p * q, q * n, CV_64FC1); + + dABdA.getMat() = Mat::zeros(p * q, p * n, CV_64FC1); + dABdB.getMat() = Mat::zeros(p * q, q * n, CV_64FC1); + + for (int i = 0; i < q; ++i) + { + for (int j = 0; j < p; ++j) + { + int ij = j + i * p; + for (int k = 0; k < n; ++k) + { + int kj = j + k * p; + dABdA.getMat().at(ij, kj) = B.getMat().at(k, i); + } + } + } + + for (int i = 0; i < q; ++i) + { + A.getMat().copyTo(dABdB.getMat().rowRange(i * p, i * p + p).colRange(i * n, i * n + n)); + } + } + + void cv::internal::JRodriguesMatlab(const Mat& src, Mat& dst) + { + Mat tmp(src.cols, src.rows, src.type()); + if (src.rows == 9) + { + Mat(src.row(0).t()).copyTo(tmp.col(0)); + Mat(src.row(1).t()).copyTo(tmp.col(3)); + Mat(src.row(2).t()).copyTo(tmp.col(6)); + Mat(src.row(3).t()).copyTo(tmp.col(1)); + Mat(src.row(4).t()).copyTo(tmp.col(4)); + Mat(src.row(5).t()).copyTo(tmp.col(7)); + Mat(src.row(6).t()).copyTo(tmp.col(2)); + Mat(src.row(7).t()).copyTo(tmp.col(5)); + Mat(src.row(8).t()).copyTo(tmp.col(8)); + } + else + { + Mat(src.col(0).t()).copyTo(tmp.row(0)); + Mat(src.col(1).t()).copyTo(tmp.row(3)); + Mat(src.col(2).t()).copyTo(tmp.row(6)); + Mat(src.col(3).t()).copyTo(tmp.row(1)); + Mat(src.col(4).t()).copyTo(tmp.row(4)); + Mat(src.col(5).t()).copyTo(tmp.row(7)); + Mat(src.col(6).t()).copyTo(tmp.row(2)); + Mat(src.col(7).t()).copyTo(tmp.row(5)); + Mat(src.col(8).t()).copyTo(tmp.row(8)); + } + dst = tmp.clone(); + } + + void cv::internal::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, + Mat& om3, Mat& T3, Mat& dom3dom1, Mat& dom3dT1, Mat& dom3dom2, + Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2) + { + Mat om1 = _om1.getMat(); + Mat om2 = _om2.getMat(); + Mat T1 = _T1.getMat().reshape(1, 3); + Mat T2 = _T2.getMat().reshape(1, 3); + + //% Rotations: + Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2; + Rodrigues(om1, R1, dR1dom1); + Rodrigues(om2, R2, dR2dom2); + JRodriguesMatlab(dR1dom1, dR1dom1); + JRodriguesMatlab(dR2dom2, dR2dom2); + R3 = R2 * R1; + Mat dR3dR2, dR3dR1; + dAB(R2, R1, dR3dR2, dR3dR1); + Mat dom3dR3; + Rodrigues(R3, om3, dom3dR3); + JRodriguesMatlab(dom3dR3, dom3dR3); + dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1; + dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2; + dom3dT1 = Mat::zeros(3, 3, CV_64FC1); + dom3dT2 = Mat::zeros(3, 3, CV_64FC1); + + //% Translations: + Mat T3t = R2 * T1; + Mat dT3tdR2, dT3tdT1; + dAB(R2, T1, dT3tdR2, dT3tdT1); + Mat dT3tdom2 = dT3tdR2 * dR2dom2; + T3 = T3t + T2; + dT3dT1 = dT3tdT1; + dT3dT2 = Mat::eye(3, 3, CV_64FC1); + dT3dom2 = dT3tdom2; + dT3dom1 = Mat::zeros(3, 3, CV_64FC1); + } + + double cv::internal::median(const Mat& row) + { + CV_Assert(row.type() == CV_64FC1); + CV_Assert(!row.empty() && row.rows == 1); + Mat tmp = row.clone(); + sort(tmp, tmp, 0); + if ((int)tmp.total() % 2) return tmp.at((int)tmp.total() / 2); + else return 0.5 *(tmp.at((int)tmp.total() / 2) + tmp.at((int)tmp.total() / 2 - 1)); + } + + cv::Vec3d cv::internal::median3d(InputArray m) + { + CV_Assert(m.depth() == CV_64F && m.getMat().rows == 1); + Mat M = Mat(m.getMat().t()).reshape(1).t(); + return Vec3d(median(M.row(0)), median(M.row(1)), median(M.row(2))); + } diff --cc modules/calib3d/test/test_fisheye.cpp index 0000000,1ea47ab..8318f82 mode 000000,100644..100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@@ -1,0 -1,613 +1,613 @@@ + /*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. + // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and/or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + + #include "test_precomp.hpp" -#include ++#include + #include "../src/fisheye.hpp" + + class fisheyeTest : public ::testing::Test { + + protected: + const static cv::Size imageSize; + const static cv::Matx33d K; + const static cv::Vec4d D; + const static cv::Matx33d R; + const static cv::Vec3d T; + std::string datasets_repository_path; + + virtual void SetUp() { - datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cameracalibration/fisheye"); ++ datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye"); + } + + protected: + std::string combine(const std::string& _item1, const std::string& _item2); + cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r); + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// TESTS:: + + TEST_F(fisheyeTest, projectPoints) + { + double cols = this->imageSize.width, + rows = this->imageSize.height; + + const int N = 20; + cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2; + undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3)); + cv::Vec2d* pts = distorted0.ptr(); + + cv::Vec2d c(this->K(0, 2), this->K(1, 2)); + for(int y = 0, k = 0; y < N; ++y) + for(int x = 0; x < N; ++x) + { + cv::Vec2d point(x*cols/(N-1.f), y*rows/(N-1.f)); + pts[k++] = (point - c) * 0.85 + c; + } + + cv::fisheye::undistortPoints(distorted0, undist1, this->K, this->D); + + cv::Vec2d* u1 = undist1.ptr(); + cv::Vec3d* u2 = undist2.ptr(); + for(int i = 0; i < (int)distorted0.total(); ++i) + u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0); + + cv::fisheye::distortPoints(undist1, distorted1, this->K, this->D); + cv::fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D); + + EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10); + EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10); + } + + TEST_F(fisheyeTest, undistortImage) + { + cv::Matx33d K = this->K; + cv::Mat D = cv::Mat(this->D); + std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg"); + cv::Matx33d newK = K; + cv::Mat distorted = cv::imread(file), undistorted; + { + newK(0, 0) = 100; + newK(1, 1) = 100; + cv::fisheye::undistortImage(distorted, undistorted, K, D, newK); + cv::Mat correct = cv::imread(combine(datasets_repository_path, "new_f_100.png")); + if (correct.empty()) + CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_f_100.png"), undistorted)); + else + EXPECT_MAT_NEAR(correct, undistorted, 1e-10); + } + { + double balance = 1.0; + cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance); + cv::fisheye::undistortImage(distorted, undistorted, K, D, newK); + cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_1.0.png")); + if (correct.empty()) + CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_1.0.png"), undistorted)); + else + EXPECT_MAT_NEAR(correct, undistorted, 1e-10); + } + + { + double balance = 0.0; + cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance); + cv::fisheye::undistortImage(distorted, undistorted, K, D, newK); + cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_0.0.png")); + if (correct.empty()) + CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_0.0.png"), undistorted)); + else + EXPECT_MAT_NEAR(correct, undistorted, 1e-10); + } + } + + TEST_F(fisheyeTest, jacobians) + { + int n = 10; + cv::Mat X(1, n, CV_64FC3); + cv::Mat om(3, 1, CV_64F), T(3, 1, CV_64F); + cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F); + cv::Mat k(4, 1, CV_64F); + double alpha; + + cv::RNG r; + + r.fill(X, cv::RNG::NORMAL, 2, 1); + X = cv::abs(X) * 10; + + r.fill(om, cv::RNG::NORMAL, 0, 1); + om = cv::abs(om); + + r.fill(T, cv::RNG::NORMAL, 0, 1); + T = cv::abs(T); T.at(2) = 4; T *= 10; + + r.fill(f, cv::RNG::NORMAL, 0, 1); + f = cv::abs(f) * 1000; + + r.fill(c, cv::RNG::NORMAL, 0, 1); + c = cv::abs(c) * 1000; + + r.fill(k, cv::RNG::NORMAL, 0, 1); + k*= 0.5; + + alpha = 0.01*r.gaussian(1); + + cv::Mat x1, x2, xpred; + cv::Matx33d K(f.at(0), alpha * f.at(0), c.at(0), + 0, f.at(1), c.at(1), + 0, 0, 1); + + cv::Mat jacobians; + cv::fisheye::projectPoints(X, x1, om, T, K, k, alpha, jacobians); + + //test on T: + cv::Mat dT(3, 1, CV_64FC1); + r.fill(dT, cv::RNG::NORMAL, 0, 1); + dT *= 1e-9*cv::norm(T); + cv::Mat T2 = T + dT; + cv::fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray()); + xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); + + //test on om: + cv::Mat dom(3, 1, CV_64FC1); + r.fill(dom, cv::RNG::NORMAL, 0, 1); + dom *= 1e-9*cv::norm(om); + cv::Mat om2 = om + dom; + cv::fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray()); + xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); + + //test on f: + cv::Mat df(2, 1, CV_64FC1); + r.fill(df, cv::RNG::NORMAL, 0, 1); + df *= 1e-9*cv::norm(f); + cv::Matx33d K2 = K + cv::Matx33d(df.at(0), df.at(0) * alpha, 0, 0, df.at(1), 0, 0, 0, 0); + cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray()); + xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); + + //test on c: + cv::Mat dc(2, 1, CV_64FC1); + r.fill(dc, cv::RNG::NORMAL, 0, 1); + dc *= 1e-9*cv::norm(c); + K2 = K + cv::Matx33d(0, 0, dc.at(0), 0, 0, dc.at(1), 0, 0, 0); + cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray()); + xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); + + //test on k: + cv::Mat dk(4, 1, CV_64FC1); + r.fill(dk, cv::RNG::NORMAL, 0, 1); + dk *= 1e-9*cv::norm(k); + cv::Mat k2 = k + dk; + cv::fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray()); + xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); + + //test on alpha: + cv::Mat dalpha(1, 1, CV_64FC1); + r.fill(dalpha, cv::RNG::NORMAL, 0, 1); + dalpha *= 1e-9*cv::norm(f); + double alpha2 = alpha + dalpha.at(0); + K2 = K + cv::Matx33d(0, f.at(0) * dalpha.at(0), 0, 0, 0, 0, 0, 0, 0); + cv::fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray()); + xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); + } + + TEST_F(fisheyeTest, Calibration) + { + const int n_images = 34; + + std::vector > imagePoints(n_images); + std::vector > objectPoints(n_images); + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> imagePoints[i]; + fs_left.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + + cv::Matx33d K; + cv::Vec4d D; + + cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D, + cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6)); + + EXPECT_MAT_NEAR(K, this->K, 1e-10); + EXPECT_MAT_NEAR(D, this->D, 1e-10); + } + + TEST_F(fisheyeTest, Homography) + { + const int n_images = 1; + + std::vector > imagePoints(n_images); + std::vector > objectPoints(n_images); + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> imagePoints[i]; + fs_left.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + cv::internal::IntrinsicParams param; + param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI), + cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5)); + + cv::Mat _imagePoints (imagePoints[0]); + cv::Mat _objectPoints(objectPoints[0]); + + cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t(); + _objectPoints = _objectPoints.reshape(1).t(); + cv::Mat objectPointsMean, covObjectPoints; + + int Np = imagePointsNormalized.cols; - cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, CV_COVAR_NORMAL | CV_COVAR_COLS); ++ cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS); + cv::SVD svd(covObjectPoints); + cv::Mat R(svd.vt); + + if (cv::norm(R(cv::Rect(2, 0, 1, 2))) < 1e-6) + R = cv::Mat::eye(3,3, CV_64FC1); + if (cv::determinant(R) < 0) + R = -R; + + cv::Mat T = -R * objectPointsMean; + cv::Mat X_new = R * _objectPoints + T * cv::Mat::ones(1, Np, CV_64FC1); + cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2)); + + cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1); + X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2)); + cv::Mat mrep = H * M; + + cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep); + + cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t(); + + cv::Vec2d std_err; + cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err); + std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1)); + + cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901); + EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12); + } + + TEST_F(fisheyeTest, EtimateUncertainties) + { + const int n_images = 34; + + std::vector > imagePoints(n_images); + std::vector > objectPoints(n_images); + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> imagePoints[i]; + fs_left.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + + cv::Matx33d K; + cv::Vec4d D; + std::vector rvec; + std::vector tvec; + + cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D, + rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6)); + + cv::internal::IntrinsicParams param, errors; + cv::Vec2d err_std; + double thresh_cond = 1e6; + int check_cond = 1; + param.Init(cv::Vec2d(K(0,0), K(1,1)), cv::Vec2d(K(0,2), K(1, 2)), D); + param.isEstimate = std::vector(9, 1); + param.isEstimate[4] = 0; + + errors.isEstimate = param.isEstimate; + + double rms; + + cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec, + errors, err_std, thresh_cond, check_cond, rms); + + EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-10); + EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10); + EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10); + EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10); + CV_Assert(abs(rms - 0.263782587133546) < 1e-10); + CV_Assert(errors.alpha == 0); + } + + TEST_F(fisheyeTest, rectify) + { + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + cv::Size calibration_size = this->imageSize, requested_size = calibration_size; + cv::Matx33d K1 = this->K, K2 = K1; + cv::Mat D1 = cv::Mat(this->D), D2 = D1; + + cv::Vec3d T = this->T; + cv::Matx33d R = this->R; + + double balance = 0.0, fov_scale = 1.1; + cv::Mat R1, R2, P1, P2, Q; + cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, R, T, R1, R2, P1, P2, Q, + cv::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale); + + cv::Mat lmapx, lmapy, rmapx, rmapy; + //rewrite for fisheye + cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy); + cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy); + + cv::Mat l, r, lundist, rundist; + cv::VideoCapture lcap(combine(folder, "left/stereo_pair_%03d.jpg")), + rcap(combine(folder, "right/stereo_pair_%03d.jpg")); + + for(int i = 0;; ++i) + { + lcap >> l; rcap >> r; + if (l.empty() || r.empty()) + break; + + int ndisp = 128; - cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), CV_RGB(255, 0, 0)); - cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), CV_RGB(255, 0, 0)); - cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), CV_RGB(255, 0, 0)); ++ cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255)); ++ cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255)); ++ cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), cv::Scalar(0, 0, 255)); + cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR); + cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR); + + cv::Mat rectification = mergeRectification(lundist, rundist); + + cv::Mat correct = cv::imread(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i))); + + if (correct.empty()) + cv::imwrite(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i)), rectification); + else + EXPECT_MAT_NEAR(correct, rectification, 1e-10); + } + } + + TEST_F(fisheyeTest, stereoCalibrate) + { + const int n_images = 34; + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + cv::Matx33d K1, K2, R; + cv::Vec3d T; + cv::Vec4d D1, D2; + + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + // flag |= cv::fisheye::CALIB_FIX_INTRINSIC; + + cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, R, T, flag, + cv::TermCriteria(3, 12, 0)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); + cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); + cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412, + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); + + cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); + + cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); + cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); + + EXPECT_MAT_NEAR(R, R_correct, 1e-10); + EXPECT_MAT_NEAR(T, T_correct, 1e-10); + + EXPECT_MAT_NEAR(K1, K1_correct, 1e-10); + EXPECT_MAT_NEAR(K2, K2_correct, 1e-10); + + EXPECT_MAT_NEAR(D1, D1_correct, 1e-10); + EXPECT_MAT_NEAR(D2, D2_correct, 1e-10); + + } + + TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic) + { + const int n_images = 34; + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + cv::Matx33d R; + cv::Vec3d T; + + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + flag |= cv::fisheye::CALIB_FIX_INTRINSIC; + + cv::Matx33d K1 (561.195925927249, 0, 621.282400272412, + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); + + cv::Matx33d K2 (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); + + cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); + cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); + + cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, R, T, flag, + cv::TermCriteria(3, 12, 0)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); + cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); + + + EXPECT_MAT_NEAR(R, R_correct, 1e-10); + EXPECT_MAT_NEAR(T, T_correct, 1e-10); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// fisheyeTest:: + + const cv::Size fisheyeTest::imageSize(1280, 800); + + const cv::Matx33d fisheyeTest::K(558.478087865323, 0, 620.458515360843, + 0, 560.506767351568, 381.939424848348, + 0, 0, 1); + + const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371); + + const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03, + -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02, + -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01); + + const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04); + + std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2) + { + std::string item1 = _item1, item2 = _item2; + std::replace(item1.begin(), item1.end(), '\\', '/'); + std::replace(item2.begin(), item2.end(), '\\', '/'); + + if (item1.empty()) + return item2; + + if (item2.empty()) + return item1; + + char last = item1[item1.size()-1]; + return item1 + (last != '/' ? "/" : "") + item2; + } + + cv::Mat fisheyeTest::mergeRectification(const cv::Mat& l, const cv::Mat& r) + { + CV_Assert(l.type() == r.type() && l.size() == r.size()); + cv::Mat merged(l.rows, l.cols * 2, l.type()); + cv::Mat lpart = merged.colRange(0, l.cols); + cv::Mat rpart = merged.colRange(l.cols, merged.cols); + l.copyTo(lpart); + r.copyTo(rpart); + + for(int i = 0; i < l.rows; i+=20) - cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), CV_RGB(0, 255, 0)); ++ cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), cv::Scalar(0, 255, 0)); + + return merged; + } diff --cc modules/core/include/opencv2/core/mat.inl.hpp index d463eec,0000000..5f09d4f mode 100644,000000..100644 --- a/modules/core/include/opencv2/core/mat.inl.hpp +++ b/modules/core/include/opencv2/core/mat.inl.hpp @@@ -1,3377 -1,0 +1,3378 @@@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#ifndef __OPENCV_CORE_MATRIX_OPERATIONS_HPP__ +#define __OPENCV_CORE_MATRIX_OPERATIONS_HPP__ + +#ifndef __cplusplus +# error mat.inl.hpp header must be compiled as C++ +#endif + +namespace cv +{ + +//////////////////////// Input/Output Arrays //////////////////////// + +inline void _InputArray::init(int _flags, const void* _obj) +{ flags = _flags; obj = (void*)_obj; } + +inline void _InputArray::init(int _flags, const void* _obj, Size _sz) +{ flags = _flags; obj = (void*)_obj; sz = _sz; } + +inline void* _InputArray::getObj() const { return obj; } + +inline _InputArray::_InputArray() { init(NONE, 0); } +inline _InputArray::_InputArray(int _flags, void* _obj) { init(_flags, _obj); } +inline _InputArray::_InputArray(const Mat& m) { init(MAT+ACCESS_READ, &m); } +inline _InputArray::_InputArray(const std::vector& vec) { init(STD_VECTOR_MAT+ACCESS_READ, &vec); } +inline _InputArray::_InputArray(const UMat& m) { init(UMAT+ACCESS_READ, &m); } +inline _InputArray::_InputArray(const std::vector& vec) { init(STD_VECTOR_UMAT+ACCESS_READ, &vec); } + +template inline +_InputArray::_InputArray(const std::vector<_Tp>& vec) +{ init(FIXED_TYPE + STD_VECTOR + DataType<_Tp>::type + ACCESS_READ, &vec); } + +template inline +_InputArray::_InputArray(const std::vector >& vec) +{ init(FIXED_TYPE + STD_VECTOR_VECTOR + DataType<_Tp>::type + ACCESS_READ, &vec); } + +template inline +_InputArray::_InputArray(const std::vector >& vec) +{ init(FIXED_TYPE + STD_VECTOR_MAT + DataType<_Tp>::type + ACCESS_READ, &vec); } + +template inline +_InputArray::_InputArray(const Matx<_Tp, m, n>& mtx) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_READ, &mtx, Size(n, m)); } + +template inline +_InputArray::_InputArray(const _Tp* vec, int n) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_READ, vec, Size(n, 1)); } + +template inline +_InputArray::_InputArray(const Mat_<_Tp>& m) +{ init(FIXED_TYPE + MAT + DataType<_Tp>::type + ACCESS_READ, &m); } + +inline _InputArray::_InputArray(const double& val) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + CV_64F + ACCESS_READ, &val, Size(1,1)); } + +inline _InputArray::_InputArray(const MatExpr& expr) +{ init(FIXED_TYPE + FIXED_SIZE + EXPR + ACCESS_READ, &expr); } + +inline _InputArray::_InputArray(const cuda::GpuMat& d_mat) +{ init(GPU_MAT + ACCESS_READ, &d_mat); } + +inline _InputArray::_InputArray(const ogl::Buffer& buf) +{ init(OPENGL_BUFFER + ACCESS_READ, &buf); } + +inline _InputArray::_InputArray(const cuda::CudaMem& cuda_mem) +{ init(CUDA_MEM + ACCESS_READ, &cuda_mem); } + +inline _InputArray::~_InputArray() {} + +inline bool _InputArray::isMat() const { return kind() == _InputArray::MAT; } +inline bool _InputArray::isUMat() const { return kind() == _InputArray::UMAT; } +inline bool _InputArray::isMatVector() const { return kind() == _InputArray::STD_VECTOR_MAT; } +inline bool _InputArray::isUMatVector() const { return kind() == _InputArray::STD_VECTOR_UMAT; } +inline bool _InputArray::isMatx() const { return kind() == _InputArray::MATX; } + +//////////////////////////////////////////////////////////////////////////////////////// + +inline _OutputArray::_OutputArray() { init(ACCESS_WRITE, 0); } +inline _OutputArray::_OutputArray(int _flags, void* _obj) { init(_flags|ACCESS_WRITE, _obj); } +inline _OutputArray::_OutputArray(Mat& m) { init(MAT+ACCESS_WRITE, &m); } +inline _OutputArray::_OutputArray(std::vector& vec) { init(STD_VECTOR_MAT+ACCESS_WRITE, &vec); } +inline _OutputArray::_OutputArray(UMat& m) { init(UMAT+ACCESS_WRITE, &m); } +inline _OutputArray::_OutputArray(std::vector& vec) { init(STD_VECTOR_UMAT+ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(std::vector<_Tp>& vec) +{ init(FIXED_TYPE + STD_VECTOR + DataType<_Tp>::type + ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(std::vector >& vec) +{ init(FIXED_TYPE + STD_VECTOR_VECTOR + DataType<_Tp>::type + ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(std::vector >& vec) +{ init(FIXED_TYPE + STD_VECTOR_MAT + DataType<_Tp>::type + ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(Mat_<_Tp>& m) +{ init(FIXED_TYPE + MAT + DataType<_Tp>::type + ACCESS_WRITE, &m); } + +template inline +_OutputArray::_OutputArray(Matx<_Tp, m, n>& mtx) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_WRITE, &mtx, Size(n, m)); } + +template inline +_OutputArray::_OutputArray(_Tp* vec, int n) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_WRITE, vec, Size(n, 1)); } + +template inline +_OutputArray::_OutputArray(const std::vector<_Tp>& vec) +{ init(FIXED_TYPE + FIXED_SIZE + STD_VECTOR + DataType<_Tp>::type + ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(const std::vector >& vec) +{ init(FIXED_TYPE + FIXED_SIZE + STD_VECTOR_VECTOR + DataType<_Tp>::type + ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(const std::vector >& vec) +{ init(FIXED_TYPE + FIXED_SIZE + STD_VECTOR_MAT + DataType<_Tp>::type + ACCESS_WRITE, &vec); } + +template inline +_OutputArray::_OutputArray(const Mat_<_Tp>& m) +{ init(FIXED_TYPE + FIXED_SIZE + MAT + DataType<_Tp>::type + ACCESS_WRITE, &m); } + +template inline +_OutputArray::_OutputArray(const Matx<_Tp, m, n>& mtx) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_WRITE, &mtx, Size(n, m)); } + +template inline +_OutputArray::_OutputArray(const _Tp* vec, int n) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_WRITE, vec, Size(n, 1)); } + +inline _OutputArray::_OutputArray(cuda::GpuMat& d_mat) +{ init(GPU_MAT + ACCESS_WRITE, &d_mat); } + +inline _OutputArray::_OutputArray(ogl::Buffer& buf) +{ init(OPENGL_BUFFER + ACCESS_WRITE, &buf); } + +inline _OutputArray::_OutputArray(cuda::CudaMem& cuda_mem) +{ init(CUDA_MEM + ACCESS_WRITE, &cuda_mem); } + +inline _OutputArray::_OutputArray(const Mat& m) +{ init(FIXED_TYPE + FIXED_SIZE + MAT + ACCESS_WRITE, &m); } + +inline _OutputArray::_OutputArray(const std::vector& vec) +{ init(FIXED_SIZE + STD_VECTOR_MAT + ACCESS_WRITE, &vec); } + +inline _OutputArray::_OutputArray(const UMat& m) +{ init(FIXED_TYPE + FIXED_SIZE + UMAT + ACCESS_WRITE, &m); } + +inline _OutputArray::_OutputArray(const std::vector& vec) +{ init(FIXED_SIZE + STD_VECTOR_UMAT + ACCESS_WRITE, &vec); } + +inline _OutputArray::_OutputArray(const cuda::GpuMat& d_mat) +{ init(FIXED_TYPE + FIXED_SIZE + GPU_MAT + ACCESS_WRITE, &d_mat); } + +inline _OutputArray::_OutputArray(const ogl::Buffer& buf) +{ init(FIXED_TYPE + FIXED_SIZE + OPENGL_BUFFER + ACCESS_WRITE, &buf); } + +inline _OutputArray::_OutputArray(const cuda::CudaMem& cuda_mem) +{ init(FIXED_TYPE + FIXED_SIZE + CUDA_MEM + ACCESS_WRITE, &cuda_mem); } + +/////////////////////////////////////////////////////////////////////////////////////////// + +inline _InputOutputArray::_InputOutputArray() { init(ACCESS_RW, 0); } +inline _InputOutputArray::_InputOutputArray(int _flags, void* _obj) { init(_flags|ACCESS_RW, _obj); } +inline _InputOutputArray::_InputOutputArray(Mat& m) { init(MAT+ACCESS_RW, &m); } +inline _InputOutputArray::_InputOutputArray(std::vector& vec) { init(STD_VECTOR_MAT+ACCESS_RW, &vec); } +inline _InputOutputArray::_InputOutputArray(UMat& m) { init(UMAT+ACCESS_RW, &m); } +inline _InputOutputArray::_InputOutputArray(std::vector& vec) { init(STD_VECTOR_UMAT+ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(std::vector<_Tp>& vec) +{ init(FIXED_TYPE + STD_VECTOR + DataType<_Tp>::type + ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(std::vector >& vec) +{ init(FIXED_TYPE + STD_VECTOR_VECTOR + DataType<_Tp>::type + ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(std::vector >& vec) +{ init(FIXED_TYPE + STD_VECTOR_MAT + DataType<_Tp>::type + ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(Mat_<_Tp>& m) +{ init(FIXED_TYPE + MAT + DataType<_Tp>::type + ACCESS_RW, &m); } + +template inline +_InputOutputArray::_InputOutputArray(Matx<_Tp, m, n>& mtx) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_RW, &mtx, Size(n, m)); } + +template inline +_InputOutputArray::_InputOutputArray(_Tp* vec, int n) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_RW, vec, Size(n, 1)); } + +template inline +_InputOutputArray::_InputOutputArray(const std::vector<_Tp>& vec) +{ init(FIXED_TYPE + FIXED_SIZE + STD_VECTOR + DataType<_Tp>::type + ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(const std::vector >& vec) +{ init(FIXED_TYPE + FIXED_SIZE + STD_VECTOR_VECTOR + DataType<_Tp>::type + ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(const std::vector >& vec) +{ init(FIXED_TYPE + FIXED_SIZE + STD_VECTOR_MAT + DataType<_Tp>::type + ACCESS_RW, &vec); } + +template inline +_InputOutputArray::_InputOutputArray(const Mat_<_Tp>& m) +{ init(FIXED_TYPE + FIXED_SIZE + MAT + DataType<_Tp>::type + ACCESS_RW, &m); } + +template inline +_InputOutputArray::_InputOutputArray(const Matx<_Tp, m, n>& mtx) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_RW, &mtx, Size(n, m)); } + +template inline +_InputOutputArray::_InputOutputArray(const _Tp* vec, int n) +{ init(FIXED_TYPE + FIXED_SIZE + MATX + DataType<_Tp>::type + ACCESS_RW, vec, Size(n, 1)); } + +inline _InputOutputArray::_InputOutputArray(cuda::GpuMat& d_mat) +{ init(GPU_MAT + ACCESS_RW, &d_mat); } + +inline _InputOutputArray::_InputOutputArray(ogl::Buffer& buf) +{ init(OPENGL_BUFFER + ACCESS_RW, &buf); } + +inline _InputOutputArray::_InputOutputArray(cuda::CudaMem& cuda_mem) +{ init(CUDA_MEM + ACCESS_RW, &cuda_mem); } + +inline _InputOutputArray::_InputOutputArray(const Mat& m) +{ init(FIXED_TYPE + FIXED_SIZE + MAT + ACCESS_RW, &m); } + +inline _InputOutputArray::_InputOutputArray(const std::vector& vec) +{ init(FIXED_SIZE + STD_VECTOR_MAT + ACCESS_RW, &vec); } + +inline _InputOutputArray::_InputOutputArray(const UMat& m) +{ init(FIXED_TYPE + FIXED_SIZE + UMAT + ACCESS_RW, &m); } + +inline _InputOutputArray::_InputOutputArray(const std::vector& vec) +{ init(FIXED_SIZE + STD_VECTOR_UMAT + ACCESS_RW, &vec); } + +inline _InputOutputArray::_InputOutputArray(const cuda::GpuMat& d_mat) +{ init(FIXED_TYPE + FIXED_SIZE + GPU_MAT + ACCESS_RW, &d_mat); } + +inline _InputOutputArray::_InputOutputArray(const ogl::Buffer& buf) +{ init(FIXED_TYPE + FIXED_SIZE + OPENGL_BUFFER + ACCESS_RW, &buf); } + +inline _InputOutputArray::_InputOutputArray(const cuda::CudaMem& cuda_mem) +{ init(FIXED_TYPE + FIXED_SIZE + CUDA_MEM + ACCESS_RW, &cuda_mem); } + +//////////////////////////////////////////// Mat ////////////////////////////////////////// + +inline +Mat::Mat() + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{} + +inline +Mat::Mat(int _rows, int _cols, int _type) + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{ + create(_rows, _cols, _type); +} + +inline +Mat::Mat(int _rows, int _cols, int _type, const Scalar& _s) + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{ + create(_rows, _cols, _type); + *this = _s; +} + +inline +Mat::Mat(Size _sz, int _type) + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{ + create( _sz.height, _sz.width, _type ); +} + +inline +Mat::Mat(Size _sz, int _type, const Scalar& _s) + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{ + create(_sz.height, _sz.width, _type); + *this = _s; +} + +inline +Mat::Mat(int _dims, const int* _sz, int _type) + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{ + create(_dims, _sz, _type); +} + +inline +Mat::Mat(int _dims, const int* _sz, int _type, const Scalar& _s) + : flags(MAGIC_VAL), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), + datalimit(0), allocator(0), u(0), size(&rows) +{ + create(_dims, _sz, _type); + *this = _s; +} + +inline +Mat::Mat(const Mat& m) + : flags(m.flags), dims(m.dims), rows(m.rows), cols(m.cols), data(m.data), + datastart(m.datastart), dataend(m.dataend), datalimit(m.datalimit), allocator(m.allocator), + u(m.u), size(&rows) +{ + if( u ) + CV_XADD(&u->refcount, 1); + if( m.dims <= 2 ) + { + step[0] = m.step[0]; step[1] = m.step[1]; + } + else + { + dims = 0; + copySize(m); + } +} + +inline +Mat::Mat(int _rows, int _cols, int _type, void* _data, size_t _step) + : flags(MAGIC_VAL + (_type & TYPE_MASK)), dims(2), rows(_rows), cols(_cols), + data((uchar*)_data), datastart((uchar*)_data), dataend(0), datalimit(0), + allocator(0), u(0), size(&rows) +{ + size_t esz = CV_ELEM_SIZE(_type), esz1 = CV_ELEM_SIZE1(_type); + size_t minstep = cols * esz; + if( _step == AUTO_STEP ) + { + _step = minstep; + flags |= CONTINUOUS_FLAG; + } + else + { + if( rows == 1 ) _step = minstep; + CV_DbgAssert( _step >= minstep ); + + if (_step % esz1 != 0) + { + CV_Error(Error::BadStep, "Step must be a multiple of esz1"); + } + + flags |= _step == minstep ? CONTINUOUS_FLAG : 0; + } + step[0] = _step; + step[1] = esz; + datalimit = datastart + _step * rows; + dataend = datalimit - _step + minstep; +} + +inline +Mat::Mat(Size _sz, int _type, void* _data, size_t _step) + : flags(MAGIC_VAL + (_type & TYPE_MASK)), dims(2), rows(_sz.height), cols(_sz.width), + data((uchar*)_data), datastart((uchar*)_data), dataend(0), datalimit(0), + allocator(0), u(0), size(&rows) +{ + size_t esz = CV_ELEM_SIZE(_type), esz1 = CV_ELEM_SIZE1(_type); + size_t minstep = cols*esz; + if( _step == AUTO_STEP ) + { + _step = minstep; + flags |= CONTINUOUS_FLAG; + } + else + { + if( rows == 1 ) _step = minstep; + CV_DbgAssert( _step >= minstep ); + + if (_step % esz1 != 0) + { + CV_Error(Error::BadStep, "Step must be a multiple of esz1"); + } + + flags |= _step == minstep ? CONTINUOUS_FLAG : 0; + } + step[0] = _step; + step[1] = esz; + datalimit = datastart + _step*rows; + dataend = datalimit - _step + minstep; +} + +template inline +Mat::Mat(const std::vector<_Tp>& vec, bool copyData) + : flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(2), rows((int)vec.size()), + cols(1), data(0), datastart(0), dataend(0), allocator(0), u(0), size(&rows) +{ + if(vec.empty()) + return; + if( !copyData ) + { + step[0] = step[1] = sizeof(_Tp); + data = datastart = (uchar*)&vec[0]; + datalimit = dataend = datastart + rows * step[0]; + } + else + Mat((int)vec.size(), 1, DataType<_Tp>::type, (uchar*)&vec[0]).copyTo(*this); +} + +template inline +Mat::Mat(const Vec<_Tp, n>& vec, bool copyData) + : flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(2), rows(n), cols(1), data(0), + datastart(0), dataend(0), allocator(0), u(0), size(&rows) +{ + if( !copyData ) + { + step[0] = step[1] = sizeof(_Tp); + data = datastart = (uchar*)vec.val; + datalimit = dataend = datastart + rows * step[0]; + } + else + Mat(n, 1, DataType<_Tp>::type, (void*)vec.val).copyTo(*this); +} + + +template inline +Mat::Mat(const Matx<_Tp,m,n>& M, bool copyData) + : flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(2), rows(m), cols(n), data(0), + datastart(0), dataend(0), allocator(0), u(0), size(&rows) +{ + if( !copyData ) + { + step[0] = cols * sizeof(_Tp); + step[1] = sizeof(_Tp); + data = datastart = (uchar*)M.val; + datalimit = dataend = datastart + rows * step[0]; + } + else + Mat(m, n, DataType<_Tp>::type, (uchar*)M.val).copyTo(*this); +} + +template inline +Mat::Mat(const Point_<_Tp>& pt, bool copyData) + : flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(2), rows(2), cols(1), data(0), + datastart(0), dataend(0), allocator(0), u(0), size(&rows) +{ + if( !copyData ) + { + step[0] = step[1] = sizeof(_Tp); + data = datastart = (uchar*)&pt.x; + datalimit = dataend = datastart + rows * step[0]; + } + else + { + create(2, 1, DataType<_Tp>::type); + ((_Tp*)data)[0] = pt.x; + ((_Tp*)data)[1] = pt.y; + } +} + +template inline +Mat::Mat(const Point3_<_Tp>& pt, bool copyData) + : flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(2), rows(3), cols(1), data(0), + datastart(0), dataend(0), allocator(0), u(0), size(&rows) +{ + if( !copyData ) + { + step[0] = step[1] = sizeof(_Tp); + data = datastart = (uchar*)&pt.x; + datalimit = dataend = datastart + rows * step[0]; + } + else + { + create(3, 1, DataType<_Tp>::type); + ((_Tp*)data)[0] = pt.x; + ((_Tp*)data)[1] = pt.y; + ((_Tp*)data)[2] = pt.z; + } +} + +template inline +Mat::Mat(const MatCommaInitializer_<_Tp>& commaInitializer) + : flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(0), rows(0), cols(0), data(0), + datastart(0), dataend(0), allocator(0), u(0), size(&rows) +{ + *this = commaInitializer.operator Mat_<_Tp>(); +} + +inline +Mat::~Mat() +{ + release(); + if( step.p != step.buf ) + fastFree(step.p); +} + +inline +Mat& Mat::operator = (const Mat& m) +{ + if( this != &m ) + { + if( m.u ) + CV_XADD(&m.u->refcount, 1); + release(); + flags = m.flags; + if( dims <= 2 && m.dims <= 2 ) + { + dims = m.dims; + rows = m.rows; + cols = m.cols; + step[0] = m.step[0]; + step[1] = m.step[1]; + } + else + copySize(m); + data = m.data; + datastart = m.datastart; + dataend = m.dataend; + datalimit = m.datalimit; + allocator = m.allocator; + u = m.u; + } + return *this; +} + +inline +Mat Mat::row(int y) const +{ + return Mat(*this, Range(y, y + 1), Range::all()); +} + +inline +Mat Mat::col(int x) const +{ + return Mat(*this, Range::all(), Range(x, x + 1)); +} + +inline +Mat Mat::rowRange(int startrow, int endrow) const +{ + return Mat(*this, Range(startrow, endrow), Range::all()); +} + +inline +Mat Mat::rowRange(const Range& r) const +{ + return Mat(*this, r, Range::all()); +} + +inline +Mat Mat::colRange(int startcol, int endcol) const +{ + return Mat(*this, Range::all(), Range(startcol, endcol)); +} + +inline +Mat Mat::colRange(const Range& r) const +{ + return Mat(*this, Range::all(), r); +} + +inline +Mat Mat::clone() const +{ + Mat m; + copyTo(m); + return m; +} + +inline +void Mat::assignTo( Mat& m, int _type ) const +{ + if( _type < 0 ) + m = *this; + else + convertTo(m, _type); +} + +inline +void Mat::create(int _rows, int _cols, int _type) +{ + _type &= TYPE_MASK; + if( dims <= 2 && rows == _rows && cols == _cols && type() == _type && data ) + return; + int sz[] = {_rows, _cols}; + create(2, sz, _type); +} + +inline +void Mat::create(Size _sz, int _type) +{ + create(_sz.height, _sz.width, _type); +} + +inline +void Mat::addref() +{ + if( u ) + CV_XADD(&u->refcount, 1); +} + +inline void Mat::release() +{ + if( u && CV_XADD(&u->refcount, -1) == 1 ) + deallocate(); + u = NULL; + data = datastart = dataend = datalimit = 0; - size.p[0] = 0; ++ for(int i = 0; i < dims; i++) ++ size.p[i] = 0; +} + +inline +Mat Mat::operator()( Range _rowRange, Range _colRange ) const +{ + return Mat(*this, _rowRange, _colRange); +} + +inline +Mat Mat::operator()( const Rect& roi ) const +{ + return Mat(*this, roi); +} + +inline +Mat Mat::operator()(const Range* ranges) const +{ + return Mat(*this, ranges); +} + +inline +bool Mat::isContinuous() const +{ + return (flags & CONTINUOUS_FLAG) != 0; +} + +inline +bool Mat::isSubmatrix() const +{ + return (flags & SUBMATRIX_FLAG) != 0; +} + +inline +size_t Mat::elemSize() const +{ + return dims > 0 ? step.p[dims - 1] : 0; +} + +inline +size_t Mat::elemSize1() const +{ + return CV_ELEM_SIZE1(flags); +} + +inline +int Mat::type() const +{ + return CV_MAT_TYPE(flags); +} + +inline +int Mat::depth() const +{ + return CV_MAT_DEPTH(flags); +} + +inline +int Mat::channels() const +{ + return CV_MAT_CN(flags); +} + +inline +size_t Mat::step1(int i) const +{ + return step.p[i] / elemSize1(); +} + +inline +bool Mat::empty() const +{ + return data == 0 || total() == 0; +} + +inline +size_t Mat::total() const +{ + if( dims <= 2 ) + return (size_t)rows * cols; + size_t p = 1; + for( int i = 0; i < dims; i++ ) + p *= size[i]; + return p; +} + +inline +uchar* Mat::ptr(int y) +{ + CV_DbgAssert( y == 0 || (data && dims >= 1 && (unsigned)y < (unsigned)size.p[0]) ); + return data + step.p[0] * y; +} + +inline +const uchar* Mat::ptr(int y) const +{ + CV_DbgAssert( y == 0 || (data && dims >= 1 && (unsigned)y < (unsigned)size.p[0]) ); + return data + step.p[0] * y; +} + +template inline +_Tp* Mat::ptr(int y) +{ + CV_DbgAssert( y == 0 || (data && dims >= 1 && (unsigned)y < (unsigned)size.p[0]) ); + return (_Tp*)(data + step.p[0] * y); +} + +template inline +const _Tp* Mat::ptr(int y) const +{ + CV_DbgAssert( y == 0 || (data && dims >= 1 && data && (unsigned)y < (unsigned)size.p[0]) ); + return (const _Tp*)(data + step.p[0] * y); +} + +inline +uchar* Mat::ptr(int i0, int i1) +{ + CV_DbgAssert( dims >= 2 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] ); + return data + i0 * step.p[0] + i1 * step.p[1]; +} + +inline +const uchar* Mat::ptr(int i0, int i1) const +{ + CV_DbgAssert( dims >= 2 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] ); + return data + i0 * step.p[0] + i1 * step.p[1]; +} + +template inline +_Tp* Mat::ptr(int i0, int i1) +{ + CV_DbgAssert( dims >= 2 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] ); + return (_Tp*)(data + i0 * step.p[0] + i1 * step.p[1]); +} + +template inline +const _Tp* Mat::ptr(int i0, int i1) const +{ + CV_DbgAssert( dims >= 2 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] ); + return (const _Tp*)(data + i0 * step.p[0] + i1 * step.p[1]); +} + +inline +uchar* Mat::ptr(int i0, int i1, int i2) +{ + CV_DbgAssert( dims >= 3 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] && + (unsigned)i2 < (unsigned)size.p[2] ); + return data + i0 * step.p[0] + i1 * step.p[1] + i2 * step.p[2]; +} + +inline +const uchar* Mat::ptr(int i0, int i1, int i2) const +{ + CV_DbgAssert( dims >= 3 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] && + (unsigned)i2 < (unsigned)size.p[2] ); + return data + i0 * step.p[0] + i1 * step.p[1] + i2 * step.p[2]; +} + +template inline +_Tp* Mat::ptr(int i0, int i1, int i2) +{ + CV_DbgAssert( dims >= 3 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] && + (unsigned)i2 < (unsigned)size.p[2] ); + return (_Tp*)(data + i0 * step.p[0] + i1 * step.p[1] + i2 * step.p[2]); +} + +template inline +const _Tp* Mat::ptr(int i0, int i1, int i2) const +{ + CV_DbgAssert( dims >= 3 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] && + (unsigned)i2 < (unsigned)size.p[2] ); + return (const _Tp*)(data + i0 * step.p[0] + i1 * step.p[1] + i2 * step.p[2]); +} + +inline +uchar* Mat::ptr(const int* idx) +{ + int i, d = dims; + uchar* p = data; + CV_DbgAssert( d >= 1 && p ); + for( i = 0; i < d; i++ ) + { + CV_DbgAssert( (unsigned)idx[i] < (unsigned)size.p[i] ); + p += idx[i] * step.p[i]; + } + return p; +} + +inline +const uchar* Mat::ptr(const int* idx) const +{ + int i, d = dims; + uchar* p = data; + CV_DbgAssert( d >= 1 && p ); + for( i = 0; i < d; i++ ) + { + CV_DbgAssert( (unsigned)idx[i] < (unsigned)size.p[i] ); + p += idx[i] * step.p[i]; + } + return p; +} + +template inline +_Tp& Mat::at(int i0, int i1) +{ + CV_DbgAssert( dims <= 2 && data && (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)(i1 * DataType<_Tp>::channels) < (unsigned)(size.p[1] * channels()) && + CV_ELEM_SIZE1(DataType<_Tp>::depth) == elemSize1()); + return ((_Tp*)(data + step.p[0] * i0))[i1]; +} + +template inline +const _Tp& Mat::at(int i0, int i1) const +{ + CV_DbgAssert( dims <= 2 && data && (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)(i1 * DataType<_Tp>::channels) < (unsigned)(size.p[1] * channels()) && + CV_ELEM_SIZE1(DataType<_Tp>::depth) == elemSize1()); + return ((const _Tp*)(data + step.p[0] * i0))[i1]; +} + +template inline +_Tp& Mat::at(Point pt) +{ + CV_DbgAssert( dims <= 2 && data && (unsigned)pt.y < (unsigned)size.p[0] && + (unsigned)(pt.x * DataType<_Tp>::channels) < (unsigned)(size.p[1] * channels()) && + CV_ELEM_SIZE1(DataType<_Tp>::depth) == elemSize1()); + return ((_Tp*)(data + step.p[0] * pt.y))[pt.x]; +} + +template inline +const _Tp& Mat::at(Point pt) const +{ + CV_DbgAssert( dims <= 2 && data && (unsigned)pt.y < (unsigned)size.p[0] && + (unsigned)(pt.x * DataType<_Tp>::channels) < (unsigned)(size.p[1] * channels()) && + CV_ELEM_SIZE1(DataType<_Tp>::depth) == elemSize1()); + return ((const _Tp*)(data + step.p[0] * pt.y))[pt.x]; +} + +template inline +_Tp& Mat::at(int i0) +{ + CV_DbgAssert( dims <= 2 && data && + (unsigned)i0 < (unsigned)(size.p[0] * size.p[1]) && + elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + if( isContinuous() || size.p[0] == 1 ) + return ((_Tp*)data)[i0]; + if( size.p[1] == 1 ) + return *(_Tp*)(data + step.p[0] * i0); + int i = i0 / cols, j = i0 - i * cols; + return ((_Tp*)(data + step.p[0] * i))[j]; +} + +template inline +const _Tp& Mat::at(int i0) const +{ + CV_DbgAssert( dims <= 2 && data && + (unsigned)i0 < (unsigned)(size.p[0] * size.p[1]) && + elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + if( isContinuous() || size.p[0] == 1 ) + return ((const _Tp*)data)[i0]; + if( size.p[1] == 1 ) + return *(const _Tp*)(data + step.p[0] * i0); + int i = i0 / cols, j = i0 - i * cols; + return ((const _Tp*)(data + step.p[0] * i))[j]; +} + +template inline +_Tp& Mat::at(int i0, int i1, int i2) +{ + CV_DbgAssert( elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + return *(_Tp*)ptr(i0, i1, i2); +} + +template inline +const _Tp& Mat::at(int i0, int i1, int i2) const +{ + CV_DbgAssert( elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + return *(const _Tp*)ptr(i0, i1, i2); +} + +template inline +_Tp& Mat::at(const int* idx) +{ + CV_DbgAssert( elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + return *(_Tp*)ptr(idx); +} + +template inline +const _Tp& Mat::at(const int* idx) const +{ + CV_DbgAssert( elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + return *(const _Tp*)ptr(idx); +} + +template inline +_Tp& Mat::at(const Vec& idx) +{ + CV_DbgAssert( elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + return *(_Tp*)ptr(idx.val); +} + +template inline +const _Tp& Mat::at(const Vec& idx) const +{ + CV_DbgAssert( elemSize() == CV_ELEM_SIZE(DataType<_Tp>::type) ); + return *(const _Tp*)ptr(idx.val); +} + +template inline +MatConstIterator_<_Tp> Mat::begin() const +{ + CV_DbgAssert( elemSize() == sizeof(_Tp) ); + return MatConstIterator_<_Tp>((const Mat_<_Tp>*)this); +} + +template inline +MatConstIterator_<_Tp> Mat::end() const +{ + CV_DbgAssert( elemSize() == sizeof(_Tp) ); + MatConstIterator_<_Tp> it((const Mat_<_Tp>*)this); + it += total(); + return it; +} + +template inline +MatIterator_<_Tp> Mat::begin() +{ + CV_DbgAssert( elemSize() == sizeof(_Tp) ); + return MatIterator_<_Tp>((Mat_<_Tp>*)this); +} + +template inline +MatIterator_<_Tp> Mat::end() +{ + CV_DbgAssert( elemSize() == sizeof(_Tp) ); + MatIterator_<_Tp> it((Mat_<_Tp>*)this); + it += total(); + return it; +} + +template inline +Mat::operator std::vector<_Tp>() const +{ + std::vector<_Tp> v; + copyTo(v); + return v; +} + +template inline +Mat::operator Vec<_Tp, n>() const +{ + CV_Assert( data && dims <= 2 && (rows == 1 || cols == 1) && + rows + cols - 1 == n && channels() == 1 ); + + if( isContinuous() && type() == DataType<_Tp>::type ) + return Vec<_Tp, n>((_Tp*)data); + Vec<_Tp, n> v; + Mat tmp(rows, cols, DataType<_Tp>::type, v.val); + convertTo(tmp, tmp.type()); + return v; +} + +template inline +Mat::operator Matx<_Tp, m, n>() const +{ + CV_Assert( data && dims <= 2 && rows == m && cols == n && channels() == 1 ); + + if( isContinuous() && type() == DataType<_Tp>::type ) + return Matx<_Tp, m, n>((_Tp*)data); + Matx<_Tp, m, n> mtx; + Mat tmp(rows, cols, DataType<_Tp>::type, mtx.val); + convertTo(tmp, tmp.type()); + return mtx; +} + +template inline +void Mat::push_back(const _Tp& elem) +{ + if( !data ) + { + *this = Mat(1, 1, DataType<_Tp>::type, (void*)&elem).clone(); + return; + } + CV_Assert(DataType<_Tp>::type == type() && cols == 1 + /* && dims == 2 (cols == 1 implies dims == 2) */); + uchar* tmp = dataend + step[0]; + if( !isSubmatrix() && isContinuous() && tmp <= datalimit ) + { + *(_Tp*)(data + (size.p[0]++) * step.p[0]) = elem; + dataend = tmp; + } + else + push_back_(&elem); +} + +template inline +void Mat::push_back(const Mat_<_Tp>& m) +{ + push_back((const Mat&)m); +} + +///////////////////////////// MatSize //////////////////////////// + +inline +MatSize::MatSize(int* _p) + : p(_p) {} + +inline +Size MatSize::operator()() const +{ + CV_DbgAssert(p[-1] <= 2); + return Size(p[1], p[0]); +} + +inline +const int& MatSize::operator[](int i) const +{ + return p[i]; +} + +inline +int& MatSize::operator[](int i) +{ + return p[i]; +} + +inline +MatSize::operator const int*() const +{ + return p; +} + +inline +bool MatSize::operator == (const MatSize& sz) const +{ + int d = p[-1]; + int dsz = sz.p[-1]; + if( d != dsz ) + return false; + if( d == 2 ) + return p[0] == sz.p[0] && p[1] == sz.p[1]; + + for( int i = 0; i < d; i++ ) + if( p[i] != sz.p[i] ) + return false; + return true; +} + +inline +bool MatSize::operator != (const MatSize& sz) const +{ + return !(*this == sz); +} + + + +///////////////////////////// MatStep //////////////////////////// + +inline +MatStep::MatStep() +{ + p = buf; p[0] = p[1] = 0; +} + +inline +MatStep::MatStep(size_t s) +{ + p = buf; p[0] = s; p[1] = 0; +} + +inline +const size_t& MatStep::operator[](int i) const +{ + return p[i]; +} + +inline +size_t& MatStep::operator[](int i) +{ + return p[i]; +} + +inline MatStep::operator size_t() const +{ + CV_DbgAssert( p == buf ); + return buf[0]; +} + +inline MatStep& MatStep::operator = (size_t s) +{ + CV_DbgAssert( p == buf ); + buf[0] = s; + return *this; +} + + + +////////////////////////////// Mat_<_Tp> //////////////////////////// + +template inline +Mat_<_Tp>::Mat_() + : Mat() +{ + flags = (flags & ~CV_MAT_TYPE_MASK) | DataType<_Tp>::type; +} + +template inline +Mat_<_Tp>::Mat_(int _rows, int _cols) + : Mat(_rows, _cols, DataType<_Tp>::type) +{ +} + +template inline +Mat_<_Tp>::Mat_(int _rows, int _cols, const _Tp& value) + : Mat(_rows, _cols, DataType<_Tp>::type) +{ + *this = value; +} + +template inline +Mat_<_Tp>::Mat_(Size _sz) + : Mat(_sz.height, _sz.width, DataType<_Tp>::type) +{} + +template inline +Mat_<_Tp>::Mat_(Size _sz, const _Tp& value) + : Mat(_sz.height, _sz.width, DataType<_Tp>::type) +{ + *this = value; +} + +template inline +Mat_<_Tp>::Mat_(int _dims, const int* _sz) + : Mat(_dims, _sz, DataType<_Tp>::type) +{} + +template inline +Mat_<_Tp>::Mat_(int _dims, const int* _sz, const _Tp& _s) + : Mat(_dims, _sz, DataType<_Tp>::type, Scalar(_s)) +{} + +template inline +Mat_<_Tp>::Mat_(const Mat_<_Tp>& m, const Range* ranges) + : Mat(m, ranges) +{} + +template inline +Mat_<_Tp>::Mat_(const Mat& m) + : Mat() +{ + flags = (flags & ~CV_MAT_TYPE_MASK) | DataType<_Tp>::type; + *this = m; +} + +template inline +Mat_<_Tp>::Mat_(const Mat_& m) + : Mat(m) +{} + +template inline +Mat_<_Tp>::Mat_(int _rows, int _cols, _Tp* _data, size_t steps) + : Mat(_rows, _cols, DataType<_Tp>::type, _data, steps) +{} + +template inline +Mat_<_Tp>::Mat_(const Mat_& m, const Range& _rowRange, const Range& _colRange) + : Mat(m, _rowRange, _colRange) +{} + +template inline +Mat_<_Tp>::Mat_(const Mat_& m, const Rect& roi) + : Mat(m, roi) +{} + +template template inline +Mat_<_Tp>::Mat_(const Vec::channel_type, n>& vec, bool copyData) + : Mat(n / DataType<_Tp>::channels, 1, DataType<_Tp>::type, (void*)&vec) +{ + CV_Assert(n%DataType<_Tp>::channels == 0); + if( copyData ) + *this = clone(); +} + +template template inline +Mat_<_Tp>::Mat_(const Matx::channel_type, m, n>& M, bool copyData) + : Mat(m, n / DataType<_Tp>::channels, DataType<_Tp>::type, (void*)&M) +{ + CV_Assert(n % DataType<_Tp>::channels == 0); + if( copyData ) + *this = clone(); +} + +template inline +Mat_<_Tp>::Mat_(const Point_::channel_type>& pt, bool copyData) + : Mat(2 / DataType<_Tp>::channels, 1, DataType<_Tp>::type, (void*)&pt) +{ + CV_Assert(2 % DataType<_Tp>::channels == 0); + if( copyData ) + *this = clone(); +} + +template inline +Mat_<_Tp>::Mat_(const Point3_::channel_type>& pt, bool copyData) + : Mat(3 / DataType<_Tp>::channels, 1, DataType<_Tp>::type, (void*)&pt) +{ + CV_Assert(3 % DataType<_Tp>::channels == 0); + if( copyData ) + *this = clone(); +} + +template inline +Mat_<_Tp>::Mat_(const MatCommaInitializer_<_Tp>& commaInitializer) + : Mat(commaInitializer) +{} + +template inline +Mat_<_Tp>::Mat_(const std::vector<_Tp>& vec, bool copyData) + : Mat(vec, copyData) +{} + +template inline +Mat_<_Tp>& Mat_<_Tp>::operator = (const Mat& m) +{ + if( DataType<_Tp>::type == m.type() ) + { + Mat::operator = (m); + return *this; + } + if( DataType<_Tp>::depth == m.depth() ) + { + return (*this = m.reshape(DataType<_Tp>::channels, m.dims, 0)); + } + CV_DbgAssert(DataType<_Tp>::channels == m.channels()); + m.convertTo(*this, type()); + return *this; +} + +template inline +Mat_<_Tp>& Mat_<_Tp>::operator = (const Mat_& m) +{ + Mat::operator=(m); + return *this; +} + +template inline +Mat_<_Tp>& Mat_<_Tp>::operator = (const _Tp& s) +{ + typedef typename DataType<_Tp>::vec_type VT; + Mat::operator=(Scalar((const VT&)s)); + return *this; +} + +template inline +void Mat_<_Tp>::create(int _rows, int _cols) +{ + Mat::create(_rows, _cols, DataType<_Tp>::type); +} + +template inline +void Mat_<_Tp>::create(Size _sz) +{ + Mat::create(_sz, DataType<_Tp>::type); +} + +template inline +void Mat_<_Tp>::create(int _dims, const int* _sz) +{ + Mat::create(_dims, _sz, DataType<_Tp>::type); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::cross(const Mat_& m) const +{ + return Mat_<_Tp>(Mat::cross(m)); +} + +template template inline +Mat_<_Tp>::operator Mat_() const +{ + return Mat_(*this); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::row(int y) const +{ + return Mat_(*this, Range(y, y+1), Range::all()); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::col(int x) const +{ + return Mat_(*this, Range::all(), Range(x, x+1)); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::diag(int d) const +{ + return Mat_(Mat::diag(d)); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::clone() const +{ + return Mat_(Mat::clone()); +} + +template inline +size_t Mat_<_Tp>::elemSize() const +{ + CV_DbgAssert( Mat::elemSize() == sizeof(_Tp) ); + return sizeof(_Tp); +} + +template inline +size_t Mat_<_Tp>::elemSize1() const +{ + CV_DbgAssert( Mat::elemSize1() == sizeof(_Tp) / DataType<_Tp>::channels ); + return sizeof(_Tp) / DataType<_Tp>::channels; +} + +template inline +int Mat_<_Tp>::type() const +{ + CV_DbgAssert( Mat::type() == DataType<_Tp>::type ); + return DataType<_Tp>::type; +} + +template inline +int Mat_<_Tp>::depth() const +{ + CV_DbgAssert( Mat::depth() == DataType<_Tp>::depth ); + return DataType<_Tp>::depth; +} + +template inline +int Mat_<_Tp>::channels() const +{ + CV_DbgAssert( Mat::channels() == DataType<_Tp>::channels ); + return DataType<_Tp>::channels; +} + +template inline +size_t Mat_<_Tp>::stepT(int i) const +{ + return step.p[i] / elemSize(); +} + +template inline +size_t Mat_<_Tp>::step1(int i) const +{ + return step.p[i] / elemSize1(); +} + +template inline +Mat_<_Tp>& Mat_<_Tp>::adjustROI( int dtop, int dbottom, int dleft, int dright ) +{ + return (Mat_<_Tp>&)(Mat::adjustROI(dtop, dbottom, dleft, dright)); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::operator()( const Range& _rowRange, const Range& _colRange ) const +{ + return Mat_<_Tp>(*this, _rowRange, _colRange); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::operator()( const Rect& roi ) const +{ + return Mat_<_Tp>(*this, roi); +} + +template inline +Mat_<_Tp> Mat_<_Tp>::operator()( const Range* ranges ) const +{ + return Mat_<_Tp>(*this, ranges); +} + +template inline +_Tp* Mat_<_Tp>::operator [](int y) +{ + return (_Tp*)ptr(y); +} + +template inline +const _Tp* Mat_<_Tp>::operator [](int y) const +{ + return (const _Tp*)ptr(y); +} + +template inline +_Tp& Mat_<_Tp>::operator ()(int i0, int i1) +{ + CV_DbgAssert( dims <= 2 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] && + type() == DataType<_Tp>::type ); + return ((_Tp*)(data + step.p[0] * i0))[i1]; +} + +template inline +const _Tp& Mat_<_Tp>::operator ()(int i0, int i1) const +{ + CV_DbgAssert( dims <= 2 && data && + (unsigned)i0 < (unsigned)size.p[0] && + (unsigned)i1 < (unsigned)size.p[1] && + type() == DataType<_Tp>::type ); + return ((const _Tp*)(data + step.p[0] * i0))[i1]; +} + +template inline +_Tp& Mat_<_Tp>::operator ()(Point pt) +{ + CV_DbgAssert( dims <= 2 && data && + (unsigned)pt.y < (unsigned)size.p[0] && + (unsigned)pt.x < (unsigned)size.p[1] && + type() == DataType<_Tp>::type ); + return ((_Tp*)(data + step.p[0] * pt.y))[pt.x]; +} + +template inline +const _Tp& Mat_<_Tp>::operator ()(Point pt) const +{ + CV_DbgAssert( dims <= 2 && data && + (unsigned)pt.y < (unsigned)size.p[0] && + (unsigned)pt.x < (unsigned)size.p[1] && + type() == DataType<_Tp>::type ); + return ((const _Tp*)(data + step.p[0] * pt.y))[pt.x]; +} + +template inline +_Tp& Mat_<_Tp>::operator ()(const int* idx) +{ + return Mat::at<_Tp>(idx); +} + +template inline +const _Tp& Mat_<_Tp>::operator ()(const int* idx) const +{ + return Mat::at<_Tp>(idx); +} + +template template inline +_Tp& Mat_<_Tp>::operator ()(const Vec& idx) +{ + return Mat::at<_Tp>(idx); +} + +template template inline +const _Tp& Mat_<_Tp>::operator ()(const Vec& idx) const +{ + return Mat::at<_Tp>(idx); +} + +template inline +_Tp& Mat_<_Tp>::operator ()(int i0) +{ + return this->at<_Tp>(i0); +} + +template inline +const _Tp& Mat_<_Tp>::operator ()(int i0) const +{ + return this->at<_Tp>(i0); +} + +template inline +_Tp& Mat_<_Tp>::operator ()(int i0, int i1, int i2) +{ + return this->at<_Tp>(i0, i1, i2); +} + +template inline +const _Tp& Mat_<_Tp>::operator ()(int i0, int i1, int i2) const +{ + return this->at<_Tp>(i0, i1, i2); +} + +template inline +Mat_<_Tp>::operator std::vector<_Tp>() const +{ + std::vector<_Tp> v; + copyTo(v); + return v; +} + +template template inline +Mat_<_Tp>::operator Vec::channel_type, n>() const +{ + CV_Assert(n % DataType<_Tp>::channels == 0); + return this->Mat::operator Vec::channel_type, n>(); +} + +template template inline +Mat_<_Tp>::operator Matx::channel_type, m, n>() const +{ + CV_Assert(n % DataType<_Tp>::channels == 0); + + Matx::channel_type, m, n> res = this->Mat::operator Matx::channel_type, m, n>(); + return res; +} + +template inline +MatConstIterator_<_Tp> Mat_<_Tp>::begin() const +{ + return Mat::begin<_Tp>(); +} + +template inline +MatConstIterator_<_Tp> Mat_<_Tp>::end() const +{ + return Mat::end<_Tp>(); +} + +template inline +MatIterator_<_Tp> Mat_<_Tp>::begin() +{ + return Mat::begin<_Tp>(); +} + +template inline +MatIterator_<_Tp> Mat_<_Tp>::end() +{ + return Mat::end<_Tp>(); +} + + +///////////////////////////// SparseMat ///////////////////////////// + +inline +SparseMat::SparseMat() + : flags(MAGIC_VAL), hdr(0) +{} + +inline +SparseMat::SparseMat(int _dims, const int* _sizes, int _type) + : flags(MAGIC_VAL), hdr(0) +{ + create(_dims, _sizes, _type); +} + +inline +SparseMat::SparseMat(const SparseMat& m) + : flags(m.flags), hdr(m.hdr) +{ + addref(); +} + +inline +SparseMat::~SparseMat() +{ + release(); +} + +inline +SparseMat& SparseMat::operator = (const SparseMat& m) +{ + if( this != &m ) + { + if( m.hdr ) + CV_XADD(&m.hdr->refcount, 1); + release(); + flags = m.flags; + hdr = m.hdr; + } + return *this; +} + +inline +SparseMat& SparseMat::operator = (const Mat& m) +{ + return (*this = SparseMat(m)); +} + +inline +SparseMat SparseMat::clone() const +{ + SparseMat temp; + this->copyTo(temp); + return temp; +} + +inline +void SparseMat::assignTo( SparseMat& m, int _type ) const +{ + if( _type < 0 ) + m = *this; + else + convertTo(m, _type); +} + +inline +void SparseMat::addref() +{ + if( hdr ) + CV_XADD(&hdr->refcount, 1); +} + +inline +void SparseMat::release() +{ + if( hdr && CV_XADD(&hdr->refcount, -1) == 1 ) + delete hdr; + hdr = 0; +} + +inline +size_t SparseMat::elemSize() const +{ + return CV_ELEM_SIZE(flags); +} + +inline +size_t SparseMat::elemSize1() const +{ + return CV_ELEM_SIZE1(flags); +} + +inline +int SparseMat::type() const +{ + return CV_MAT_TYPE(flags); +} + +inline +int SparseMat::depth() const +{ + return CV_MAT_DEPTH(flags); +} + +inline +int SparseMat::channels() const +{ + return CV_MAT_CN(flags); +} + +inline +const int* SparseMat::size() const +{ + return hdr ? hdr->size : 0; +} + +inline +int SparseMat::size(int i) const +{ + if( hdr ) + { + CV_DbgAssert((unsigned)i < (unsigned)hdr->dims); + return hdr->size[i]; + } + return 0; +} + +inline +int SparseMat::dims() const +{ + return hdr ? hdr->dims : 0; +} + +inline +size_t SparseMat::nzcount() const +{ + return hdr ? hdr->nodeCount : 0; +} + +inline +size_t SparseMat::hash(int i0) const +{ + return (size_t)i0; +} + +inline +size_t SparseMat::hash(int i0, int i1) const +{ + return (size_t)(unsigned)i0 * HASH_SCALE + (unsigned)i1; +} + +inline +size_t SparseMat::hash(int i0, int i1, int i2) const +{ + return ((size_t)(unsigned)i0 * HASH_SCALE + (unsigned)i1) * HASH_SCALE + (unsigned)i2; +} + +inline +size_t SparseMat::hash(const int* idx) const +{ + size_t h = (unsigned)idx[0]; + if( !hdr ) + return 0; + int d = hdr->dims; + for(int i = 1; i < d; i++ ) + h = h * HASH_SCALE + (unsigned)idx[i]; + return h; +} + +template inline +_Tp& SparseMat::ref(int i0, size_t* hashval) +{ + return *(_Tp*)((SparseMat*)this)->ptr(i0, true, hashval); +} + +template inline +_Tp& SparseMat::ref(int i0, int i1, size_t* hashval) +{ + return *(_Tp*)((SparseMat*)this)->ptr(i0, i1, true, hashval); +} + +template inline +_Tp& SparseMat::ref(int i0, int i1, int i2, size_t* hashval) +{ + return *(_Tp*)((SparseMat*)this)->ptr(i0, i1, i2, true, hashval); +} + +template inline +_Tp& SparseMat::ref(const int* idx, size_t* hashval) +{ + return *(_Tp*)((SparseMat*)this)->ptr(idx, true, hashval); +} + +template inline +_Tp SparseMat::value(int i0, size_t* hashval) const +{ + const _Tp* p = (const _Tp*)((SparseMat*)this)->ptr(i0, false, hashval); + return p ? *p : _Tp(); +} + +template inline +_Tp SparseMat::value(int i0, int i1, size_t* hashval) const +{ + const _Tp* p = (const _Tp*)((SparseMat*)this)->ptr(i0, i1, false, hashval); + return p ? *p : _Tp(); +} + +template inline +_Tp SparseMat::value(int i0, int i1, int i2, size_t* hashval) const +{ + const _Tp* p = (const _Tp*)((SparseMat*)this)->ptr(i0, i1, i2, false, hashval); + return p ? *p : _Tp(); +} + +template inline +_Tp SparseMat::value(const int* idx, size_t* hashval) const +{ + const _Tp* p = (const _Tp*)((SparseMat*)this)->ptr(idx, false, hashval); + return p ? *p : _Tp(); +} + +template inline +const _Tp* SparseMat::find(int i0, size_t* hashval) const +{ + return (const _Tp*)((SparseMat*)this)->ptr(i0, false, hashval); +} + +template inline +const _Tp* SparseMat::find(int i0, int i1, size_t* hashval) const +{ + return (const _Tp*)((SparseMat*)this)->ptr(i0, i1, false, hashval); +} + +template inline +const _Tp* SparseMat::find(int i0, int i1, int i2, size_t* hashval) const +{ + return (const _Tp*)((SparseMat*)this)->ptr(i0, i1, i2, false, hashval); +} + +template inline +const _Tp* SparseMat::find(const int* idx, size_t* hashval) const +{ + return (const _Tp*)((SparseMat*)this)->ptr(idx, false, hashval); +} + +template inline +_Tp& SparseMat::value(Node* n) +{ + return *(_Tp*)((uchar*)n + hdr->valueOffset); +} + +template inline +const _Tp& SparseMat::value(const Node* n) const +{ + return *(const _Tp*)((const uchar*)n + hdr->valueOffset); +} + +inline +SparseMat::Node* SparseMat::node(size_t nidx) +{ + return (Node*)(void*)&hdr->pool[nidx]; +} + +inline +const SparseMat::Node* SparseMat::node(size_t nidx) const +{ + return (const Node*)(const void*)&hdr->pool[nidx]; +} + +inline +SparseMatIterator SparseMat::begin() +{ + return SparseMatIterator(this); +} + +inline +SparseMatConstIterator SparseMat::begin() const +{ + return SparseMatConstIterator(this); +} + +inline +SparseMatIterator SparseMat::end() +{ + SparseMatIterator it(this); + it.seekEnd(); + return it; +} + +inline +SparseMatConstIterator SparseMat::end() const +{ + SparseMatConstIterator it(this); + it.seekEnd(); + return it; +} + +template inline +SparseMatIterator_<_Tp> SparseMat::begin() +{ + return SparseMatIterator_<_Tp>(this); +} + +template inline +SparseMatConstIterator_<_Tp> SparseMat::begin() const +{ + return SparseMatConstIterator_<_Tp>(this); +} + +template inline +SparseMatIterator_<_Tp> SparseMat::end() +{ + SparseMatIterator_<_Tp> it(this); + it.seekEnd(); + return it; +} + +template inline +SparseMatConstIterator_<_Tp> SparseMat::end() const +{ + SparseMatConstIterator_<_Tp> it(this); + it.seekEnd(); + return it; +} + + + +///////////////////////////// SparseMat_ //////////////////////////// + +template inline +SparseMat_<_Tp>::SparseMat_() +{ + flags = MAGIC_VAL | DataType<_Tp>::type; +} + +template inline +SparseMat_<_Tp>::SparseMat_(int _dims, const int* _sizes) + : SparseMat(_dims, _sizes, DataType<_Tp>::type) +{} + +template inline +SparseMat_<_Tp>::SparseMat_(const SparseMat& m) +{ + if( m.type() == DataType<_Tp>::type ) + *this = (const SparseMat_<_Tp>&)m; + else + m.convertTo(*this, DataType<_Tp>::type); +} + +template inline +SparseMat_<_Tp>::SparseMat_(const SparseMat_<_Tp>& m) +{ + this->flags = m.flags; + this->hdr = m.hdr; + if( this->hdr ) + CV_XADD(&this->hdr->refcount, 1); +} + +template inline +SparseMat_<_Tp>::SparseMat_(const Mat& m) +{ + SparseMat sm(m); + *this = sm; +} + +template inline +SparseMat_<_Tp>& SparseMat_<_Tp>::operator = (const SparseMat_<_Tp>& m) +{ + if( this != &m ) + { + if( m.hdr ) CV_XADD(&m.hdr->refcount, 1); + release(); + flags = m.flags; + hdr = m.hdr; + } + return *this; +} + +template inline +SparseMat_<_Tp>& SparseMat_<_Tp>::operator = (const SparseMat& m) +{ + if( m.type() == DataType<_Tp>::type ) + return (*this = (const SparseMat_<_Tp>&)m); + m.convertTo(*this, DataType<_Tp>::type); + return *this; +} + +template inline +SparseMat_<_Tp>& SparseMat_<_Tp>::operator = (const Mat& m) +{ + return (*this = SparseMat(m)); +} + +template inline +SparseMat_<_Tp> SparseMat_<_Tp>::clone() const +{ + SparseMat_<_Tp> m; + this->copyTo(m); + return m; +} + +template inline +void SparseMat_<_Tp>::create(int _dims, const int* _sizes) +{ + SparseMat::create(_dims, _sizes, DataType<_Tp>::type); +} + +template inline +int SparseMat_<_Tp>::type() const +{ + return DataType<_Tp>::type; +} + +template inline +int SparseMat_<_Tp>::depth() const +{ + return DataType<_Tp>::depth; +} + +template inline +int SparseMat_<_Tp>::channels() const +{ + return DataType<_Tp>::channels; +} + +template inline +_Tp& SparseMat_<_Tp>::ref(int i0, size_t* hashval) +{ + return SparseMat::ref<_Tp>(i0, hashval); +} + +template inline +_Tp SparseMat_<_Tp>::operator()(int i0, size_t* hashval) const +{ + return SparseMat::value<_Tp>(i0, hashval); +} + +template inline +_Tp& SparseMat_<_Tp>::ref(int i0, int i1, size_t* hashval) +{ + return SparseMat::ref<_Tp>(i0, i1, hashval); +} + +template inline +_Tp SparseMat_<_Tp>::operator()(int i0, int i1, size_t* hashval) const +{ + return SparseMat::value<_Tp>(i0, i1, hashval); +} + +template inline +_Tp& SparseMat_<_Tp>::ref(int i0, int i1, int i2, size_t* hashval) +{ + return SparseMat::ref<_Tp>(i0, i1, i2, hashval); +} + +template inline +_Tp SparseMat_<_Tp>::operator()(int i0, int i1, int i2, size_t* hashval) const +{ + return SparseMat::value<_Tp>(i0, i1, i2, hashval); +} + +template inline +_Tp& SparseMat_<_Tp>::ref(const int* idx, size_t* hashval) +{ + return SparseMat::ref<_Tp>(idx, hashval); +} + +template inline +_Tp SparseMat_<_Tp>::operator()(const int* idx, size_t* hashval) const +{ + return SparseMat::value<_Tp>(idx, hashval); +} + +template inline +SparseMatIterator_<_Tp> SparseMat_<_Tp>::begin() +{ + return SparseMatIterator_<_Tp>(this); +} + +template inline +SparseMatConstIterator_<_Tp> SparseMat_<_Tp>::begin() const +{ + return SparseMatConstIterator_<_Tp>(this); +} + +template inline +SparseMatIterator_<_Tp> SparseMat_<_Tp>::end() +{ + SparseMatIterator_<_Tp> it(this); + it.seekEnd(); + return it; +} + +template inline +SparseMatConstIterator_<_Tp> SparseMat_<_Tp>::end() const +{ + SparseMatConstIterator_<_Tp> it(this); + it.seekEnd(); + return it; +} + + + +////////////////////////// MatConstIterator ///////////////////////// + +inline +MatConstIterator::MatConstIterator() + : m(0), elemSize(0), ptr(0), sliceStart(0), sliceEnd(0) +{} + +inline +MatConstIterator::MatConstIterator(const Mat* _m) + : m(_m), elemSize(_m->elemSize()), ptr(0), sliceStart(0), sliceEnd(0) +{ + if( m && m->isContinuous() ) + { + sliceStart = m->data; + sliceEnd = sliceStart + m->total()*elemSize; + } + seek((const int*)0); +} + +inline +MatConstIterator::MatConstIterator(const Mat* _m, int _row, int _col) + : m(_m), elemSize(_m->elemSize()), ptr(0), sliceStart(0), sliceEnd(0) +{ + CV_Assert(m && m->dims <= 2); + if( m->isContinuous() ) + { + sliceStart = m->data; + sliceEnd = sliceStart + m->total()*elemSize; + } + int idx[] = {_row, _col}; + seek(idx); +} + +inline +MatConstIterator::MatConstIterator(const Mat* _m, Point _pt) + : m(_m), elemSize(_m->elemSize()), ptr(0), sliceStart(0), sliceEnd(0) +{ + CV_Assert(m && m->dims <= 2); + if( m->isContinuous() ) + { + sliceStart = m->data; + sliceEnd = sliceStart + m->total()*elemSize; + } + int idx[] = {_pt.y, _pt.x}; + seek(idx); +} + +inline +MatConstIterator::MatConstIterator(const MatConstIterator& it) + : m(it.m), elemSize(it.elemSize), ptr(it.ptr), sliceStart(it.sliceStart), sliceEnd(it.sliceEnd) +{} + +inline +MatConstIterator& MatConstIterator::operator = (const MatConstIterator& it ) +{ + m = it.m; elemSize = it.elemSize; ptr = it.ptr; + sliceStart = it.sliceStart; sliceEnd = it.sliceEnd; + return *this; +} + +inline +uchar* MatConstIterator::operator *() const +{ + return ptr; +} + +inline MatConstIterator& MatConstIterator::operator += (ptrdiff_t ofs) +{ + if( !m || ofs == 0 ) + return *this; + ptrdiff_t ofsb = ofs*elemSize; + ptr += ofsb; + if( ptr < sliceStart || sliceEnd <= ptr ) + { + ptr -= ofsb; + seek(ofs, true); + } + return *this; +} + +inline +MatConstIterator& MatConstIterator::operator -= (ptrdiff_t ofs) +{ + return (*this += -ofs); +} + +inline +MatConstIterator& MatConstIterator::operator --() +{ + if( m && (ptr -= elemSize) < sliceStart ) + { + ptr += elemSize; + seek(-1, true); + } + return *this; +} + +inline +MatConstIterator MatConstIterator::operator --(int) +{ + MatConstIterator b = *this; + *this += -1; + return b; +} + +inline +MatConstIterator& MatConstIterator::operator ++() +{ + if( m && (ptr += elemSize) >= sliceEnd ) + { + ptr -= elemSize; + seek(1, true); + } + return *this; +} + +inline MatConstIterator MatConstIterator::operator ++(int) +{ + MatConstIterator b = *this; + *this += 1; + return b; +} + + +static inline +bool operator == (const MatConstIterator& a, const MatConstIterator& b) +{ + return a.m == b.m && a.ptr == b.ptr; +} + +static inline +bool operator != (const MatConstIterator& a, const MatConstIterator& b) +{ + return !(a == b); +} + +static inline +bool operator < (const MatConstIterator& a, const MatConstIterator& b) +{ + return a.ptr < b.ptr; +} + +static inline +bool operator > (const MatConstIterator& a, const MatConstIterator& b) +{ + return a.ptr > b.ptr; +} + +static inline +bool operator <= (const MatConstIterator& a, const MatConstIterator& b) +{ + return a.ptr <= b.ptr; +} + +static inline +bool operator >= (const MatConstIterator& a, const MatConstIterator& b) +{ + return a.ptr >= b.ptr; +} + +static inline +ptrdiff_t operator - (const MatConstIterator& b, const MatConstIterator& a) +{ + if( a.m != b.m ) + return ((size_t)(-1) >> 1); + if( a.sliceEnd == b.sliceEnd ) + return (b.ptr - a.ptr)/b.elemSize; + + return b.lpos() - a.lpos(); +} + +static inline +MatConstIterator operator + (const MatConstIterator& a, ptrdiff_t ofs) +{ + MatConstIterator b = a; + return b += ofs; +} + +static inline +MatConstIterator operator + (ptrdiff_t ofs, const MatConstIterator& a) +{ + MatConstIterator b = a; + return b += ofs; +} + +static inline +MatConstIterator operator - (const MatConstIterator& a, ptrdiff_t ofs) +{ + MatConstIterator b = a; + return b += -ofs; +} + + +inline +uchar* MatConstIterator::operator [](ptrdiff_t i) const +{ + return *(*this + i); +} + + + +///////////////////////// MatConstIterator_ ///////////////////////// + +template inline +MatConstIterator_<_Tp>::MatConstIterator_() +{} + +template inline +MatConstIterator_<_Tp>::MatConstIterator_(const Mat_<_Tp>* _m) + : MatConstIterator(_m) +{} + +template inline +MatConstIterator_<_Tp>::MatConstIterator_(const Mat_<_Tp>* _m, int _row, int _col) + : MatConstIterator(_m, _row, _col) +{} + +template inline +MatConstIterator_<_Tp>::MatConstIterator_(const Mat_<_Tp>* _m, Point _pt) + : MatConstIterator(_m, _pt) +{} + +template inline +MatConstIterator_<_Tp>::MatConstIterator_(const MatConstIterator_& it) + : MatConstIterator(it) +{} + +template inline +MatConstIterator_<_Tp>& MatConstIterator_<_Tp>::operator = (const MatConstIterator_& it ) +{ + MatConstIterator::operator = (it); + return *this; +} + +template inline +_Tp MatConstIterator_<_Tp>::operator *() const +{ + return *(_Tp*)(this->ptr); +} + +template inline +MatConstIterator_<_Tp>& MatConstIterator_<_Tp>::operator += (ptrdiff_t ofs) +{ + MatConstIterator::operator += (ofs); + return *this; +} + +template inline +MatConstIterator_<_Tp>& MatConstIterator_<_Tp>::operator -= (ptrdiff_t ofs) +{ + return (*this += -ofs); +} + +template inline +MatConstIterator_<_Tp>& MatConstIterator_<_Tp>::operator --() +{ + MatConstIterator::operator --(); + return *this; +} + +template inline +MatConstIterator_<_Tp> MatConstIterator_<_Tp>::operator --(int) +{ + MatConstIterator_ b = *this; + MatConstIterator::operator --(); + return b; +} + +template inline +MatConstIterator_<_Tp>& MatConstIterator_<_Tp>::operator ++() +{ + MatConstIterator::operator ++(); + return *this; +} + +template inline +MatConstIterator_<_Tp> MatConstIterator_<_Tp>::operator ++(int) +{ + MatConstIterator_ b = *this; + MatConstIterator::operator ++(); + return b; +} + + +template inline +Point MatConstIterator_<_Tp>::pos() const +{ + if( !m ) + return Point(); + CV_DbgAssert( m->dims <= 2 ); + if( m->isContinuous() ) + { + ptrdiff_t ofs = (const _Tp*)ptr - (const _Tp*)m->data; + int y = (int)(ofs / m->cols); + int x = (int)(ofs - (ptrdiff_t)y * m->cols); + return Point(x, y); + } + else + { + ptrdiff_t ofs = (uchar*)ptr - m->data; + int y = (int)(ofs / m->step); + int x = (int)((ofs - y * m->step)/sizeof(_Tp)); + return Point(x, y); + } +} + + +template static inline +bool operator == (const MatConstIterator_<_Tp>& a, const MatConstIterator_<_Tp>& b) +{ + return a.m == b.m && a.ptr == b.ptr; +} + +template static inline +bool operator != (const MatConstIterator_<_Tp>& a, const MatConstIterator_<_Tp>& b) +{ + return a.m != b.m || a.ptr != b.ptr; +} + +template static inline +MatConstIterator_<_Tp> operator + (const MatConstIterator_<_Tp>& a, ptrdiff_t ofs) +{ + MatConstIterator t = (const MatConstIterator&)a + ofs; + return (MatConstIterator_<_Tp>&)t; +} + +template static inline +MatConstIterator_<_Tp> operator + (ptrdiff_t ofs, const MatConstIterator_<_Tp>& a) +{ + MatConstIterator t = (const MatConstIterator&)a + ofs; + return (MatConstIterator_<_Tp>&)t; +} + +template static inline +MatConstIterator_<_Tp> operator - (const MatConstIterator_<_Tp>& a, ptrdiff_t ofs) +{ + MatConstIterator t = (const MatConstIterator&)a - ofs; + return (MatConstIterator_<_Tp>&)t; +} + +template inline +_Tp MatConstIterator_<_Tp>::operator [](ptrdiff_t i) const +{ + return *(_Tp*)MatConstIterator::operator [](i); +} + + + +//////////////////////////// MatIterator_ /////////////////////////// + +template inline +MatIterator_<_Tp>::MatIterator_() + : MatConstIterator_<_Tp>() +{} + +template inline +MatIterator_<_Tp>::MatIterator_(Mat_<_Tp>* _m) + : MatConstIterator_<_Tp>(_m) +{} + +template inline +MatIterator_<_Tp>::MatIterator_(Mat_<_Tp>* _m, int _row, int _col) + : MatConstIterator_<_Tp>(_m, _row, _col) +{} + +template inline +MatIterator_<_Tp>::MatIterator_(const Mat_<_Tp>* _m, Point _pt) + : MatConstIterator_<_Tp>(_m, _pt) +{} + +template inline +MatIterator_<_Tp>::MatIterator_(const Mat_<_Tp>* _m, const int* _idx) + : MatConstIterator_<_Tp>(_m, _idx) +{} + +template inline +MatIterator_<_Tp>::MatIterator_(const MatIterator_& it) + : MatConstIterator_<_Tp>(it) +{} + +template inline +MatIterator_<_Tp>& MatIterator_<_Tp>::operator = (const MatIterator_<_Tp>& it ) +{ + MatConstIterator::operator = (it); + return *this; +} + +template inline +_Tp& MatIterator_<_Tp>::operator *() const +{ + return *(_Tp*)(this->ptr); +} + +template inline +MatIterator_<_Tp>& MatIterator_<_Tp>::operator += (ptrdiff_t ofs) +{ + MatConstIterator::operator += (ofs); + return *this; +} + +template inline +MatIterator_<_Tp>& MatIterator_<_Tp>::operator -= (ptrdiff_t ofs) +{ + MatConstIterator::operator += (-ofs); + return *this; +} + +template inline +MatIterator_<_Tp>& MatIterator_<_Tp>::operator --() +{ + MatConstIterator::operator --(); + return *this; +} + +template inline +MatIterator_<_Tp> MatIterator_<_Tp>::operator --(int) +{ + MatIterator_ b = *this; + MatConstIterator::operator --(); + return b; +} + +template inline +MatIterator_<_Tp>& MatIterator_<_Tp>::operator ++() +{ + MatConstIterator::operator ++(); + return *this; +} + +template inline +MatIterator_<_Tp> MatIterator_<_Tp>::operator ++(int) +{ + MatIterator_ b = *this; + MatConstIterator::operator ++(); + return b; +} + +template inline +_Tp& MatIterator_<_Tp>::operator [](ptrdiff_t i) const +{ + return *(*this + i); +} + + +template static inline +bool operator == (const MatIterator_<_Tp>& a, const MatIterator_<_Tp>& b) +{ + return a.m == b.m && a.ptr == b.ptr; +} + +template static inline +bool operator != (const MatIterator_<_Tp>& a, const MatIterator_<_Tp>& b) +{ + return a.m != b.m || a.ptr != b.ptr; +} + +template static inline +MatIterator_<_Tp> operator + (const MatIterator_<_Tp>& a, ptrdiff_t ofs) +{ + MatConstIterator t = (const MatConstIterator&)a + ofs; + return (MatIterator_<_Tp>&)t; +} + +template static inline +MatIterator_<_Tp> operator + (ptrdiff_t ofs, const MatIterator_<_Tp>& a) +{ + MatConstIterator t = (const MatConstIterator&)a + ofs; + return (MatIterator_<_Tp>&)t; +} + +template static inline +MatIterator_<_Tp> operator - (const MatIterator_<_Tp>& a, ptrdiff_t ofs) +{ + MatConstIterator t = (const MatConstIterator&)a - ofs; + return (MatIterator_<_Tp>&)t; +} + + + +/////////////////////// SparseMatConstIterator ////////////////////// + +inline +SparseMatConstIterator::SparseMatConstIterator() + : m(0), hashidx(0), ptr(0) +{} + +inline +SparseMatConstIterator::SparseMatConstIterator(const SparseMatConstIterator& it) + : m(it.m), hashidx(it.hashidx), ptr(it.ptr) +{} + +inline SparseMatConstIterator& SparseMatConstIterator::operator = (const SparseMatConstIterator& it) +{ + if( this != &it ) + { + m = it.m; + hashidx = it.hashidx; + ptr = it.ptr; + } + return *this; +} + +template inline +const _Tp& SparseMatConstIterator::value() const +{ + return *(_Tp*)ptr; +} + +inline +const SparseMat::Node* SparseMatConstIterator::node() const +{ + return (ptr && m && m->hdr) ? (const SparseMat::Node*)(const void*)(ptr - m->hdr->valueOffset) : 0; +} + +inline +SparseMatConstIterator SparseMatConstIterator::operator ++(int) +{ + SparseMatConstIterator it = *this; + ++*this; + return it; +} + +inline +void SparseMatConstIterator::seekEnd() +{ + if( m && m->hdr ) + { + hashidx = m->hdr->hashtab.size(); + ptr = 0; + } +} + + +static inline +bool operator == (const SparseMatConstIterator& it1, const SparseMatConstIterator& it2) +{ + return it1.m == it2.m && it1.ptr == it2.ptr; +} + +static inline +bool operator != (const SparseMatConstIterator& it1, const SparseMatConstIterator& it2) +{ + return !(it1 == it2); +} + + + +///////////////////////// SparseMatIterator ///////////////////////// + +inline +SparseMatIterator::SparseMatIterator() +{} + +inline +SparseMatIterator::SparseMatIterator(SparseMat* _m) + : SparseMatConstIterator(_m) +{} + +inline +SparseMatIterator::SparseMatIterator(const SparseMatIterator& it) + : SparseMatConstIterator(it) +{} + +inline +SparseMatIterator& SparseMatIterator::operator = (const SparseMatIterator& it) +{ + (SparseMatConstIterator&)*this = it; + return *this; +} + +template inline +_Tp& SparseMatIterator::value() const +{ + return *(_Tp*)ptr; +} + +inline +SparseMat::Node* SparseMatIterator::node() const +{ + return (SparseMat::Node*)SparseMatConstIterator::node(); +} + +inline +SparseMatIterator& SparseMatIterator::operator ++() +{ + SparseMatConstIterator::operator ++(); + return *this; +} + +inline +SparseMatIterator SparseMatIterator::operator ++(int) +{ + SparseMatIterator it = *this; + ++*this; + return it; +} + + + +////////////////////// SparseMatConstIterator_ ////////////////////// + +template inline +SparseMatConstIterator_<_Tp>::SparseMatConstIterator_() +{} + +template inline +SparseMatConstIterator_<_Tp>::SparseMatConstIterator_(const SparseMat_<_Tp>* _m) + : SparseMatConstIterator(_m) +{} + +template inline +SparseMatConstIterator_<_Tp>::SparseMatConstIterator_(const SparseMat* _m) + : SparseMatConstIterator(_m) +{ + CV_Assert( _m->type() == DataType<_Tp>::type ); +} + +template inline +SparseMatConstIterator_<_Tp>::SparseMatConstIterator_(const SparseMatConstIterator_<_Tp>& it) + : SparseMatConstIterator(it) +{} + +template inline +SparseMatConstIterator_<_Tp>& SparseMatConstIterator_<_Tp>::operator = (const SparseMatConstIterator_<_Tp>& it) +{ + return reinterpret_cast&> + (*reinterpret_cast(this) = + reinterpret_cast(it)); +} + +template inline +const _Tp& SparseMatConstIterator_<_Tp>::operator *() const +{ + return *(const _Tp*)this->ptr; +} + +template inline +SparseMatConstIterator_<_Tp>& SparseMatConstIterator_<_Tp>::operator ++() +{ + SparseMatConstIterator::operator ++(); + return *this; +} + +template inline +SparseMatConstIterator_<_Tp> SparseMatConstIterator_<_Tp>::operator ++(int) +{ - SparseMatConstIterator it = *this; ++ SparseMatConstIterator_<_Tp> it = *this; + SparseMatConstIterator::operator ++(); + return it; +} + + + +///////////////////////// SparseMatIterator_ //////////////////////// + +template inline +SparseMatIterator_<_Tp>::SparseMatIterator_() +{} + +template inline +SparseMatIterator_<_Tp>::SparseMatIterator_(SparseMat_<_Tp>* _m) + : SparseMatConstIterator_<_Tp>(_m) +{} + +template inline +SparseMatIterator_<_Tp>::SparseMatIterator_(SparseMat* _m) + : SparseMatConstIterator_<_Tp>(_m) +{} + +template inline +SparseMatIterator_<_Tp>::SparseMatIterator_(const SparseMatIterator_<_Tp>& it) + : SparseMatConstIterator_<_Tp>(it) +{} + +template inline +SparseMatIterator_<_Tp>& SparseMatIterator_<_Tp>::operator = (const SparseMatIterator_<_Tp>& it) +{ + return reinterpret_cast&> + (*reinterpret_cast(this) = + reinterpret_cast(it)); +} + +template inline +_Tp& SparseMatIterator_<_Tp>::operator *() const +{ + return *(_Tp*)this->ptr; +} + +template inline +SparseMatIterator_<_Tp>& SparseMatIterator_<_Tp>::operator ++() +{ + SparseMatConstIterator::operator ++(); + return *this; +} + +template inline +SparseMatIterator_<_Tp> SparseMatIterator_<_Tp>::operator ++(int) +{ - SparseMatIterator it = *this; ++ SparseMatIterator_<_Tp> it = *this; + SparseMatConstIterator::operator ++(); + return it; +} + + + +//////////////////////// MatCommaInitializer_ /////////////////////// + +template inline +MatCommaInitializer_<_Tp>::MatCommaInitializer_(Mat_<_Tp>* _m) + : it(_m) +{} + +template template inline +MatCommaInitializer_<_Tp>& MatCommaInitializer_<_Tp>::operator , (T2 v) +{ + CV_DbgAssert( this->it < ((const Mat_<_Tp>*)this->it.m)->end() ); + *this->it = _Tp(v); + ++this->it; + return *this; +} + +template inline +MatCommaInitializer_<_Tp>::operator Mat_<_Tp>() const +{ + CV_DbgAssert( this->it == ((const Mat_<_Tp>*)this->it.m)->end() ); + return Mat_<_Tp>(*this->it.m); +} + + +template static inline +MatCommaInitializer_<_Tp> operator << (const Mat_<_Tp>& m, T2 val) +{ + MatCommaInitializer_<_Tp> commaInitializer((Mat_<_Tp>*)&m); + return (commaInitializer, val); +} + + + +///////////////////////// Matrix Expressions //////////////////////// + +inline +Mat& Mat::operator = (const MatExpr& e) +{ + e.op->assign(e, *this); + return *this; +} + +template inline +Mat_<_Tp>::Mat_(const MatExpr& e) +{ + e.op->assign(e, *this, DataType<_Tp>::type); +} + +template inline +Mat_<_Tp>& Mat_<_Tp>::operator = (const MatExpr& e) +{ + e.op->assign(e, *this, DataType<_Tp>::type); + return *this; +} + +template inline +MatExpr Mat_<_Tp>::zeros(int rows, int cols) +{ + return Mat::zeros(rows, cols, DataType<_Tp>::type); +} + +template inline +MatExpr Mat_<_Tp>::zeros(Size sz) +{ + return Mat::zeros(sz, DataType<_Tp>::type); +} + +template inline +MatExpr Mat_<_Tp>::ones(int rows, int cols) +{ + return Mat::ones(rows, cols, DataType<_Tp>::type); +} + +template inline +MatExpr Mat_<_Tp>::ones(Size sz) +{ + return Mat::ones(sz, DataType<_Tp>::type); +} + +template inline +MatExpr Mat_<_Tp>::eye(int rows, int cols) +{ + return Mat::eye(rows, cols, DataType<_Tp>::type); +} + +template inline +MatExpr Mat_<_Tp>::eye(Size sz) +{ + return Mat::eye(sz, DataType<_Tp>::type); +} + +inline +MatExpr::MatExpr() + : op(0), flags(0), a(Mat()), b(Mat()), c(Mat()), alpha(0), beta(0), s() +{} + +inline +MatExpr::MatExpr(const MatOp* _op, int _flags, const Mat& _a, const Mat& _b, + const Mat& _c, double _alpha, double _beta, const Scalar& _s) + : op(_op), flags(_flags), a(_a), b(_b), c(_c), alpha(_alpha), beta(_beta), s(_s) +{} + +inline +MatExpr::operator Mat() const +{ + Mat m; + op->assign(*this, m); + return m; +} + +template inline +MatExpr::operator Mat_<_Tp>() const +{ + Mat_<_Tp> m; + op->assign(*this, m, DataType<_Tp>::type); + return m; +} + + +template static inline +MatExpr min(const Mat_<_Tp>& a, const Mat_<_Tp>& b) +{ + return cv::min((const Mat&)a, (const Mat&)b); +} + +template static inline +MatExpr min(const Mat_<_Tp>& a, double s) +{ + return cv::min((const Mat&)a, s); +} + +template static inline +MatExpr min(double s, const Mat_<_Tp>& a) +{ + return cv::min((const Mat&)a, s); +} + +template static inline +MatExpr max(const Mat_<_Tp>& a, const Mat_<_Tp>& b) +{ + return cv::max((const Mat&)a, (const Mat&)b); +} + +template static inline +MatExpr max(const Mat_<_Tp>& a, double s) +{ + return cv::max((const Mat&)a, s); +} + +template static inline +MatExpr max(double s, const Mat_<_Tp>& a) +{ + return cv::max((const Mat&)a, s); +} + +template static inline +MatExpr abs(const Mat_<_Tp>& m) +{ + return cv::abs((const Mat&)m); +} + + +static inline +Mat& operator += (Mat& a, const MatExpr& b) +{ + b.op->augAssignAdd(b, a); + return a; +} + +static inline +const Mat& operator += (const Mat& a, const MatExpr& b) +{ + b.op->augAssignAdd(b, (Mat&)a); + return a; +} + +template static inline +Mat_<_Tp>& operator += (Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignAdd(b, a); + return a; +} + +template static inline +const Mat_<_Tp>& operator += (const Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignAdd(b, (Mat&)a); + return a; +} + +static inline +Mat& operator -= (Mat& a, const MatExpr& b) +{ + b.op->augAssignSubtract(b, a); + return a; +} + +static inline +const Mat& operator -= (const Mat& a, const MatExpr& b) +{ + b.op->augAssignSubtract(b, (Mat&)a); + return a; +} + +template static inline +Mat_<_Tp>& operator -= (Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignSubtract(b, a); + return a; +} + +template static inline +const Mat_<_Tp>& operator -= (const Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignSubtract(b, (Mat&)a); + return a; +} + +static inline +Mat& operator *= (Mat& a, const MatExpr& b) +{ + b.op->augAssignMultiply(b, a); + return a; +} + +static inline +const Mat& operator *= (const Mat& a, const MatExpr& b) +{ + b.op->augAssignMultiply(b, (Mat&)a); + return a; +} + +template static inline +Mat_<_Tp>& operator *= (Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignMultiply(b, a); + return a; +} + +template static inline +const Mat_<_Tp>& operator *= (const Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignMultiply(b, (Mat&)a); + return a; +} + +static inline +Mat& operator /= (Mat& a, const MatExpr& b) +{ + b.op->augAssignDivide(b, a); + return a; +} + +static inline +const Mat& operator /= (const Mat& a, const MatExpr& b) +{ + b.op->augAssignDivide(b, (Mat&)a); + return a; +} + +template static inline +Mat_<_Tp>& operator /= (Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignDivide(b, a); + return a; +} + +template static inline +const Mat_<_Tp>& operator /= (const Mat_<_Tp>& a, const MatExpr& b) +{ + b.op->augAssignDivide(b, (Mat&)a); + return a; +} + + +//////////////////////////////// UMat //////////////////////////////// + +inline +UMat::UMat(UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{} + +inline +UMat::UMat(int _rows, int _cols, int _type, UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{ + create(_rows, _cols, _type); +} + +inline +UMat::UMat(int _rows, int _cols, int _type, const Scalar& _s, UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{ + create(_rows, _cols, _type); + *this = _s; +} + +inline +UMat::UMat(Size _sz, int _type, UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{ + create( _sz.height, _sz.width, _type ); +} + +inline +UMat::UMat(Size _sz, int _type, const Scalar& _s, UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{ + create(_sz.height, _sz.width, _type); + *this = _s; +} + +inline +UMat::UMat(int _dims, const int* _sz, int _type, UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{ + create(_dims, _sz, _type); +} + +inline +UMat::UMat(int _dims, const int* _sz, int _type, const Scalar& _s, UMatUsageFlags _usageFlags) +: flags(MAGIC_VAL), dims(0), rows(0), cols(0), allocator(0), usageFlags(_usageFlags), u(0), offset(0), size(&rows) +{ + create(_dims, _sz, _type); + *this = _s; +} + +inline +UMat::UMat(const UMat& m) +: flags(m.flags), dims(m.dims), rows(m.rows), cols(m.cols), allocator(m.allocator), + usageFlags(m.usageFlags), u(m.u), offset(m.offset), size(&rows) +{ + addref(); + if( m.dims <= 2 ) + { + step[0] = m.step[0]; step[1] = m.step[1]; + } + else + { + dims = 0; + copySize(m); + } +} + + +template inline +UMat::UMat(const std::vector<_Tp>& vec, bool copyData) +: flags(MAGIC_VAL | DataType<_Tp>::type | CV_MAT_CONT_FLAG), dims(2), rows((int)vec.size()), +cols(1), allocator(0), usageFlags(USAGE_DEFAULT), u(0), offset(0), size(&rows) +{ + if(vec.empty()) + return; + if( !copyData ) + { + // !!!TODO!!! + CV_Error(Error::StsNotImplemented, ""); + } + else + Mat((int)vec.size(), 1, DataType<_Tp>::type, (uchar*)&vec[0]).copyTo(*this); +} + + +inline +UMat& UMat::operator = (const UMat& m) +{ + if( this != &m ) + { + const_cast(m).addref(); + release(); + flags = m.flags; + if( dims <= 2 && m.dims <= 2 ) + { + dims = m.dims; + rows = m.rows; + cols = m.cols; + step[0] = m.step[0]; + step[1] = m.step[1]; + } + else + copySize(m); + allocator = m.allocator; + if (usageFlags == USAGE_DEFAULT) + usageFlags = m.usageFlags; + u = m.u; + offset = m.offset; + } + return *this; +} + +inline +UMat UMat::row(int y) const +{ + return UMat(*this, Range(y, y + 1), Range::all()); +} + +inline +UMat UMat::col(int x) const +{ + return UMat(*this, Range::all(), Range(x, x + 1)); +} + +inline +UMat UMat::rowRange(int startrow, int endrow) const +{ + return UMat(*this, Range(startrow, endrow), Range::all()); +} + +inline +UMat UMat::rowRange(const Range& r) const +{ + return UMat(*this, r, Range::all()); +} + +inline +UMat UMat::colRange(int startcol, int endcol) const +{ + return UMat(*this, Range::all(), Range(startcol, endcol)); +} + +inline +UMat UMat::colRange(const Range& r) const +{ + return UMat(*this, Range::all(), r); +} + +inline +UMat UMat::clone() const +{ + UMat m; + copyTo(m); + return m; +} + +inline +void UMat::assignTo( UMat& m, int _type ) const +{ + if( _type < 0 ) + m = *this; + else + convertTo(m, _type); +} + +inline +void UMat::create(int _rows, int _cols, int _type, UMatUsageFlags _usageFlags) +{ + _type &= TYPE_MASK; + if( dims <= 2 && rows == _rows && cols == _cols && type() == _type && u ) + return; + int sz[] = {_rows, _cols}; + create(2, sz, _type, _usageFlags); +} + +inline +void UMat::create(Size _sz, int _type, UMatUsageFlags _usageFlags) +{ + create(_sz.height, _sz.width, _type, _usageFlags); +} + +inline +void UMat::addref() +{ + if( u ) + CV_XADD(&(u->urefcount), 1); +} + +inline void UMat::release() +{ + if( u && CV_XADD(&(u->urefcount), -1) == 1 ) + deallocate(); + size.p[0] = 0; + u = 0; +} + +inline +UMat UMat::operator()( Range _rowRange, Range _colRange ) const +{ + return UMat(*this, _rowRange, _colRange); +} + +inline +UMat UMat::operator()( const Rect& roi ) const +{ + return UMat(*this, roi); +} + +inline +UMat UMat::operator()(const Range* ranges) const +{ + return UMat(*this, ranges); +} + +inline +bool UMat::isContinuous() const +{ + return (flags & CONTINUOUS_FLAG) != 0; +} + +inline +bool UMat::isSubmatrix() const +{ + return (flags & SUBMATRIX_FLAG) != 0; +} + +inline +size_t UMat::elemSize() const +{ + return dims > 0 ? step.p[dims - 1] : 0; +} + +inline +size_t UMat::elemSize1() const +{ + return CV_ELEM_SIZE1(flags); +} + +inline +int UMat::type() const +{ + return CV_MAT_TYPE(flags); +} + +inline +int UMat::depth() const +{ + return CV_MAT_DEPTH(flags); +} + +inline +int UMat::channels() const +{ + return CV_MAT_CN(flags); +} + +inline +size_t UMat::step1(int i) const +{ + return step.p[i] / elemSize1(); +} + +inline +bool UMat::empty() const +{ + return u == 0 || total() == 0; +} + +inline +size_t UMat::total() const +{ + if( dims <= 2 ) + return (size_t)rows * cols; + size_t p = 1; + for( int i = 0; i < dims; i++ ) + p *= size[i]; + return p; +} + +inline bool UMatData::hostCopyObsolete() const { return (flags & HOST_COPY_OBSOLETE) != 0; } +inline bool UMatData::deviceCopyObsolete() const { return (flags & DEVICE_COPY_OBSOLETE) != 0; } +inline bool UMatData::copyOnMap() const { return (flags & COPY_ON_MAP) != 0; } +inline bool UMatData::tempUMat() const { return (flags & TEMP_UMAT) != 0; } +inline bool UMatData::tempCopiedUMat() const { return (flags & TEMP_COPIED_UMAT) == TEMP_COPIED_UMAT; } + +inline void UMatData::markHostCopyObsolete(bool flag) +{ + if(flag) + flags |= HOST_COPY_OBSOLETE; + else + flags &= ~HOST_COPY_OBSOLETE; +} +inline void UMatData::markDeviceCopyObsolete(bool flag) +{ + if(flag) + flags |= DEVICE_COPY_OBSOLETE; + else + flags &= ~DEVICE_COPY_OBSOLETE; +} + +inline UMatDataAutoLock::UMatDataAutoLock(UMatData* _u) : u(_u) { u->lock(); } +inline UMatDataAutoLock::~UMatDataAutoLock() { u->unlock(); } + +} //cv + +#endif diff --cc modules/features2d/include/opencv2/features2d.hpp index 9f46ee2,0000000..7be9248 mode 100644,000000..100644 --- a/modules/features2d/include/opencv2/features2d.hpp +++ b/modules/features2d/include/opencv2/features2d.hpp @@@ -1,1625 -1,0 +1,1628 @@@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#ifndef __OPENCV_FEATURES_2D_HPP__ +#define __OPENCV_FEATURES_2D_HPP__ + +#include "opencv2/core.hpp" +#include "opencv2/flann/miniflann.hpp" + +namespace cv +{ + +CV_EXPORTS bool initModule_features2d(); + +// //! writes vector of keypoints to the file storage +// CV_EXPORTS void write(FileStorage& fs, const String& name, const std::vector& keypoints); +// //! reads vector of keypoints from the specified file storage node +// CV_EXPORTS void read(const FileNode& node, CV_OUT std::vector& keypoints); + +/* + * A class filters a vector of keypoints. + * Because now it is difficult to provide a convenient interface for all usage scenarios of the keypoints filter class, + * it has only several needed by now static methods. + */ +class CV_EXPORTS KeyPointsFilter +{ +public: + KeyPointsFilter(){} + + /* + * Remove keypoints within borderPixels of an image edge. + */ + static void runByImageBorder( std::vector& keypoints, Size imageSize, int borderSize ); + /* + * Remove keypoints of sizes out of range. + */ + static void runByKeypointSize( std::vector& keypoints, float minSize, + float maxSize=FLT_MAX ); + /* + * Remove keypoints from some image by mask for pixels of this image. + */ + static void runByPixelsMask( std::vector& keypoints, const Mat& mask ); + /* + * Remove duplicated keypoints. + */ + static void removeDuplicated( std::vector& keypoints ); + + /* + * Retain the specified number of the best keypoints (according to the response) + */ + static void retainBest( std::vector& keypoints, int npoints ); +}; + + +/************************************ Base Classes ************************************/ + +/* + * Abstract base class for 2D image feature detectors. + */ +class CV_EXPORTS_W FeatureDetector : public virtual Algorithm +{ +public: + virtual ~FeatureDetector(); + + /* + * Detect keypoints in an image. + * image The image. + * keypoints The detected keypoints. + * mask Mask specifying where to look for keypoints (optional). Must be a char + * matrix with non-zero values in the region of interest. + */ + CV_WRAP void detect( InputArray image, CV_OUT std::vector& keypoints, InputArray mask=noArray() ) const; + + /* + * Detect keypoints in an image set. + * images Image collection. + * keypoints Collection of keypoints detected in an input images. keypoints[i] is a set of keypoints detected in an images[i]. + * masks Masks for image set. masks[i] is a mask for images[i]. + */ + void detect( InputArrayOfArrays images, std::vector >& keypoints, InputArrayOfArrays masks=noArray() ) const; + + // Return true if detector object is empty + CV_WRAP virtual bool empty() const; + + // Create feature detector by detector name. + CV_WRAP static Ptr create( const String& detectorType ); + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const = 0; + + /* + * Remove keypoints that are not in the mask. + * Helper function, useful when wrapping a library call for keypoint detection that + * does not support a mask argument. + */ + static void removeInvalidPoints( const Mat & mask, std::vector& keypoints ); +}; + + +/* + * Abstract base class for computing descriptors for image keypoints. + * + * In this interface we assume a keypoint descriptor can be represented as a + * dense, fixed-dimensional vector of some basic type. Most descriptors used + * in practice follow this pattern, as it makes it very easy to compute + * distances between descriptors. Therefore we represent a collection of + * descriptors as a Mat, where each row is one keypoint descriptor. + */ +class CV_EXPORTS_W DescriptorExtractor : public virtual Algorithm +{ +public: + virtual ~DescriptorExtractor(); + + /* + * Compute the descriptors for a set of keypoints in an image. + * image The image. + * keypoints The input keypoints. Keypoints for which a descriptor cannot be computed are removed. + * descriptors Copmputed descriptors. Row i is the descriptor for keypoint i. + */ + CV_WRAP void compute( InputArray image, CV_OUT CV_IN_OUT std::vector& keypoints, OutputArray descriptors ) const; + + /* + * Compute the descriptors for a keypoints collection detected in image collection. + * images Image collection. + * keypoints Input keypoints collection. keypoints[i] is keypoints detected in images[i]. + * Keypoints for which a descriptor cannot be computed are removed. + * descriptors Descriptor collection. descriptors[i] are descriptors computed for set keypoints[i]. + */ + void compute( InputArrayOfArrays images, std::vector >& keypoints, OutputArrayOfArrays descriptors ) const; + + CV_WRAP virtual int descriptorSize() const = 0; + CV_WRAP virtual int descriptorType() const = 0; + CV_WRAP virtual int defaultNorm() const = 0; + + CV_WRAP virtual bool empty() const; + + CV_WRAP static Ptr create( const String& descriptorExtractorType ); + +protected: + virtual void computeImpl( InputArray image, std::vector& keypoints, OutputArray descriptors ) const = 0; + + /* + * Remove keypoints within borderPixels of an image edge. + */ + static void removeBorderKeypoints( std::vector& keypoints, + Size imageSize, int borderSize ); +}; + + + +/* + * Abstract base class for simultaneous 2D feature detection descriptor extraction. + */ +class CV_EXPORTS_W Feature2D : public FeatureDetector, public DescriptorExtractor +{ +public: + /* + * Detect keypoints in an image. + * image The image. + * keypoints The detected keypoints. + * mask Mask specifying where to look for keypoints (optional). Must be a char + * matrix with non-zero values in the region of interest. + * useProvidedKeypoints If true, the method will skip the detection phase and will compute + * descriptors for the provided keypoints + */ + CV_WRAP_AS(detectAndCompute) virtual void operator()( InputArray image, InputArray mask, + CV_OUT std::vector& keypoints, + OutputArray descriptors, + bool useProvidedKeypoints=false ) const = 0; + + CV_WRAP void compute( InputArray image, CV_OUT CV_IN_OUT std::vector& keypoints, OutputArray descriptors ) const; + + // Create feature detector and descriptor extractor by name. + CV_WRAP static Ptr create( const String& name ); +}; + +/*! + BRISK implementation +*/ +class CV_EXPORTS_W BRISK : public Feature2D +{ +public: + CV_WRAP explicit BRISK(int thresh=30, int octaves=3, float patternScale=1.0f); + + virtual ~BRISK(); + + // returns the descriptor size in bytes + int descriptorSize() const; + // returns the descriptor type + int descriptorType() const; + // returns the default norm type + int defaultNorm() const; + + // Compute the BRISK features on an image + void operator()(InputArray image, InputArray mask, std::vector& keypoints) const; + + // Compute the BRISK features and descriptors on an image + void operator()( InputArray image, InputArray mask, std::vector& keypoints, + OutputArray descriptors, bool useProvidedKeypoints=false ) const; + + AlgorithmInfo* info() const; + + // custom setup + CV_WRAP explicit BRISK(std::vector &radiusList, std::vector &numberList, + float dMax=5.85f, float dMin=8.2f, std::vector indexChange=std::vector()); + + // call this to generate the kernel: + // circle of radius r (pixels), with n points; + // short pairings with dMax, long pairings with dMin + CV_WRAP void generateKernel(std::vector &radiusList, + std::vector &numberList, float dMax=5.85f, float dMin=8.2f, + std::vector indexChange=std::vector()); + +protected: + + void computeImpl( InputArray image, std::vector& keypoints, OutputArray descriptors ) const; + void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + void computeKeypointsNoOrientation(InputArray image, InputArray mask, std::vector& keypoints) const; + void computeDescriptorsAndOrOrientation(InputArray image, InputArray mask, std::vector& keypoints, + OutputArray descriptors, bool doDescriptors, bool doOrientation, + bool useProvidedKeypoints) const; + + // Feature parameters + CV_PROP_RW int threshold; + CV_PROP_RW int octaves; + + // some helper structures for the Brisk pattern representation + struct BriskPatternPoint{ + float x; // x coordinate relative to center + float y; // x coordinate relative to center + float sigma; // Gaussian smoothing sigma + }; + struct BriskShortPair{ + unsigned int i; // index of the first pattern point + unsigned int j; // index of other pattern point + }; + struct BriskLongPair{ + unsigned int i; // index of the first pattern point + unsigned int j; // index of other pattern point + int weighted_dx; // 1024.0/dx + int weighted_dy; // 1024.0/dy + }; + inline int smoothedIntensity(const cv::Mat& image, + const cv::Mat& integral,const float key_x, + const float key_y, const unsigned int scale, + const unsigned int rot, const unsigned int point) const; + // pattern properties + BriskPatternPoint* patternPoints_; //[i][rotation][scale] + unsigned int points_; // total number of collocation points + float* scaleList_; // lists the scaling per scale index [scale] + unsigned int* sizeList_; // lists the total pattern size per scale index [scale] + static const unsigned int scales_; // scales discretization + static const float scalerange_; // span of sizes 40->4 Octaves - else, this needs to be adjusted... + static const unsigned int n_rot_; // discretization of the rotation look-up + + // pairs + int strings_; // number of uchars the descriptor consists of + float dMax_; // short pair maximum distance + float dMin_; // long pair maximum distance + BriskShortPair* shortPairs_; // d<_dMax + BriskLongPair* longPairs_; // d>_dMin + unsigned int noShortPairs_; // number of shortParis + unsigned int noLongPairs_; // number of longParis + + // general + static const float basicSize_; +}; + + +/*! + ORB implementation. +*/ +class CV_EXPORTS_W ORB : public Feature2D +{ +public: + // the size of the signature in bytes + enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 }; + + CV_WRAP explicit ORB(int nfeatures = 500, float scaleFactor = 1.2f, int nlevels = 8, int edgeThreshold = 31, + int firstLevel = 0, int WTA_K=2, int scoreType=ORB::HARRIS_SCORE, int patchSize=31 ); + + // returns the descriptor size in bytes + int descriptorSize() const; + // returns the descriptor type + int descriptorType() const; + // returns the default norm type + int defaultNorm() const; + + // Compute the ORB features and descriptors on an image + void operator()(InputArray image, InputArray mask, std::vector& keypoints) const; + + // Compute the ORB features and descriptors on an image + void operator()( InputArray image, InputArray mask, std::vector& keypoints, + OutputArray descriptors, bool useProvidedKeypoints=false ) const; + + AlgorithmInfo* info() const; + +protected: + + void computeImpl( InputArray image, std::vector& keypoints, OutputArray descriptors ) const; + void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + CV_PROP_RW int nfeatures; + CV_PROP_RW double scaleFactor; + CV_PROP_RW int nlevels; + CV_PROP_RW int edgeThreshold; + CV_PROP_RW int firstLevel; + CV_PROP_RW int WTA_K; + CV_PROP_RW int scoreType; + CV_PROP_RW int patchSize; +}; + +typedef ORB OrbFeatureDetector; +typedef ORB OrbDescriptorExtractor; + +/*! + FREAK implementation +*/ +class CV_EXPORTS FREAK : public DescriptorExtractor +{ +public: + /** Constructor + * @param orientationNormalized enable orientation normalization + * @param scaleNormalized enable scale normalization + * @param patternScale scaling of the description pattern + * @param nbOctave number of octaves covered by the detected keypoints + * @param selectedPairs (optional) user defined selected pairs + */ + explicit FREAK( bool orientationNormalized = true, + bool scaleNormalized = true, + float patternScale = 22.0f, + int nOctaves = 4, + const std::vector& selectedPairs = std::vector()); + FREAK( const FREAK& rhs ); + FREAK& operator=( const FREAK& ); + + virtual ~FREAK(); + + /** returns the descriptor length in bytes */ + virtual int descriptorSize() const; + + /** returns the descriptor type */ + virtual int descriptorType() const; + + /** returns the default norm type */ + virtual int defaultNorm() const; + + /** select the 512 "best description pairs" + * @param images grayscale images set + * @param keypoints set of detected keypoints + * @param corrThresh correlation threshold + * @param verbose print construction information + * @return list of best pair indexes + */ + std::vector selectPairs( const std::vector& images, std::vector >& keypoints, + const double corrThresh = 0.7, bool verbose = true ); + + AlgorithmInfo* info() const; + + enum + { + NB_SCALES = 64, NB_PAIRS = 512, NB_ORIENPAIRS = 45 + }; + +protected: + virtual void computeImpl( InputArray image, std::vector& keypoints, OutputArray descriptors ) const; + void buildPattern(); + + template + imgType meanIntensity( InputArray image, InputArray integral, const float kp_x, const float kp_y, + const unsigned int scale, const unsigned int rot, const unsigned int point ) const; + + template + void computeDescriptors( InputArray image, std::vector& keypoints, OutputArray descriptors ) const; + + template + void extractDescriptor(srcMatType *pointsValue, void ** ptr) const; + + bool orientationNormalized; //true if the orientation is normalized, false otherwise + bool scaleNormalized; //true if the scale is normalized, false otherwise + double patternScale; //scaling of the pattern + int nOctaves; //number of octaves + bool extAll; // true if all pairs need to be extracted for pairs selection + + double patternScale0; + int nOctaves0; + std::vector selectedPairs0; + + struct PatternPoint + { + float x; // x coordinate relative to center + float y; // x coordinate relative to center + float sigma; // Gaussian smoothing sigma + }; + + struct DescriptionPair + { + uchar i; // index of the first point + uchar j; // index of the second point + }; + + struct OrientationPair + { + uchar i; // index of the first point + uchar j; // index of the second point + int weight_dx; // dx/(norm_sq))*4096 + int weight_dy; // dy/(norm_sq))*4096 + }; + + std::vector patternLookup; // look-up table for the pattern points (position+sigma of all points at all scales and orientation) + int patternSizes[NB_SCALES]; // size of the pattern at a specific scale (used to check if a point is within image boundaries) + DescriptionPair descriptionPairs[NB_PAIRS]; + OrientationPair orientationPairs[NB_ORIENPAIRS]; +}; + + +/*! + Maximal Stable Extremal Regions class. + + The class implements MSER algorithm introduced by J. Matas. + Unlike SIFT, SURF and many other detectors in OpenCV, this is salient region detector, + not the salient point detector. + + It returns the regions, each of those is encoded as a contour. +*/ +class CV_EXPORTS_W MSER : public FeatureDetector +{ +public: + //! the full constructor + CV_WRAP explicit MSER( int _delta=5, int _min_area=60, int _max_area=14400, + double _max_variation=0.25, double _min_diversity=.2, + int _max_evolution=200, double _area_threshold=1.01, + double _min_margin=0.003, int _edge_blur_size=5 ); + + //! the operator that extracts the MSERs from the image or the specific part of it + CV_WRAP_AS(detect) void operator()( InputArray image, CV_OUT std::vector >& msers, + InputArray mask=noArray() ) const; + AlgorithmInfo* info() const; + +protected: + void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + int delta; + int minArea; + int maxArea; + double maxVariation; + double minDiversity; + int maxEvolution; + double areaThreshold; + double minMargin; + int edgeBlurSize; +}; + +typedef MSER MserFeatureDetector; + +/*! + The "Star" Detector. + + The class implements the keypoint detector introduced by K. Konolige. +*/ +class CV_EXPORTS_W StarDetector : public FeatureDetector +{ +public: + //! the full constructor + CV_WRAP StarDetector(int _maxSize=45, int _responseThreshold=30, + int _lineThresholdProjected=10, + int _lineThresholdBinarized=8, + int _suppressNonmaxSize=5); + + //! finds the keypoints in the image + CV_WRAP_AS(detect) void operator()(const Mat& image, + CV_OUT std::vector& keypoints) const; + + AlgorithmInfo* info() const; + +protected: + void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + int maxSize; + int responseThreshold; + int lineThresholdProjected; + int lineThresholdBinarized; + int suppressNonmaxSize; +}; + +//! detects corners using FAST algorithm by E. Rosten +CV_EXPORTS void FAST( InputArray image, CV_OUT std::vector& keypoints, + int threshold, bool nonmaxSuppression=true ); + +CV_EXPORTS void FAST( InputArray image, CV_OUT std::vector& keypoints, + int threshold, bool nonmaxSuppression, int type ); + +class CV_EXPORTS_W FastFeatureDetector : public FeatureDetector +{ +public: + enum Type + { + TYPE_5_8 = 0, TYPE_7_12 = 1, TYPE_9_16 = 2 + }; + + CV_WRAP FastFeatureDetector( int threshold=10, bool nonmaxSuppression=true); + CV_WRAP FastFeatureDetector( int threshold, bool nonmaxSuppression, int type); + AlgorithmInfo* info() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + int threshold; + bool nonmaxSuppression; + int type; +}; + + +class CV_EXPORTS_W GFTTDetector : public FeatureDetector +{ +public: + CV_WRAP GFTTDetector( int maxCorners=1000, double qualityLevel=0.01, double minDistance=1, + int blockSize=3, bool useHarrisDetector=false, double k=0.04 ); + AlgorithmInfo* info() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + int nfeatures; + double qualityLevel; + double minDistance; + int blockSize; + bool useHarrisDetector; + double k; +}; + +typedef GFTTDetector GoodFeaturesToTrackDetector; +typedef StarDetector StarFeatureDetector; + +class CV_EXPORTS_W SimpleBlobDetector : public FeatureDetector +{ +public: + struct CV_EXPORTS_W_SIMPLE Params + { + CV_WRAP Params(); + CV_PROP_RW float thresholdStep; + CV_PROP_RW float minThreshold; + CV_PROP_RW float maxThreshold; + CV_PROP_RW size_t minRepeatability; + CV_PROP_RW float minDistBetweenBlobs; + + CV_PROP_RW bool filterByColor; + CV_PROP_RW uchar blobColor; + + CV_PROP_RW bool filterByArea; + CV_PROP_RW float minArea, maxArea; + + CV_PROP_RW bool filterByCircularity; + CV_PROP_RW float minCircularity, maxCircularity; + + CV_PROP_RW bool filterByInertia; + CV_PROP_RW float minInertiaRatio, maxInertiaRatio; + + CV_PROP_RW bool filterByConvexity; + CV_PROP_RW float minConvexity, maxConvexity; + + void read( const FileNode& fn ); + void write( FileStorage& fs ) const; + }; + + CV_WRAP SimpleBlobDetector(const SimpleBlobDetector::Params ¶meters = SimpleBlobDetector::Params()); + + virtual void read( const FileNode& fn ); + virtual void write( FileStorage& fs ) const; + +protected: + struct CV_EXPORTS Center + { + Point2d location; + double radius; + double confidence; + }; + + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + virtual void findBlobs(InputArray image, InputArray binaryImage, std::vector
¢ers) const; + + Params params; + AlgorithmInfo* info() const; +}; + + +class CV_EXPORTS_W DenseFeatureDetector : public FeatureDetector +{ +public: + CV_WRAP explicit DenseFeatureDetector( float initFeatureScale=1.f, int featureScaleLevels=1, + float featureScaleMul=0.1f, + int initXyStep=6, int initImgBound=0, + bool varyXyStepWithScale=true, + bool varyImgBoundWithScale=false ); + AlgorithmInfo* info() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + double initFeatureScale; + int featureScaleLevels; + double featureScaleMul; + + int initXyStep; + int initImgBound; + + bool varyXyStepWithScale; + bool varyImgBoundWithScale; +}; + +/* + * Adapts a detector to partition the source image into a grid and detect + * points in each cell. + */ +class CV_EXPORTS_W GridAdaptedFeatureDetector : public FeatureDetector +{ +public: + /* + * detector Detector that will be adapted. + * maxTotalKeypoints Maximum count of keypoints detected on the image. Only the strongest keypoints + * will be keeped. + * gridRows Grid rows count. + * gridCols Grid column count. + */ + CV_WRAP GridAdaptedFeatureDetector( const Ptr& detector=Ptr(), + int maxTotalKeypoints=1000, + int gridRows=4, int gridCols=4 ); + + // TODO implement read/write + virtual bool empty() const; + + AlgorithmInfo* info() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + Ptr detector; + int maxTotalKeypoints; + int gridRows; + int gridCols; +}; + +/* + * Adapts a detector to detect points over multiple levels of a Gaussian + * pyramid. Useful for detectors that are not inherently scaled. + */ +class CV_EXPORTS_W PyramidAdaptedFeatureDetector : public FeatureDetector +{ +public: + // maxLevel - The 0-based index of the last pyramid layer + CV_WRAP PyramidAdaptedFeatureDetector( const Ptr& detector, int maxLevel=2 ); + + // TODO implement read/write + virtual bool empty() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + Ptr detector; + int maxLevel; +}; + +/** \brief A feature detector parameter adjuster, this is used by the DynamicAdaptedFeatureDetector + * and is a wrapper for FeatureDetector that allow them to be adjusted after a detection + */ +class CV_EXPORTS AdjusterAdapter: public FeatureDetector +{ +public: + /** pure virtual interface + */ + virtual ~AdjusterAdapter() {} + /** too few features were detected so, adjust the detector params accordingly + * \param min the minimum number of desired features + * \param n_detected the number previously detected + */ + virtual void tooFew(int min, int n_detected) = 0; + /** too many features were detected so, adjust the detector params accordingly + * \param max the maximum number of desired features + * \param n_detected the number previously detected + */ + virtual void tooMany(int max, int n_detected) = 0; + /** are params maxed out or still valid? + * \return false if the parameters can't be adjusted any more + */ + virtual bool good() const = 0; + + virtual Ptr clone() const = 0; + + static Ptr create( const String& detectorType ); +}; +/** \brief an adaptively adjusting detector that iteratively detects until the desired number + * of features are detected. + * Beware that this is not thread safe - as the adjustment of parameters breaks the const + * of the detection routine... + * /TODO Make this const correct and thread safe + * + * sample usage: + //will create a detector that attempts to find 100 - 110 FAST Keypoints, and will at most run + //FAST feature detection 10 times until that number of keypoints are found + Ptr detector(new DynamicAdaptedFeatureDetector(new FastAdjuster(20,true),100, 110, 10)); + + */ +class CV_EXPORTS DynamicAdaptedFeatureDetector: public FeatureDetector +{ +public: + + /** \param adjuster an AdjusterAdapter that will do the detection and parameter adjustment + * \param max_features the maximum desired number of features + * \param max_iters the maximum number of times to try to adjust the feature detector params + * for the FastAdjuster this can be high, but with Star or Surf this can get time consuming + * \param min_features the minimum desired features + */ + DynamicAdaptedFeatureDetector( const Ptr& adjuster, int min_features=400, int max_features=500, int max_iters=5 ); + + virtual bool empty() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + +private: + DynamicAdaptedFeatureDetector& operator=(const DynamicAdaptedFeatureDetector&); + DynamicAdaptedFeatureDetector(const DynamicAdaptedFeatureDetector&); + + int escape_iters_; + int min_features_, max_features_; + const Ptr adjuster_; +}; + +/**\brief an adjust for the FAST detector. This will basically decrement or increment the + * threshold by 1 + */ +class CV_EXPORTS FastAdjuster: public AdjusterAdapter +{ +public: + /**\param init_thresh the initial threshold to start with, default = 20 + * \param nonmax whether to use non max or not for fast feature detection + */ + FastAdjuster(int init_thresh=20, bool nonmax=true, int min_thresh=1, int max_thresh=200); + + virtual void tooFew(int minv, int n_detected); + virtual void tooMany(int maxv, int n_detected); + virtual bool good() const; + + virtual Ptr clone() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + int thresh_; + bool nonmax_; + int init_thresh_, min_thresh_, max_thresh_; +}; + + +/** An adjuster for StarFeatureDetector, this one adjusts the responseThreshold for now + * TODO find a faster way to converge the parameters for Star - use CvStarDetectorParams + */ +class CV_EXPORTS StarAdjuster: public AdjusterAdapter +{ +public: + StarAdjuster(double initial_thresh=30.0, double min_thresh=2., double max_thresh=200.); + + virtual void tooFew(int minv, int n_detected); + virtual void tooMany(int maxv, int n_detected); + virtual bool good() const; + + virtual Ptr clone() const; + +protected: + virtual void detectImpl(InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + double thresh_, init_thresh_, min_thresh_, max_thresh_; +}; + +class CV_EXPORTS SurfAdjuster: public AdjusterAdapter +{ +public: + SurfAdjuster( double initial_thresh=400.f, double min_thresh=2, double max_thresh=1000 ); + + virtual void tooFew(int minv, int n_detected); + virtual void tooMany(int maxv, int n_detected); + virtual bool good() const; + + virtual Ptr clone() const; + +protected: + virtual void detectImpl( InputArray image, std::vector& keypoints, InputArray mask=noArray() ) const; + + double thresh_, init_thresh_, min_thresh_, max_thresh_; +}; + +CV_EXPORTS Mat windowedMatchingMask( const std::vector& keypoints1, const std::vector& keypoints2, + float maxDeltaX, float maxDeltaY ); + + + +/* + * OpponentColorDescriptorExtractor + * + * Adapts a descriptor extractor to compute descriptors in Opponent Color Space + * (refer to van de Sande et al., CGIV 2008 "Color Descriptors for Object Category Recognition"). + * Input RGB image is transformed in Opponent Color Space. Then unadapted descriptor extractor + * (set in constructor) computes descriptors on each of the three channel and concatenate + * them into a single color descriptor. + */ +class CV_EXPORTS OpponentColorDescriptorExtractor : public DescriptorExtractor +{ +public: + OpponentColorDescriptorExtractor( const Ptr& descriptorExtractor ); + + virtual void read( const FileNode& ); + virtual void write( FileStorage& ) const; + + virtual int descriptorSize() const; + virtual int descriptorType() const; + virtual int defaultNorm() const; + + virtual bool empty() const; + +protected: + virtual void computeImpl( InputArray image, std::vector& keypoints, OutputArray descriptors ) const; + + Ptr descriptorExtractor; +}; + +/* + * BRIEF Descriptor + */ +class CV_EXPORTS BriefDescriptorExtractor : public DescriptorExtractor +{ +public: + static const int PATCH_SIZE = 48; + static const int KERNEL_SIZE = 9; + + // bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes. + BriefDescriptorExtractor( int bytes = 32 ); + + virtual void read( const FileNode& ); + virtual void write( FileStorage& ) const; + + virtual int descriptorSize() const; + virtual int descriptorType() const; + virtual int defaultNorm() const; + + /// @todo read and write for brief + + AlgorithmInfo* info() const; + +protected: + virtual void computeImpl(InputArray image, std::vector& keypoints, OutputArray descriptors) const; + + typedef void(*PixelTestFn)(InputArray, const std::vector&, OutputArray); + + int bytes_; + PixelTestFn test_fn_; +}; + +/*! +KAZE implementation +*/ +class CV_EXPORTS_W KAZE : public Feature2D +{ +public: + CV_WRAP KAZE(); + CV_WRAP explicit KAZE(bool extended, bool upright); + + virtual ~KAZE(); + + // returns the descriptor size in bytes + int descriptorSize() const; + // returns the descriptor type + int descriptorType() const; + // returns the default norm type + int defaultNorm() const; + + AlgorithmInfo* info() const; + + // Compute the KAZE features on an image + void operator()(InputArray image, InputArray mask, std::vector& keypoints) const; + + // Compute the KAZE features and descriptors on an image + void operator()(InputArray image, InputArray mask, std::vector& keypoints, + OutputArray descriptors, bool useProvidedKeypoints = false) const; + +protected: + void detectImpl(InputArray image, std::vector& keypoints, InputArray mask) const; + void computeImpl(InputArray image, std::vector& keypoints, OutputArray descriptors) const; + + CV_PROP bool extended; + CV_PROP bool upright; +}; + +/*! +AKAZE implementation +*/ +class CV_EXPORTS_W AKAZE : public Feature2D +{ +public: + /// AKAZE Descriptor Type + enum DESCRIPTOR_TYPE { + DESCRIPTOR_KAZE_UPRIGHT = 2, ///< Upright descriptors, not invariant to rotation + DESCRIPTOR_KAZE = 3, + DESCRIPTOR_MLDB_UPRIGHT = 4, ///< Upright descriptors, not invariant to rotation + DESCRIPTOR_MLDB = 5 + }; + + CV_WRAP AKAZE(); + explicit AKAZE(DESCRIPTOR_TYPE descriptor_type, int descriptor_size = 0, int descriptor_channels = 3); + + virtual ~AKAZE(); + + // returns the descriptor size in bytes + int descriptorSize() const; + // returns the descriptor type + int descriptorType() const; + // returns the default norm type + int defaultNorm() const; + + // Compute the AKAZE features on an image + void operator()(InputArray image, InputArray mask, std::vector& keypoints) const; + + // Compute the AKAZE features and descriptors on an image + void operator()(InputArray image, InputArray mask, std::vector& keypoints, + OutputArray descriptors, bool useProvidedKeypoints = false) const; + + AlgorithmInfo* info() const; + +protected: + + void computeImpl(InputArray image, std::vector& keypoints, OutputArray descriptors) const; + void detectImpl(InputArray image, std::vector& keypoints, InputArray mask = noArray()) const; + + CV_PROP int descriptor; + CV_PROP int descriptor_channels; + CV_PROP int descriptor_size; + +}; +/****************************************************************************************\ +* Distance * +\****************************************************************************************/ + +template +struct CV_EXPORTS Accumulator +{ + typedef T Type; +}; + +template<> struct Accumulator { typedef float Type; }; +template<> struct Accumulator { typedef float Type; }; +template<> struct Accumulator { typedef float Type; }; +template<> struct Accumulator { typedef float Type; }; + +/* + * Squared Euclidean distance functor + */ +template +struct CV_EXPORTS SL2 +{ + enum { normType = NORM_L2SQR }; + typedef T ValueType; + typedef typename Accumulator::Type ResultType; + + ResultType operator()( const T* a, const T* b, int size ) const + { + return normL2Sqr(a, b, size); + } +}; + +/* + * Euclidean distance functor + */ +template +struct CV_EXPORTS L2 +{ + enum { normType = NORM_L2 }; + typedef T ValueType; + typedef typename Accumulator::Type ResultType; + + ResultType operator()( const T* a, const T* b, int size ) const + { + return (ResultType)std::sqrt((double)normL2Sqr(a, b, size)); + } +}; + +/* + * Manhattan distance (city block distance) functor + */ +template +struct CV_EXPORTS L1 +{ + enum { normType = NORM_L1 }; + typedef T ValueType; + typedef typename Accumulator::Type ResultType; + + ResultType operator()( const T* a, const T* b, int size ) const + { + return normL1(a, b, size); + } +}; + +/* + * Hamming distance functor - counts the bit differences between two strings - useful for the Brief descriptor + * bit count of A exclusive XOR'ed with B + */ +struct CV_EXPORTS Hamming +{ + enum { normType = NORM_HAMMING }; + typedef unsigned char ValueType; + typedef int ResultType; + + /** this will count the bits in a ^ b + */ + ResultType operator()( const unsigned char* a, const unsigned char* b, int size ) const + { + return normHamming(a, b, size); + } +}; + +typedef Hamming HammingLUT; + +template struct HammingMultilevel +{ + enum { normType = NORM_HAMMING + (cellsize>1) }; + typedef unsigned char ValueType; + typedef int ResultType; + + ResultType operator()( const unsigned char* a, const unsigned char* b, int size ) const + { + return normHamming(a, b, size, cellsize); + } +}; + +/****************************************************************************************\ +* DescriptorMatcher * +\****************************************************************************************/ +/* + * Abstract base class for matching two sets of descriptors. + */ +class CV_EXPORTS_W DescriptorMatcher : public Algorithm +{ +public: + virtual ~DescriptorMatcher(); + + /* + * Add descriptors to train descriptor collection. + * descriptors Descriptors to add. Each descriptors[i] is a descriptors set from one image. + */ + CV_WRAP virtual void add( InputArrayOfArrays descriptors ); + /* + * Get train descriptors collection. + */ + CV_WRAP const std::vector& getTrainDescriptors() const; + /* + * Clear train descriptors collection. + */ + CV_WRAP virtual void clear(); + + /* + * Return true if there are not train descriptors in collection. + */ + CV_WRAP virtual bool empty() const; + /* + * Return true if the matcher supports mask in match methods. + */ + CV_WRAP virtual bool isMaskSupported() const = 0; + + /* + * Train matcher (e.g. train flann index). + * In all methods to match the method train() is run every time before matching. + * Some descriptor matchers (e.g. BruteForceMatcher) have empty implementation + * of this method, other matchers really train their inner structures + * (e.g. FlannBasedMatcher trains flann::Index). So nonempty implementation + * of train() should check the class object state and do traing/retraining + * only if the state requires that (e.g. FlannBasedMatcher trains flann::Index + * if it has not trained yet or if new descriptors have been added to the train + * collection). + */ + CV_WRAP virtual void train(); + /* + * Group of methods to match descriptors from image pair. + * Method train() is run in this methods. + */ + // Find one best match for each query descriptor (if mask is empty). + CV_WRAP void match( InputArray queryDescriptors, InputArray trainDescriptors, + CV_OUT std::vector& matches, InputArray mask=noArray() ) const; + // Find k best matches for each query descriptor (in increasing order of distances). + // compactResult is used when mask is not empty. If compactResult is false matches + // vector will have the same size as queryDescriptors rows. If compactResult is true + // matches vector will not contain matches for fully masked out query descriptors. + CV_WRAP void knnMatch( InputArray queryDescriptors, InputArray trainDescriptors, + CV_OUT std::vector >& matches, int k, + InputArray mask=noArray(), bool compactResult=false ) const; + // Find best matches for each query descriptor which have distance less than + // maxDistance (in increasing order of distances). + void radiusMatch( InputArray queryDescriptors, InputArray trainDescriptors, + std::vector >& matches, float maxDistance, + InputArray mask=noArray(), bool compactResult=false ) const; + /* + * Group of methods to match descriptors from one image to image set. + * See description of similar methods for matching image pair above. + */ + CV_WRAP void match( InputArray queryDescriptors, CV_OUT std::vector& matches, + InputArrayOfArrays masks=noArray() ); + CV_WRAP void knnMatch( InputArray queryDescriptors, CV_OUT std::vector >& matches, int k, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + void radiusMatch( InputArray queryDescriptors, std::vector >& matches, float maxDistance, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + + // Reads matcher object from a file node + virtual void read( const FileNode& ); + // Writes matcher object to a file storage + virtual void write( FileStorage& ) const; + + // Clone the matcher. If emptyTrainData is false the method create deep copy of the object, i.e. copies + // both parameters and train data. If emptyTrainData is true the method create object copy with current parameters + // but with empty train data. + virtual Ptr clone( bool emptyTrainData=false ) const = 0; + + CV_WRAP static Ptr create( const String& descriptorMatcherType ); +protected: + /* + * Class to work with descriptors from several images as with one merged matrix. + * It is used e.g. in FlannBasedMatcher. + */ + class CV_EXPORTS DescriptorCollection + { + public: + DescriptorCollection(); + DescriptorCollection( const DescriptorCollection& collection ); + virtual ~DescriptorCollection(); + + // Vector of matrices "descriptors" will be merged to one matrix "mergedDescriptors" here. + void set( const std::vector& descriptors ); + virtual void clear(); + + const Mat& getDescriptors() const; + const Mat getDescriptor( int imgIdx, int localDescIdx ) const; + const Mat getDescriptor( int globalDescIdx ) const; + void getLocalIdx( int globalDescIdx, int& imgIdx, int& localDescIdx ) const; + + int size() const; + + protected: + Mat mergedDescriptors; + std::vector startIdxs; + }; + + // In fact the matching is implemented only by the following two methods. These methods suppose + // that the class object has been trained already. Public match methods call these methods + // after calling train(). + virtual void knnMatchImpl( InputArray queryDescriptors, std::vector >& matches, int k, + InputArrayOfArrays masks=noArray(), bool compactResult=false ) = 0; + virtual void radiusMatchImpl( InputArray queryDescriptors, std::vector >& matches, float maxDistance, + InputArrayOfArrays masks=noArray(), bool compactResult=false ) = 0; + + static bool isPossibleMatch( InputArray mask, int queryIdx, int trainIdx ); + static bool isMaskedOut( InputArrayOfArrays masks, int queryIdx ); + + static Mat clone_op( Mat m ) { return m.clone(); } + void checkMasks( InputArrayOfArrays masks, int queryDescriptorsCount ) const; + + // Collection of descriptors from train images. + std::vector trainDescCollection; + std::vector utrainDescCollection; +}; + +/* + * Brute-force descriptor matcher. + * + * For each descriptor in the first set, this matcher finds the closest + * descriptor in the second set by trying each one. + * + * For efficiency, BruteForceMatcher is templated on the distance metric. + * For float descriptors, a common choice would be cv::L2. + */ +class CV_EXPORTS_W BFMatcher : public DescriptorMatcher +{ +public: + CV_WRAP BFMatcher( int normType=NORM_L2, bool crossCheck=false ); + virtual ~BFMatcher() {} + + virtual bool isMaskSupported() const { return true; } + + virtual Ptr clone( bool emptyTrainData=false ) const; + + AlgorithmInfo* info() const; +protected: + virtual void knnMatchImpl( InputArray queryDescriptors, std::vector >& matches, int k, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + virtual void radiusMatchImpl( InputArray queryDescriptors, std::vector >& matches, float maxDistance, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + + int normType; + bool crossCheck; +}; + + +/* + * Flann based matcher + */ +class CV_EXPORTS_W FlannBasedMatcher : public DescriptorMatcher +{ +public: + CV_WRAP FlannBasedMatcher( const Ptr& indexParams=makePtr(), + const Ptr& searchParams=makePtr() ); + + virtual void add( InputArrayOfArrays descriptors ); + virtual void clear(); + + // Reads matcher object from a file node + virtual void read( const FileNode& ); + // Writes matcher object to a file storage + virtual void write( FileStorage& ) const; + + virtual void train(); + virtual bool isMaskSupported() const; + + virtual Ptr clone( bool emptyTrainData=false ) const; + + AlgorithmInfo* info() const; +protected: + static void convertToDMatches( const DescriptorCollection& descriptors, + const Mat& indices, const Mat& distances, + std::vector >& matches ); + + virtual void knnMatchImpl( InputArray queryDescriptors, std::vector >& matches, int k, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + virtual void radiusMatchImpl( InputArray queryDescriptors, std::vector >& matches, float maxDistance, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + + Ptr indexParams; + Ptr searchParams; + Ptr flannIndex; + + DescriptorCollection mergedDescriptors; + int addedDescCount; +}; + +/****************************************************************************************\ +* GenericDescriptorMatcher * +\****************************************************************************************/ +/* + * Abstract interface for a keypoint descriptor and matcher + */ +class GenericDescriptorMatcher; +typedef GenericDescriptorMatcher GenericDescriptorMatch; + +class CV_EXPORTS GenericDescriptorMatcher +{ +public: + GenericDescriptorMatcher(); + virtual ~GenericDescriptorMatcher(); + + /* + * Add train collection: images and keypoints from them. + * images A set of train images. + * ketpoints Keypoint collection that have been detected on train images. + * + * Keypoints for which a descriptor cannot be computed are removed. Such keypoints + * must be filtered in this method befor adding keypoints to train collection "trainPointCollection". + * If inheritor class need perform such prefiltering the method add() must be overloaded. + * In the other class methods programmer has access to the train keypoints by a constant link. + */ + virtual void add( InputArrayOfArrays images, + std::vector >& keypoints ); + + const std::vector& getTrainImages() const; + const std::vector >& getTrainKeypoints() const; + + /* + * Clear images and keypoints storing in train collection. + */ + virtual void clear(); + /* + * Returns true if matcher supports mask to match descriptors. + */ + virtual bool isMaskSupported() = 0; + /* + * Train some inner structures (e.g. flann index or decision trees). + * train() methods is run every time in matching methods. So the method implementation + * should has a check whether these inner structures need be trained/retrained or not. + */ + virtual void train(); + + /* + * Classifies query keypoints. + * queryImage The query image + * queryKeypoints Keypoints from the query image + * trainImage The train image + * trainKeypoints Keypoints from the train image + */ + // Classify keypoints from query image under one train image. + void classify( InputArray queryImage, std::vector& queryKeypoints, + InputArray trainImage, std::vector& trainKeypoints ) const; + // Classify keypoints from query image under train image collection. + void classify( InputArray queryImage, std::vector& queryKeypoints ); + + /* + * Group of methods to match keypoints from image pair. + * Keypoints for which a descriptor cannot be computed are removed. + * train() method is called here. + */ + // Find one best match for each query descriptor (if mask is empty). + void match( InputArray queryImage, std::vector& queryKeypoints, + InputArray trainImage, std::vector& trainKeypoints, + std::vector& matches, InputArray mask=noArray() ) const; + // Find k best matches for each query keypoint (in increasing order of distances). + // compactResult is used when mask is not empty. If compactResult is false matches + // vector will have the same size as queryDescriptors rows. + // If compactResult is true matches vector will not contain matches for fully masked out query descriptors. + void knnMatch( InputArray queryImage, std::vector& queryKeypoints, + InputArray trainImage, std::vector& trainKeypoints, + std::vector >& matches, int k, + InputArray mask=noArray(), bool compactResult=false ) const; + // Find best matches for each query descriptor which have distance less than maxDistance (in increasing order of distances). + void radiusMatch( InputArray queryImage, std::vector& queryKeypoints, + InputArray trainImage, std::vector& trainKeypoints, + std::vector >& matches, float maxDistance, + InputArray mask=noArray(), bool compactResult=false ) const; + /* + * Group of methods to match keypoints from one image to image set. + * See description of similar methods for matching image pair above. + */ + void match( InputArray queryImage, std::vector& queryKeypoints, + std::vector& matches, InputArrayOfArrays masks=noArray() ); + void knnMatch( InputArray queryImage, std::vector& queryKeypoints, + std::vector >& matches, int k, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + void radiusMatch(InputArray queryImage, std::vector& queryKeypoints, + std::vector >& matches, float maxDistance, + InputArrayOfArrays masks=noArray(), bool compactResult=false ); + + // Reads matcher object from a file node + virtual void read( const FileNode& fn ); + // Writes matcher object to a file storage + virtual void write( FileStorage& fs ) const; + + // Return true if matching object is empty (e.g. feature detector or descriptor matcher are empty) + virtual bool empty() const; + + // Clone the matcher. If emptyTrainData is false the method create deep copy of the object, i.e. copies + // both parameters and train data. If emptyTrainData is true the method create object copy with current parameters + // but with empty train data. + virtual Ptr clone( bool emptyTrainData=false ) const = 0; + + static Ptr create( const String& genericDescritptorMatcherType, + const String ¶msFilename=String() ); + +protected: + // In fact the matching is implemented only by the following two methods. These methods suppose + // that the class object has been trained already. Public match methods call these methods + // after calling train(). + virtual void knnMatchImpl( InputArray queryImage, std::vector& queryKeypoints, + std::vector >& matches, int k, + InputArrayOfArrays masks, bool compactResult ) = 0; + virtual void radiusMatchImpl( InputArray queryImage, std::vector& queryKeypoints, + std::vector >& matches, float maxDistance, + InputArrayOfArrays masks, bool compactResult ) = 0; + /* + * A storage for sets of keypoints together with corresponding images and class IDs + */ + class CV_EXPORTS KeyPointCollection + { + public: + KeyPointCollection(); + KeyPointCollection( const KeyPointCollection& collection ); + void add( const std::vector& images, const std::vector >& keypoints ); + void clear(); + + // Returns the total number of keypoints in the collection + size_t keypointCount() const; + size_t imageCount() const; + + const std::vector >& getKeypoints() const; + const std::vector& getKeypoints( int imgIdx ) const; + const KeyPoint& getKeyPoint( int imgIdx, int localPointIdx ) const; + const KeyPoint& getKeyPoint( int globalPointIdx ) const; + void getLocalIdx( int globalPointIdx, int& imgIdx, int& localPointIdx ) const; + + const std::vector& getImages() const; + const Mat& getImage( int imgIdx ) const; + + protected: + int pointCount; + + std::vector images; + std::vector > keypoints; + // global indices of the first points in each image, startIndices.size() = keypoints.size() + std::vector startIndices; + + private: + static Mat clone_op( Mat m ) { return m.clone(); } + }; + + KeyPointCollection trainPointCollection; +}; + + +/****************************************************************************************\ +* VectorDescriptorMatcher * +\****************************************************************************************/ + +/* + * A class used for matching descriptors that can be described as vectors in a finite-dimensional space + */ +class VectorDescriptorMatcher; +typedef VectorDescriptorMatcher VectorDescriptorMatch; + +class CV_EXPORTS VectorDescriptorMatcher : public GenericDescriptorMatcher +{ +public: + VectorDescriptorMatcher( const Ptr& extractor, const Ptr& matcher ); + virtual ~VectorDescriptorMatcher(); + + virtual void add( InputArrayOfArrays imgCollection, + std::vector >& pointCollection ); + + virtual void clear(); + + virtual void train(); + + virtual bool isMaskSupported(); + + virtual void read( const FileNode& fn ); + virtual void write( FileStorage& fs ) const; + virtual bool empty() const; + + virtual Ptr clone( bool emptyTrainData=false ) const; + +protected: + virtual void knnMatchImpl( InputArray queryImage, std::vector& queryKeypoints, + std::vector >& matches, int k, + InputArrayOfArrays masks, bool compactResult ); + virtual void radiusMatchImpl( InputArray queryImage, std::vector& queryKeypoints, + std::vector >& matches, float maxDistance, + InputArrayOfArrays masks, bool compactResult ); + + Ptr extractor; + Ptr matcher; +}; + +/****************************************************************************************\ +* Drawing functions * +\****************************************************************************************/ +struct CV_EXPORTS DrawMatchesFlags +{ + enum{ DEFAULT = 0, // Output image matrix will be created (Mat::create), + // i.e. existing memory of output image may be reused. + // Two source image, matches and single keypoints will be drawn. + // For each keypoint only the center point will be drawn (without + // the circle around keypoint with keypoint size and orientation). + DRAW_OVER_OUTIMG = 1, // Output image matrix will not be created (Mat::create). + // Matches will be drawn on existing content of output image. + NOT_DRAW_SINGLE_POINTS = 2, // Single keypoints will not be drawn. + DRAW_RICH_KEYPOINTS = 4 // For each keypoint the circle around keypoint with keypoint size and + // orientation will be drawn. + }; +}; + +// Draw keypoints. +CV_EXPORTS_W void drawKeypoints( InputArray image, const std::vector& keypoints, InputOutputArray outImage, + const Scalar& color=Scalar::all(-1), int flags=DrawMatchesFlags::DEFAULT ); + +// Draws matches of keypints from two images on output image. +CV_EXPORTS_W void drawMatches( InputArray img1, const std::vector& keypoints1, + InputArray img2, const std::vector& keypoints2, + const std::vector& matches1to2, InputOutputArray outImg, + const Scalar& matchColor=Scalar::all(-1), const Scalar& singlePointColor=Scalar::all(-1), + const std::vector& matchesMask=std::vector(), int flags=DrawMatchesFlags::DEFAULT ); + +CV_EXPORTS_AS(drawMatchesKnn) void drawMatches( InputArray img1, const std::vector& keypoints1, + InputArray img2, const std::vector& keypoints2, + const std::vector >& matches1to2, InputOutputArray outImg, + const Scalar& matchColor=Scalar::all(-1), const Scalar& singlePointColor=Scalar::all(-1), + const std::vector >& matchesMask=std::vector >(), int flags=DrawMatchesFlags::DEFAULT ); + +/****************************************************************************************\ +* Functions to evaluate the feature detectors and [generic] descriptor extractors * +\****************************************************************************************/ + +CV_EXPORTS void evaluateFeatureDetector( const Mat& img1, const Mat& img2, const Mat& H1to2, + std::vector* keypoints1, std::vector* keypoints2, + float& repeatability, int& correspCount, + const Ptr& fdetector=Ptr() ); + +CV_EXPORTS void computeRecallPrecisionCurve( const std::vector >& matches1to2, + const std::vector >& correctMatches1to2Mask, + std::vector& recallPrecisionCurve ); + +CV_EXPORTS float getRecall( const std::vector& recallPrecisionCurve, float l_precision ); +CV_EXPORTS int getNearestPoint( const std::vector& recallPrecisionCurve, float l_precision ); + +CV_EXPORTS void evaluateGenericDescriptorMatcher( const Mat& img1, const Mat& img2, const Mat& H1to2, + std::vector& keypoints1, std::vector& keypoints2, + std::vector >* matches1to2, std::vector >* correctMatches1to2Mask, + std::vector& recallPrecisionCurve, + const Ptr& dmatch=Ptr() ); + + +/****************************************************************************************\ +* Bag of visual words * +\****************************************************************************************/ +/* + * Abstract base class for training of a 'bag of visual words' vocabulary from a set of descriptors + */ - class CV_EXPORTS BOWTrainer ++class CV_EXPORTS_W BOWTrainer +{ +public: + BOWTrainer(); + virtual ~BOWTrainer(); + - void add( const Mat& descriptors ); - const std::vector& getDescriptors() const; - int descriptorsCount() const; ++ CV_WRAP void add( const Mat& descriptors ); ++ CV_WRAP const std::vector& getDescriptors() const; ++ CV_WRAP int descriptorsCount() const; + - virtual void clear(); ++ CV_WRAP virtual void clear(); + + /* + * Train visual words vocabulary, that is cluster training descriptors and + * compute cluster centers. + * Returns cluster centers. + * + * descriptors Training descriptors computed on images keypoints. + */ - virtual Mat cluster() const = 0; - virtual Mat cluster( const Mat& descriptors ) const = 0; ++ CV_WRAP virtual Mat cluster() const = 0; ++ CV_WRAP virtual Mat cluster( const Mat& descriptors ) const = 0; + +protected: + std::vector descriptors; + int size; +}; + +/* + * This is BOWTrainer using cv::kmeans to get vocabulary. + */ - class CV_EXPORTS BOWKMeansTrainer : public BOWTrainer ++class CV_EXPORTS_W BOWKMeansTrainer : public BOWTrainer +{ +public: - BOWKMeansTrainer( int clusterCount, const TermCriteria& termcrit=TermCriteria(), ++ CV_WRAP BOWKMeansTrainer( int clusterCount, const TermCriteria& termcrit=TermCriteria(), + int attempts=3, int flags=KMEANS_PP_CENTERS ); + virtual ~BOWKMeansTrainer(); + + // Returns trained vocabulary (i.e. cluster centers). - virtual Mat cluster() const; - virtual Mat cluster( const Mat& descriptors ) const; ++ CV_WRAP virtual Mat cluster() const; ++ CV_WRAP virtual Mat cluster( const Mat& descriptors ) const; + +protected: + + int clusterCount; + TermCriteria termcrit; + int attempts; + int flags; +}; + +/* + * Class to compute image descriptor using bag of visual words. + */ - class CV_EXPORTS BOWImgDescriptorExtractor ++class CV_EXPORTS_W BOWImgDescriptorExtractor +{ +public: - BOWImgDescriptorExtractor( const Ptr& dextractor, ++ CV_WRAP BOWImgDescriptorExtractor( const Ptr& dextractor, + const Ptr& dmatcher ); + BOWImgDescriptorExtractor( const Ptr& dmatcher ); + virtual ~BOWImgDescriptorExtractor(); + - void setVocabulary( const Mat& vocabulary ); - const Mat& getVocabulary() const; ++ CV_WRAP void setVocabulary( const Mat& vocabulary ); ++ CV_WRAP const Mat& getVocabulary() const; + void compute( InputArray image, std::vector& keypoints, OutputArray imgDescriptor, + std::vector >* pointIdxsOfClusters=0, Mat* descriptors=0 ); + void compute( InputArray keypointDescriptors, OutputArray imgDescriptor, + std::vector >* pointIdxsOfClusters=0 ); + // compute() is not constant because DescriptorMatcher::match is not constant + - int descriptorSize() const; - int descriptorType() const; ++ CV_WRAP_AS(compute) void compute2( const Mat& image, std::vector& keypoints, CV_OUT Mat& imgDescriptor ) ++ { compute(image,keypoints,imgDescriptor); } ++ ++ CV_WRAP int descriptorSize() const; ++ CV_WRAP int descriptorType() const; + +protected: + Mat vocabulary; + Ptr dextractor; + Ptr dmatcher; +}; + +} /* namespace cv */ + +#endif diff --cc modules/highgui/src/loadsave.cpp index cdcaa23,81c708a..fc942ef --- a/modules/highgui/src/loadsave.cpp +++ b/modules/highgui/src/loadsave.cpp @@@ -138,10 -137,10 +138,10 @@@ static ImageDecoder findDecoder( const maxlen = std::max(maxlen, len); } - string signature(maxlen, ' '); ++ String signature(maxlen, ' '); size_t bufSize = buf.rows*buf.cols*buf.elemSize(); maxlen = std::min(maxlen, bufSize); - String signature(maxlen, ' '); - memcpy( &signature[0], buf.data, maxlen ); + memcpy( (void*)signature.c_str(), buf.data, maxlen ); for( i = 0; i < codecs.decoders.size(); i++ ) { diff --cc modules/imgproc/src/pyramids.cpp index 1e4f89c,4000167..1430731 --- a/modules/imgproc/src/pyramids.cpp +++ b/modules/imgproc/src/pyramids.cpp @@@ -502,14 -403,12 +502,16 @@@ static bool ocl_pyrUp( InputArray _src void cv::pyrDown( InputArray _src, OutputArray _dst, const Size& _dsz, int borderType ) { + CV_Assert(borderType != BORDER_CONSTANT); + + CV_OCL_RUN(_src.dims() <= 2 && _dst.isUMat(), + ocl_pyrDown(_src, _dst, _dsz, borderType)) + Mat src = _src.getMat(); - Size dsz = _dsz == Size() ? Size((src.cols + 1)/2, (src.rows + 1)/2) : _dsz; + Size dsz = _dsz.area() == 0 ? Size((src.cols + 1)/2, (src.rows + 1)/2) : _dsz; _dst.create( dsz, src.type() ); Mat dst = _dst.getMat(); + int depth = src.depth(); #ifdef HAVE_TEGRA_OPTIMIZATION if(borderType == BORDER_DEFAULT && tegra::pyrDown(src, dst)) @@@ -571,14 -435,12 +573,16 @@@ void cv::pyrUp( InputArray _src, OutputArray _dst, const Size& _dsz, int borderType ) { + CV_Assert(borderType == BORDER_DEFAULT); + + CV_OCL_RUN(_src.dims() <= 2 && _dst.isUMat(), + ocl_pyrUp(_src, _dst, _dsz, borderType)) + Mat src = _src.getMat(); - Size dsz = _dsz == Size() ? Size(src.cols*2, src.rows*2) : _dsz; + Size dsz = _dsz.area() == 0 ? Size(src.cols*2, src.rows*2) : _dsz; _dst.create( dsz, src.type() ); Mat dst = _dst.getMat(); + int depth = src.depth(); #ifdef HAVE_TEGRA_OPTIMIZATION if(borderType == BORDER_DEFAULT && tegra::pyrUp(src, dst)) @@@ -640,16 -467,8 +644,18 @@@ void cv::buildPyramid( InputArray _src, OutputArrayOfArrays _dst, int maxlevel, int borderType ) { + CV_Assert(borderType != BORDER_CONSTANT); + + if (_src.dims() <= 2 && _dst.isUMatVector()) + { + UMat src = _src.getUMat(); + _dst.create( maxlevel + 1, 1, 0 ); + _dst.getUMatRef(0) = src; + for( int i = 1; i <= maxlevel; i++ ) + pyrDown( _dst.getUMatRef(i-1), _dst.getUMatRef(i), Size(), borderType ); + return; + } + Mat src = _src.getMat(); _dst.create( maxlevel + 1, 1, 0 ); _dst.getMatRef(0) = src;