diff --git a/doc/tutorials/introduction/android_binary_package/android_dev_intro.rst b/doc/tutorials/introduction/android_binary_package/android_dev_intro.rst index 9545bee28..8c470925f 100644 --- a/doc/tutorials/introduction/android_binary_package/android_dev_intro.rst +++ b/doc/tutorials/introduction/android_binary_package/android_dev_intro.rst @@ -5,7 +5,7 @@ Introduction into Android Development ************************************* -This guide was designed to help you in learning Android development basics and seting up your +This guide was designed to help you in learning Android development basics and setting up your working environment quickly. It was written with Windows 7 in mind, though it would work with Linux (Ubuntu), Mac OS X and any other OS supported by Android SDK. diff --git a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst index b484e8d03..691a6b9eb 100644 --- a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst +++ b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst @@ -862,7 +862,7 @@ is minimized. If the parameter ``method`` is set to the default value 0, the fun uses all the point pairs to compute an initial homography estimate with a simple least-squares scheme. However, if not all of the point pairs ( -:math:`srcPoints_i`,:math:`dstPoints_i` ) fit the rigid perspective transformation (that is, there +:math:`srcPoints_i`, :math:`dstPoints_i` ) fit the rigid perspective transformation (that is, there are some outliers), this initial estimate will be poor. In this case, you can use one of the two robust methods. Both methods, ``RANSAC`` and ``LMeDS`` , try many different random subsets of the corresponding point pairs (of four pairs each), estimate @@ -885,7 +885,7 @@ if there are no outliers and the noise is rather small, use the default method ( The function is used to find initial intrinsic and extrinsic matrices. Homography matrix is determined up to a scale. Thus, it is normalized so that -:math:`h_{33}=1` . +:math:`h_{33}=1`. Note that whenever an H matrix cannot be estimated, an empty one will be returned. .. seealso:: @@ -1500,6 +1500,364 @@ The function reconstructs 3-dimensional points (in homogeneous coordinates) by u :ocv:func:`reprojectImageTo3D` +fisheye +---------- + +The methods in this namespace use a so-called fisheye camera model. :: + + namespace fisheye + { + //! projects 3D points using fisheye model + void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, + InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); + + //! projects points using fisheye model + 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 + void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0); + + //! undistorts 2D points using fisheye model + 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 + 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. + 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 + 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 + 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 + 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 calibration + 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)); + }; + + +Definitions: +Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +The coordinate vector of P in the camera reference frame is: + +.. class:: center +.. math:: + + Xc = R X + T + +where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +call x, y and z the 3 coordinates of Xc: + +.. class:: center +.. math:: + x = Xc_1 \\ + y = Xc_2 \\ + z = Xc_3 + +The pinehole projection coordinates of P is [a; b] where + +.. class:: center +.. math:: + + a = x / z \ and \ b = y / z \\ + r^2 = a^2 + b^2 \\ + \theta = atan(r) + +Fisheye distortion: + +.. class:: center +.. math:: + + \theta_d = \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8) + +The distorted point coordinates are [x'; y'] where + +..class:: center +.. math:: + + x' = (\theta_d / r) x \\ + y' = (\theta_d / r) y + +Finally, convertion into pixel coordinates: The final pixel coordinates vector [u; v] where: + +.. class:: center +.. math:: + + u = f_x (x' + \alpha y') + c_x \\ + v = f_y yy + c_y + +fisheye::projectPoints +--------------------------- +Projects points using fisheye model + +.. ocv:function:: void fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()) + +.. ocv:function:: void fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec, InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()) + + :param objectPoints: Array of object points, 1xN/Nx1 3-channel (or ``vector`` ), where N is the number of points in the view. + + :param rvec: Rotation vector. See :ocv:func:`Rodrigues` for details. + + :param tvec: Translation vector. + + :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`. + + :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param alpha: The skew coefficient. + + :param imagePoints: Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or ``vector``. + + :param jacobian: Optional output 2Nx15 jacobian matrix of derivatives of image points with respect to components of the focal lengths, coordinates of the principal point, distortion coefficients, rotation vector, translation vector, and the skew. In the old interface different components of the jacobian are returned via different output parameters. + +The function computes projections of 3D points to the image plane given intrinsic and extrinsic camera parameters. Optionally, the function computes Jacobians - matrices of partial derivatives of image points coordinates (as functions of all the input parameters) with respect to the particular parameters, intrinsic and/or extrinsic. + +fisheye::distortPoints +------------------------- +Distorts 2D points using fisheye model. + +.. ocv:function:: void fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0) + + :param undistorted: Array of object points, 1xN/Nx1 2-channel (or ``vector`` ), where N is the number of points in the view. + + :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`. + + :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param alpha: The skew coefficient. + + :param distorted: Output array of image points, 1xN/Nx1 2-channel, or ``vector`` . + +fisheye::undistortPoints +----------------------------- +Undistorts 2D points using fisheye model + +.. ocv:function:: void fisheye::undistortPoints(InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray()) + + :param distorted: Array of object points, 1xN/Nx1 2-channel (or ``vector`` ), where N is the number of points in the view. + + :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`. + + :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param R: Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel + + :param P: New camera matrix (3x3) or new projection matrix (3x4) + + :param undistorted: Output array of image points, 1xN/Nx1 2-channel, or ``vector`` . + + +fisheye::initUndistortRectifyMap +------------------------------------- +Computes 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. + +.. ocv:function:: void fisheye::initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P, const cv::Size& size, int m1type, OutputArray map1, OutputArray map2) + + :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`. + + :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param R: Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel + + :param P: New camera matrix (3x3) or new projection matrix (3x4) + + :param size: Undistorted image size. + + :param m1type: Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps() for details. + + :param map1: The first output map. + + :param map2: The second output map. + +fisheye::undistortImage +----------------------- +Transforms an image to compensate for fisheye lens distortion. + +.. ocv:function:: void fisheye::undistortImage(InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size()) + + :param distorted: image with fisheye lens distortion. + + :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`. + + :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param Knew: Camera matrix of the distorted image. By default, it is the identity matrix but you may additionally scale and shift the result by using a different matrix. + + :param undistorted: Output image with compensated fisheye lens distortion. + +The function transforms an image to compensate radial and tangential lens distortion. + +The function is simply a combination of +:ocv:func:`fisheye::initUndistortRectifyMap` (with unity ``R`` ) and +:ocv:func:`remap` (with bilinear interpolation). See the former function for details of the transformation being performed. + +See below the results of undistortImage. + * a\) result of :ocv:func:`undistort` of perspective camera model (all possible coefficients (k_1, k_2, k_3, k_4, k_5, k_6) of distortion were optimized under calibration) + * b\) result of :ocv:func:`fisheye::undistortImage` of fisheye camera model (all possible coefficients (k_1, k_2, k_3, k_4) of fisheye distortion were optimized under calibration) + * c\) original image was captured with fisheye lens + +Pictures a) and b) almost the same. But if we consider points of image located far from the center of image, we can notice that on image a) these points are distorted. + +.. image:: pics/fisheye_undistorted.jpg + + +fisheye::estimateNewCameraMatrixForUndistortRectify +---------------------------------------------------------- +Estimates new camera matrix for undistortion or rectification. + +.. ocv:function:: void fisheye::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) + + :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`. + + :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param R: Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel + + :param P: New camera matrix (3x3) or new projection matrix (3x4) + + :param balance: Sets the new focal length in range between the min focal length and the max focal length. Balance is in range of [0, 1]. + + :param fov_scale: Divisor for new focal length. + +fisheye::stereoRectify +------------------------------ +Stereo rectification for fisheye camera model + +.. ocv:function:: void 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 = Size(), double balance = 0.0, double fov_scale = 1.0) + + :param K1: First camera matrix. + + :param K2: Second camera matrix. + + :param D1: First camera distortion parameters. + + :param D2: Second camera distortion parameters. + + :param imageSize: Size of the image used for stereo calibration. + + :param rotation: Rotation matrix between the coordinate systems of the first and the second cameras. + + :param tvec: Translation vector between coordinate systems of the cameras. + + :param R1: Output 3x3 rectification transform (rotation matrix) for the first camera. + + :param R2: Output 3x3 rectification transform (rotation matrix) for the second camera. + + :param P1: Output 3x4 projection matrix in the new (rectified) coordinate systems for the first camera. + + :param P2: Output 3x4 projection matrix in the new (rectified) coordinate systems for the second camera. + + :param Q: Output :math:`4 \times 4` disparity-to-depth mapping matrix (see :ocv:func:`reprojectImageTo3D` ). + + :param flags: Operation flags that may be zero or ``CV_CALIB_ZERO_DISPARITY`` . If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. And if the flag is not set, the function may still shift the images in the horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the useful image area. + + :param alpha: Free scaling parameter. If it is -1 or absent, the function performs the default scaling. Otherwise, the parameter should be between 0 and 1. ``alpha=0`` means that the rectified images are zoomed and shifted so that only valid pixels are visible (no black areas after rectification). ``alpha=1`` means that the rectified image is decimated and shifted so that all the pixels from the original images from the cameras are retained in the rectified images (no source image pixels are lost). Obviously, any intermediate value yields an intermediate result between those two extreme cases. + + :param newImageSize: New image resolution after rectification. The same size should be passed to :ocv:func:`initUndistortRectifyMap` (see the ``stereo_calib.cpp`` sample in OpenCV samples directory). When (0,0) is passed (default), it is set to the original ``imageSize`` . Setting it to larger value can help you preserve details in the original image, especially when there is a big radial distortion. + + :param roi1: Optional output rectangles inside the rectified images where all the pixels are valid. If ``alpha=0`` , the ROIs cover the whole images. Otherwise, they are likely to be smaller (see the picture below). + + :param roi2: Optional output rectangles inside the rectified images where all the pixels are valid. If ``alpha=0`` , the ROIs cover the whole images. Otherwise, they are likely to be smaller (see the picture below). + + :param balance: Sets the new focal length in range between the min focal length and the max focal length. Balance is in range of [0, 1]. + + :param fov_scale: Divisor for new focal length. + + + +fisheye::calibrate +---------------------------- +Performs camera calibaration + +.. ocv:function:: double fisheye::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)) + + :param objectPoints: vector of vectors of calibration pattern points in the calibration pattern coordinate space. + + :param imagePoints: vector of vectors of the projections of calibration pattern points. ``imagePoints.size()`` and ``objectPoints.size()`` and ``imagePoints[i].size()`` must be equal to ``objectPoints[i].size()`` for each ``i``. + + :param image_size: Size of the image used only to initialize the intrinsic camera matrix. + + :param K: Output 3x3 floating-point camera matrix :math:`A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}` . If ``fisheye::CALIB_USE_INTRINSIC_GUESS``/ is specified, some or all of ``fx, fy, cx, cy`` must be initialized before calling the function. + + :param D: Output vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`. + + :param rvecs: Output vector of rotation vectors (see :ocv:func:`Rodrigues` ) estimated for each pattern view. That is, each k-th rotation vector together with the corresponding k-th translation vector (see the next output parameter description) brings the calibration pattern from the model coordinate space (in which object points are specified) to the world coordinate space, that is, a real position of the calibration pattern in the k-th pattern view (k=0.. *M* -1). + + :param tvecs: Output vector of translation vectors estimated for each pattern view. + + :param flags: Different flags that may be zero or a combination of the following values: + + * **fisheye::CALIB_USE_INTRINSIC_GUESS** ``cameraMatrix`` contains valid initial values of ``fx, fy, cx, cy`` that are optimized further. Otherwise, ``(cx, cy)`` is initially set to the image center ( ``imageSize`` is used), and focal distances are computed in a least-squares fashion. + + * **fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration of intrinsic optimization. + + * **fisheye::CALIB_CHECK_COND** The functions will check validity of condition number. + + * **fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero. + + * **fisheye::CALIB_FIX_K1..4** Selected distortion coefficients are set to zeros and stay zero. + + :param criteria: Termination criteria for the iterative optimization algorithm. + + +fisheye::stereoCalibrate +---------------------------- +Performs stereo calibration + +.. ocv:function:: double fisheye::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)) + + :param objectPoints: Vector of vectors of the calibration pattern points. + + :param imagePoints1: Vector of vectors of the projections of the calibration pattern points, observed by the first camera. + + :param imagePoints2: Vector of vectors of the projections of the calibration pattern points, observed by the second camera. + + :param K1: Input/output first camera matrix: :math:`\vecthreethree{f_x^{(j)}}{0}{c_x^{(j)}}{0}{f_y^{(j)}}{c_y^{(j)}}{0}{0}{1}` , :math:`j = 0,\, 1` . If any of ``fisheye::CALIB_USE_INTRINSIC_GUESS`` , ``fisheye::CV_CALIB_FIX_INTRINSIC`` are specified, some or all of the matrix components must be initialized. + + :param D1: Input/output vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)` of 4 elements. + + :param K2: Input/output second camera matrix. The parameter is similar to ``K1`` . + + :param D2: Input/output lens distortion coefficients for the second camera. The parameter is similar to ``D1`` . + + :param imageSize: Size of the image used only to initialize intrinsic camera matrix. + + :param R: Output rotation matrix between the 1st and the 2nd camera coordinate systems. + + :param T: Output translation vector between the coordinate systems of the cameras. + + :param flags: Different flags that may be zero or a combination of the following values: + + * **fisheye::CV_CALIB_FIX_INTRINSIC** Fix ``K1, K2?`` and ``D1, D2?`` so that only ``R, T`` matrices are estimated. + + * **fisheye::CALIB_USE_INTRINSIC_GUESS** ``K1, K2`` contains valid initial values of ``fx, fy, cx, cy`` that are optimized further. Otherwise, ``(cx, cy)`` is initially set to the image center (``imageSize`` is used), and focal distances are computed in a least-squares fashion. + + * **fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration of intrinsic optimization. + + * **fisheye::CALIB_CHECK_COND** The functions will check validity of condition number. + + * **fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero. + + * **fisheye::CALIB_FIX_K1..4** Selected distortion coefficients are set to zeros and stay zero. + + :param criteria: Termination criteria for the iterative optimization algorithm. .. [BT98] Birchfield, S. and Tomasi, C. A pixel dissimilarity measure that is insensitive to image sampling. IEEE Transactions on Pattern Analysis and Machine Intelligence. 1998. diff --git a/modules/calib3d/doc/pics/fisheye_undistorted.jpg b/modules/calib3d/doc/pics/fisheye_undistorted.jpg new file mode 100644 index 000000000..bfdc9afd5 Binary files /dev/null and b/modules/calib3d/doc/pics/fisheye_undistorted.jpg differ diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 8b9b69c3a..c7b538808 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -46,6 +46,7 @@ #include "opencv2/core.hpp" #include "opencv2/features2d.hpp" +#include "opencv2/core/affine.hpp" namespace cv { @@ -411,6 +412,66 @@ CV_EXPORTS_W Ptr createStereoSGBM(int minDisparity, int numDispariti 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 --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp new file mode 100644 index 000000000..ce4144bbb --- /dev/null +++ b/modules/calib3d/src/fisheye.cpp @@ -0,0 +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 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 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, 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); + } + } + + 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 --git a/modules/calib3d/src/fisheye.hpp b/modules/calib3d/src/fisheye.hpp new file mode 100644 index 000000000..82c9f3459 --- /dev/null +++ b/modules/calib3d/src/fisheye.hpp @@ -0,0 +1,61 @@ +#ifndef FISHEYE_INTERNAL_H +#define FISHEYE_INTERNAL_H +#include "precomp.hpp" + +namespace cv { namespace internal { + +struct CV_EXPORTS IntrinsicParams +{ + Vec2d f; + Vec2d c; + Vec4d k; + double alpha; + std::vector isEstimate; + + IntrinsicParams(); + IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0); + IntrinsicParams operator+(const Mat& a); + IntrinsicParams& operator =(const Mat& a); + void Init(const cv::Vec2d& f, const cv::Vec2d& c, const cv::Vec4d& k = Vec4d(0,0,0,0), const double& alpha = 0); +}; + +void projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints, + cv::InputArray _rvec,cv::InputArray _tvec, + const IntrinsicParams& param, cv::OutputArray jacobian); + +void ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec, + Mat& tvec, Mat& J, const int MaxIter, + const IntrinsicParams& param, const double thresh_cond); +CV_EXPORTS Mat ComputeHomography(Mat m, Mat M); + +CV_EXPORTS Mat NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param); + +void InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk); + +void CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + const IntrinsicParams& param, const int check_cond, + const double thresh_cond, InputOutputArray omc, InputOutputArray Tc); + +void 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_EXPORTS void 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); + +void dAB(cv::InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB); + +void JRodriguesMatlab(const Mat& src, Mat& dst); + +void 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); + +double median(const Mat& row); + +Vec3d median3d(InputArray m); + +}} + +#endif diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp new file mode 100644 index 000000000..8318f8216 --- /dev/null +++ b/modules/calib3d/test/test_fisheye.cpp @@ -0,0 +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 "../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(), "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::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::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::Scalar(0, 255, 0)); + + return merged; +} diff --git a/modules/core/include/opencv2/core/core_c.h b/modules/core/include/opencv2/core/core_c.h index df25e73fb..81720fa2d 100644 --- a/modules/core/include/opencv2/core/core_c.h +++ b/modules/core/include/opencv2/core/core_c.h @@ -1122,7 +1122,7 @@ CV_INLINE CvSetElem* cvSetNew( CvSet* set_header ) set_header->active_count++; } else - cvSetAdd( set_header, NULL, (CvSetElem**)&elem ); + cvSetAdd( set_header, NULL, &elem ); return elem; } diff --git a/modules/core/include/opencv2/core/mat.inl.hpp b/modules/core/include/opencv2/core/mat.inl.hpp index d463eec67..5f09d4f7d 100644 --- a/modules/core/include/opencv2/core/mat.inl.hpp +++ b/modules/core/include/opencv2/core/mat.inl.hpp @@ -643,7 +643,8 @@ inline void Mat::release() deallocate(); u = NULL; data = datastart = dataend = datalimit = 0; - size.p[0] = 0; + for(int i = 0; i < dims; i++) + size.p[i] = 0; } inline @@ -2733,7 +2734,7 @@ SparseMatConstIterator_<_Tp>& SparseMatConstIterator_<_Tp>::operator ++() template inline SparseMatConstIterator_<_Tp> SparseMatConstIterator_<_Tp>::operator ++(int) { - SparseMatConstIterator it = *this; + SparseMatConstIterator_<_Tp> it = *this; SparseMatConstIterator::operator ++(); return it; } @@ -2785,7 +2786,7 @@ SparseMatIterator_<_Tp>& SparseMatIterator_<_Tp>::operator ++() template inline SparseMatIterator_<_Tp> SparseMatIterator_<_Tp>::operator ++(int) { - SparseMatIterator it = *this; + SparseMatIterator_<_Tp> it = *this; SparseMatConstIterator::operator ++(); return it; } diff --git a/modules/core/include/opencv2/core/types_c.h b/modules/core/include/opencv2/core/types_c.h index 161a4bd10..ecf5f597c 100644 --- a/modules/core/include/opencv2/core/types_c.h +++ b/modules/core/include/opencv2/core/types_c.h @@ -548,7 +548,7 @@ CV_INLINE void cvmSet( CvMat* mat, int row, int col, double value ) else { assert( type == CV_64FC1 ); - ((double*)(void*)(mat->data.ptr + (size_t)mat->step*row))[col] = (double)value; + ((double*)(void*)(mat->data.ptr + (size_t)mat->step*row))[col] = value; } } diff --git a/modules/features2d/include/opencv2/features2d.hpp b/modules/features2d/include/opencv2/features2d.hpp index 9f46ee23e..7be92484f 100644 --- a/modules/features2d/include/opencv2/features2d.hpp +++ b/modules/features2d/include/opencv2/features2d.hpp @@ -1543,17 +1543,17 @@ CV_EXPORTS void evaluateGenericDescriptorMatcher( const Mat& img1, const Mat& im /* * 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 @@ -1562,8 +1562,8 @@ public: * * 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; @@ -1573,16 +1573,16 @@ protected: /* * 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: @@ -1595,24 +1595,27 @@ protected: /* * 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; diff --git a/modules/flann/include/opencv2/flann/dist.h b/modules/flann/include/opencv2/flann/dist.h index 6a03b48a1..e7459f3ce 100644 --- a/modules/flann/include/opencv2/flann/dist.h +++ b/modules/flann/include/opencv2/flann/dist.h @@ -837,6 +837,66 @@ typename Distance::ResultType ensureSquareDistance( typename Distance::ResultTyp return dummy( dist ); } + +/* + * ...and a template to ensure the user that he will process the normal distance, + * and not squared distance, without loosing processing time calling sqrt(ensureSquareDistance) + * that will result in doing actually sqrt(dist*dist) for L1 distance for instance. + */ +template +struct simpleDistance +{ + typedef typename Distance::ResultType ResultType; + ResultType operator()( ResultType dist ) { return dist; } +}; + + +template +struct simpleDistance, ElementType> +{ + typedef typename L2_Simple::ResultType ResultType; + ResultType operator()( ResultType dist ) { return sqrt(dist); } +}; + +template +struct simpleDistance, ElementType> +{ + typedef typename L2::ResultType ResultType; + ResultType operator()( ResultType dist ) { return sqrt(dist); } +}; + + +template +struct simpleDistance, ElementType> +{ + typedef typename MinkowskiDistance::ResultType ResultType; + ResultType operator()( ResultType dist ) { return sqrt(dist); } +}; + +template +struct simpleDistance, ElementType> +{ + typedef typename HellingerDistance::ResultType ResultType; + ResultType operator()( ResultType dist ) { return sqrt(dist); } +}; + +template +struct simpleDistance, ElementType> +{ + typedef typename ChiSquareDistance::ResultType ResultType; + ResultType operator()( ResultType dist ) { return sqrt(dist); } +}; + + +template +typename Distance::ResultType ensureSimpleDistance( typename Distance::ResultType dist ) +{ + typedef typename Distance::ElementType ElementType; + + simpleDistance dummy; + return dummy( dist ); +} + } #endif //OPENCV_FLANN_DIST_H_ diff --git a/modules/flann/include/opencv2/flann/lsh_index.h b/modules/flann/include/opencv2/flann/lsh_index.h index 4d4670ea5..23988d436 100644 --- a/modules/flann/include/opencv2/flann/lsh_index.h +++ b/modules/flann/include/opencv2/flann/lsh_index.h @@ -109,10 +109,22 @@ public: */ void buildIndex() { + std::vector indices(feature_size_ * CHAR_BIT); + tables_.resize(table_number_); for (unsigned int i = 0; i < table_number_; ++i) { + + //re-initialize the random indices table that the LshTable will use to pick its sub-dimensions + if( (indices.size() == feature_size_ * CHAR_BIT) || (indices.size() < key_size_) ) + { + indices.resize( feature_size_ * CHAR_BIT ); + for (size_t j = 0; j < feature_size_ * CHAR_BIT; ++j) + indices[j] = j; + std::random_shuffle(indices.begin(), indices.end()); + } + lsh::LshTable& table = tables_[i]; - table = lsh::LshTable(feature_size_, key_size_); + table = lsh::LshTable(feature_size_, key_size_, indices); // Add the features to the table table.add(dataset_); diff --git a/modules/flann/include/opencv2/flann/lsh_table.h b/modules/flann/include/opencv2/flann/lsh_table.h index 2c99a3aee..956e14ef1 100644 --- a/modules/flann/include/opencv2/flann/lsh_table.h +++ b/modules/flann/include/opencv2/flann/lsh_table.h @@ -153,7 +153,7 @@ public: * @param feature_size is the size of the feature (considered as a ElementType[]) * @param key_size is the number of bits that are turned on in the feature */ - LshTable(unsigned int /*feature_size*/, unsigned int /*key_size*/) + LshTable(unsigned int /*feature_size*/, unsigned int /*key_size*/, std::vector & /*indices*/) { std::cerr << "LSH is not implemented for that type" << std::endl; assert(0); @@ -339,20 +339,20 @@ private: // Specialization for unsigned char template<> -inline LshTable::LshTable(unsigned int feature_size, unsigned int subsignature_size) +inline LshTable::LshTable( unsigned int feature_size, + unsigned int subsignature_size, + std::vector & indices ) { initialize(subsignature_size); // Allocate the mask mask_ = std::vector((size_t)ceil((float)(feature_size * sizeof(char)) / (float)sizeof(size_t)), 0); - // A bit brutal but fast to code - std::vector indices(feature_size * CHAR_BIT); - for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i; - std::random_shuffle(indices.begin(), indices.end()); - // Generate a random set of order of subsignature_size_ bits for (unsigned int i = 0; i < key_size_; ++i) { - size_t index = indices[i]; + //Ensure the Nth bit will be selected only once among the different LshTables + //to avoid having two different tables with signatures sharing many dimensions/many bits + size_t index = indices[0]; + indices.erase( indices.begin() ); // Set that bit in the mask size_t divisor = CHAR_BIT * sizeof(size_t); diff --git a/modules/highgui/src/cap_libv4l.cpp b/modules/highgui/src/cap_libv4l.cpp index e7aa5b5df..a05c4b212 100644 --- a/modules/highgui/src/cap_libv4l.cpp +++ b/modules/highgui/src/cap_libv4l.cpp @@ -158,12 +158,12 @@ the symptoms were damaged image and 'Corrupt JPEG data: premature end of data se 11th patch: Apr 13, 2010, Filipe Almeida filipe.almeida@ist.utl.pt - Tries to setup all properties first through v4l2_ioctl call. -- Allows seting up all Video4Linux properties through cvSetCaptureProperty instead of only CV_CAP_PROP_BRIGHTNESS, CV_CAP_PROP_CONTRAST, CV_CAP_PROP_SATURATION, CV_CAP_PROP_HUE, CV_CAP_PROP_GAIN and CV_CAP_PROP_EXPOSURE. +- Allows setting up all Video4Linux properties through cvSetCaptureProperty instead of only CV_CAP_PROP_BRIGHTNESS, CV_CAP_PROP_CONTRAST, CV_CAP_PROP_SATURATION, CV_CAP_PROP_HUE, CV_CAP_PROP_GAIN and CV_CAP_PROP_EXPOSURE. 12th patch: Apr 16, 2010, Filipe Almeida filipe.almeida@ist.utl.pt - CvCaptureCAM_V4L structure cleanup (no longer needs _{min,max,} variables) - Introduction of v4l2_ctrl_range - minimum and maximum allowed values for v4l controls -- Allows seting up all Video4Linux properties through cvSetCaptureProperty using input values between 0.0 and 1.0 +- Allows setting up all Video4Linux properties through cvSetCaptureProperty using input values between 0.0 and 1.0 - Gets v4l properties first through v4l2_ioctl call (ignores capture->is_v4l2_device) - cvGetCaptureProperty adjusted to support the changes - Returns device properties to initial values after device closes diff --git a/modules/highgui/src/loadsave.cpp b/modules/highgui/src/loadsave.cpp index cdcaa23e5..fc942ef66 100644 --- a/modules/highgui/src/loadsave.cpp +++ b/modules/highgui/src/loadsave.cpp @@ -138,9 +138,9 @@ static ImageDecoder findDecoder( const Mat& buf ) maxlen = std::max(maxlen, len); } + String signature(maxlen, ' '); size_t bufSize = buf.rows*buf.cols*buf.elemSize(); maxlen = std::min(maxlen, bufSize); - String signature(maxlen, ' '); memcpy( (void*)signature.c_str(), buf.data, maxlen ); for( i = 0; i < codecs.decoders.size(); i++ ) diff --git a/modules/imgproc/doc/filtering.rst b/modules/imgproc/doc/filtering.rst index a055b129f..3906594be 100755 --- a/modules/imgproc/doc/filtering.rst +++ b/modules/imgproc/doc/filtering.rst @@ -504,6 +504,8 @@ Constructs the Gaussian pyramid for an image. :param maxlevel: 0-based index of the last (the smallest) pyramid layer. It must be non-negative. + :param borderType: Pixel extrapolation method (BORDER_CONSTANT don't supported). See :ocv:func:`borderInterpolate` for details. + The function constructs a vector of images and builds the Gaussian pyramid by recursively applying :ocv:func:`pyrDown` to the previously built pyramid layers, starting from ``dst[0]==src`` . @@ -1256,12 +1258,16 @@ Blurs an image and downsamples it. :param dst: output image; it has the specified size and the same type as ``src``. - :param dstsize: size of the output image; by default, it is computed as ``Size((src.cols+1)/2, (src.rows+1)/2)``, but in any case, the following conditions should be satisfied: + :param dstsize: size of the output image. - .. math:: + :param borderType: Pixel extrapolation method (BORDER_CONSTANT don't supported). See :ocv:func:`borderInterpolate` for details. - \begin{array}{l} - | \texttt{dstsize.width} *2-src.cols| \leq 2 \\ | \texttt{dstsize.height} *2-src.rows| \leq 2 \end{array} +By default, size of the output image is computed as ``Size((src.cols+1)/2, (src.rows+1)/2)``, but in any case, the following conditions should be satisfied: + +.. math:: + + \begin{array}{l} + | \texttt{dstsize.width} *2-src.cols| \leq 2 \\ | \texttt{dstsize.height} *2-src.rows| \leq 2 \end{array} The function performs the downsampling step of the Gaussian pyramid construction. First, it convolves the source image with the kernel: @@ -1271,8 +1277,6 @@ The function performs the downsampling step of the Gaussian pyramid construction Then, it downsamples the image by rejecting even rows and columns. - - pyrUp ----- Upsamples an image and then blurs it. @@ -1287,12 +1291,16 @@ Upsamples an image and then blurs it. :param dst: output image. It has the specified size and the same type as ``src`` . - :param dstsize: size of the output image; by default, it is computed as ``Size(src.cols*2, (src.rows*2)``, but in any case, the following conditions should be satisfied: + :param dstsize: size of the output image. - .. math:: + :param borderType: Pixel extrapolation method (only BORDER_DEFAULT supported). See :ocv:func:`borderInterpolate` for details. - \begin{array}{l} - | \texttt{dstsize.width} -src.cols*2| \leq ( \texttt{dstsize.width} \mod 2) \\ | \texttt{dstsize.height} -src.rows*2| \leq ( \texttt{dstsize.height} \mod 2) \end{array} +By default, size of the output image is computed as ``Size(src.cols*2, (src.rows*2)``, but in any case, the following conditions should be satisfied: + +.. math:: + + \begin{array}{l} + | \texttt{dstsize.width} -src.cols*2| \leq ( \texttt{dstsize.width} \mod 2) \\ | \texttt{dstsize.height} -src.rows*2| \leq ( \texttt{dstsize.height} \mod 2) \end{array} The function performs the upsampling step of the Gaussian pyramid construction, though it can actually be used to construct the Laplacian pyramid. First, it upsamples the source image by injecting even zero rows and columns and then convolves the result with the same kernel as in :ocv:func:`pyrDown` multiplied by 4. diff --git a/modules/imgproc/src/pyramids.cpp b/modules/imgproc/src/pyramids.cpp index 1e4f89cc6..1430731b1 100644 --- a/modules/imgproc/src/pyramids.cpp +++ b/modules/imgproc/src/pyramids.cpp @@ -502,6 +502,8 @@ static bool ocl_pyrUp( InputArray _src, OutputArray _dst, const Size& _dsz, int 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)) @@ -571,6 +573,8 @@ void cv::pyrDown( InputArray _src, OutputArray _dst, const Size& _dsz, int borde 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)) @@ -640,6 +644,8 @@ void cv::pyrUp( InputArray _src, OutputArray _dst, const Size& _dsz, int borderT 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(); diff --git a/modules/viz/src/vtk/vtkCocoaInteractorFix.mm b/modules/viz/src/vtk/vtkCocoaInteractorFix.mm index dad41b073..481baf96b 100644 --- a/modules/viz/src/vtk/vtkCocoaInteractorFix.mm +++ b/modules/viz/src/vtk/vtkCocoaInteractorFix.mm @@ -1,48 +1,54 @@ /*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) 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. -// -// Authors: -// * Anatoly Baksheev, Itseez Inc. myname.mysurname <> mycompany.com -// -// This workaround code was taken from PCL library(www.pointclouds.org) -// -//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) 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. + // + // Authors: + // * Anatoly Baksheev, Itseez Inc. myname.mysurname <> mycompany.com + // + // This workaround code was taken from PCL library(www.pointclouds.org) + // + // Modified by Jasper Shemilt to work with VTK 6.2 + // The fix was needed because GetCocoaServer has been moved from + // vtkCocoaRenderWindowInteractor to vtkCocoaRenderWindow in VTK 6.2. + // This alteration to VTK happened almost a year ago according to the gitHub + // commit a3e9fc9. + // + //M*/ #import #include @@ -118,14 +124,14 @@ [application stop:application]; NSEvent *event = [NSEvent otherEventWithType:NSApplicationDefined - location:NSMakePoint(0.0,0.0) - modifierFlags:0 - timestamp:0 - windowNumber:-1 - context:nil - subtype:0 - data1:0 - data2:0]; + location:NSMakePoint(0.0,0.0) + modifierFlags:0 + timestamp:0 + windowNumber:-1 + context:nil + subtype:0 + data1:0 + data2:0]; [application postEvent:event atStart:YES]; } @@ -154,30 +160,121 @@ //---------------------------------------------------------------------------- +#if VTK_MAJOR_VERSION >= 6 && VTK_MINOR_VERSION >=2 + namespace cv { namespace viz -{ - class vtkCocoaRenderWindowInteractorFix : public vtkCocoaRenderWindowInteractor { - public: - static vtkCocoaRenderWindowInteractorFix *New (); - vtkTypeMacro (vtkCocoaRenderWindowInteractorFix, vtkCocoaRenderWindowInteractor) + class vtkCocoaRenderWindowInteractorFix : public vtkCocoaRenderWindowInteractor + { + public: + static vtkCocoaRenderWindowInteractorFix *New (); + vtkTypeMacro (vtkCocoaRenderWindowInteractorFix, vtkCocoaRenderWindowInteractor) - virtual void Start (); - virtual void TerminateApp (); + virtual void Start (); + virtual void TerminateApp (); - protected: - vtkCocoaRenderWindowInteractorFix () {} - ~vtkCocoaRenderWindowInteractorFix () {} + protected: + vtkCocoaRenderWindowInteractorFix () {} + ~vtkCocoaRenderWindowInteractorFix () {} - private: - vtkCocoaRenderWindowInteractorFix (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. - void operator = (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. - }; + private: + vtkCocoaRenderWindowInteractorFix (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. + void operator = (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. + }; - vtkStandardNewMacro (vtkCocoaRenderWindowInteractorFix) + vtkStandardNewMacro (vtkCocoaRenderWindowInteractorFix) - vtkSmartPointer vtkCocoaRenderWindowInteractorNew(); -}} + vtkSmartPointer vtkCocoaRenderWindowInteractorNew(); + + class vtkCocoaRenderWindowFix : public vtkCocoaRenderWindow + { + public: + static vtkCocoaRenderWindowFix *New (); + vtkTypeMacro ( vtkCocoaRenderWindowFix, vtkCocoaRenderWindow) + + virtual vtkCocoaServerFix * GetCocoaServer (); + virtual void SetCocoaServer (void* ); + + protected: + vtkCocoaRenderWindowFix () {} + ~vtkCocoaRenderWindowFix () {} + + private: + vtkCocoaRenderWindowFix (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. + void operator = (const vtkCocoaRenderWindowFix&); // Not implemented. + }; + + vtkStandardNewMacro (vtkCocoaRenderWindowFix) + + vtkSmartPointer vtkCocoaRenderWindowNew(); + }} + +vtkCocoaServerFix * cv::viz::vtkCocoaRenderWindowFix::GetCocoaServer () +{ + return reinterpret_cast (this->GetCocoaServer ()); +} + +void cv::viz::vtkCocoaRenderWindowFix::SetCocoaServer (void* server) +{ + this->SetCocoaServer (server); +} + +void cv::viz::vtkCocoaRenderWindowInteractorFix::Start () +{ + vtkCocoaRenderWindowFix* renWin = vtkCocoaRenderWindowFix::SafeDownCast(this->GetRenderWindow ()); + if (renWin != NULL) + { + vtkCocoaServerFix *server = reinterpret_cast (renWin->GetCocoaServer ()); + if (!renWin->GetCocoaServer ()) + { + server = [vtkCocoaServerFix cocoaServerWithRenderWindow:renWin]; + renWin->SetCocoaServer (reinterpret_cast (server)); + } + + [server start]; + } +} + +void cv::viz::vtkCocoaRenderWindowInteractorFix::TerminateApp () +{ + vtkCocoaRenderWindowFix *renWin = vtkCocoaRenderWindowFix::SafeDownCast (this->RenderWindow); + if (renWin) + { + vtkCocoaServerFix *server = reinterpret_cast (renWin->GetCocoaServer ()); + [server stop]; + } +} + +vtkSmartPointer cv::viz::vtkCocoaRenderWindowInteractorNew() +{ + return vtkSmartPointer::New(); +} + +#else +namespace cv { namespace viz + { + class vtkCocoaRenderWindowInteractorFix : public vtkCocoaRenderWindowInteractor + { + public: + static vtkCocoaRenderWindowInteractorFix *New (); + vtkTypeMacro (vtkCocoaRenderWindowInteractorFix, vtkCocoaRenderWindowInteractor) + + virtual void Start (); + virtual void TerminateApp (); + + protected: + vtkCocoaRenderWindowInteractorFix () {} + ~vtkCocoaRenderWindowInteractorFix () {} + + private: + vtkCocoaRenderWindowInteractorFix (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. + void operator = (const vtkCocoaRenderWindowInteractorFix&); // Not implemented. + }; + + vtkStandardNewMacro (vtkCocoaRenderWindowInteractorFix) + + vtkSmartPointer vtkCocoaRenderWindowInteractorNew(); + }} void cv::viz::vtkCocoaRenderWindowInteractorFix::Start () { @@ -209,3 +306,5 @@ vtkSmartPointer cv::viz::vtkCocoaRenderWindowInteract { return vtkSmartPointer::New(); } + +#endif diff --git a/samples/android/15-puzzle/src/org/opencv/samples/puzzle15/Puzzle15Activity.java b/samples/android/15-puzzle/src/org/opencv/samples/puzzle15/Puzzle15Activity.java index a87aafed4..b59da5975 100644 --- a/samples/android/15-puzzle/src/org/opencv/samples/puzzle15/Puzzle15Activity.java +++ b/samples/android/15-puzzle/src/org/opencv/samples/puzzle15/Puzzle15Activity.java @@ -56,7 +56,7 @@ public class Puzzle15Activity extends Activity implements CvCameraViewListener, super.onCreate(savedInstanceState); getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); - Log.d(TAG, "Creating and seting view"); + Log.d(TAG, "Creating and setting view"); mOpenCvCameraView = (CameraBridgeViewBase) new JavaCameraView(this, -1); setContentView(mOpenCvCameraView); mOpenCvCameraView.setCvCameraViewListener(this);