From 6f8c3b13d8c2a4f79c9fc207b416095bb07f317f Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Mon, 11 Nov 2024 08:22:56 +0100 Subject: [PATCH] Merge pull request #26437 from vrabaud:4x_calibration_base Backport C++ stereo/stereo_geom.cpp:5.x to calib3d/stereo_geom.cpp:4.x #26437 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/calib3d/src/calib3d_c_api.h | 55 -- modules/calib3d/src/calibration.cpp | 760 --------------------- modules/calib3d/src/calibration_base.cpp | 5 +- modules/calib3d/src/compat_ptsetreg.cpp | 77 --- modules/calib3d/src/fisheye.cpp | 84 --- modules/calib3d/src/precomp.hpp | 4 + modules/calib3d/src/stereo_geom.cpp | 713 +++++++++++++++++++ modules/calib3d/src/undistort.dispatch.cpp | 11 +- 8 files changed, 721 insertions(+), 988 deletions(-) create mode 100644 modules/calib3d/src/stereo_geom.cpp diff --git a/modules/calib3d/src/calib3d_c_api.h b/modules/calib3d/src/calib3d_c_api.h index 76d5f79450..68d3c20b78 100644 --- a/modules/calib3d/src/calib3d_c_api.h +++ b/modules/calib3d/src/calib3d_c_api.h @@ -55,16 +55,6 @@ extern "C" { * Camera Calibration, Pose Estimation and Stereo * \****************************************************************************************/ -void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst ); - -/* For each input point on one of images - computes parameters of the corresponding - epipolar line on the other image */ -void cvComputeCorrespondEpilines( const CvMat* points, - int which_image, - const CvMat* fundamental_matrix, - CvMat* correspondent_lines ); - /* Finds perspective transformation between the object plane and image (view) plane */ int cvFindHomography( const CvMat* src_points, const CvMat* dst_points, @@ -97,51 +87,6 @@ double cvCalibrateCamera2( const CvMat* object_points, CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) ); -/* Computes the transformation from one camera coordinate system to another one - from a few correspondent views of the same calibration target. Optionally, calibrates - both cameras */ -double cvStereoCalibrate( const CvMat* object_points, const CvMat* image_points1, - const CvMat* image_points2, const CvMat* npoints, - CvMat* camera_matrix1, CvMat* dist_coeffs1, - CvMat* camera_matrix2, CvMat* dist_coeffs2, - CvSize image_size, CvMat* R, CvMat* T, - CvMat* E CV_DEFAULT(0), CvMat* F CV_DEFAULT(0), - int flags CV_DEFAULT(CV_CALIB_FIX_INTRINSIC), - CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( - CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,1e-6)) ); - -#define CV_CALIB_ZERO_DISPARITY 1024 - -/* Computes 3D rotations (+ optional shift) for each camera coordinate system to make both - views parallel (=> to make all the epipolar lines horizontal or vertical) */ -void cvStereoRectify( const CvMat* camera_matrix1, const CvMat* camera_matrix2, - const CvMat* dist_coeffs1, const CvMat* dist_coeffs2, - CvSize image_size, const CvMat* R, const CvMat* T, - CvMat* R1, CvMat* R2, CvMat* P1, CvMat* P2, - CvMat* Q CV_DEFAULT(0), - int flags CV_DEFAULT(CV_CALIB_ZERO_DISPARITY), - double alpha CV_DEFAULT(-1), - CvSize new_image_size CV_DEFAULT(cvSize(0,0)), - CvRect* valid_pix_ROI1 CV_DEFAULT(0), - CvRect* valid_pix_ROI2 CV_DEFAULT(0)); - -/* Computes rectification transformations for uncalibrated pair of images using a set - of point correspondences */ -int cvStereoRectifyUncalibrated( const CvMat* points1, const CvMat* points2, - const CvMat* F, CvSize img_size, - CvMat* H1, CvMat* H2, - double threshold CV_DEFAULT(5)); - -/** @brief Computes the original (undistorted) feature coordinates - from the observed (distorted) coordinates -@see cv::undistortPoints -*/ -void cvUndistortPoints( const CvMat* src, CvMat* dst, - const CvMat* camera_matrix, - const CvMat* dist_coeffs, - const CvMat* R CV_DEFAULT(0), - const CvMat* P CV_DEFAULT(0)); - #ifdef __cplusplus } // extern "C" #endif diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index a10d83b7eb..58544909a2 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1225,575 +1225,6 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i return std::sqrt(reprojErr/(pointsTotal*2)); } -double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1, - const CvMat* _imagePoints2, const CvMat* _npoints, - CvMat* _cameraMatrix1, CvMat* _distCoeffs1, - CvMat* _cameraMatrix2, CvMat* _distCoeffs2, - CvSize imageSize, CvMat* matR, CvMat* matT, - CvMat* matE, CvMat* matF, - int flags, - CvTermCriteria termCrit ) -{ - return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1, - _distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE, - matF, NULL, NULL, NULL, flags, termCrit); -} - -static void -icvGetRectangles( const CvMat* cameraMatrix, const CvMat* distCoeffs, - const CvMat* R, const CvMat* newCameraMatrix, CvSize imgSize, - cv::Rect_& inner, cv::Rect_& outer ) -{ - const int N = 9; - int x, y, k; - cv::Ptr _pts(cvCreateMat(1, N*N, CV_64FC2)); - CvPoint2D64f* pts = (CvPoint2D64f*)(_pts->data.ptr); - - for( y = k = 0; y < N; y++ ) - for( x = 0; x < N; x++ ) - pts[k++] = cvPoint2D64f((double)x*(imgSize.width-1)/(N-1), - (double)y*(imgSize.height-1)/(N-1)); - - cvUndistortPoints(_pts, _pts, cameraMatrix, distCoeffs, R, newCameraMatrix); - - double iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX; - double oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX; - // find the inscribed rectangle. - // the code will likely not work with extreme rotation matrices (R) (>45%) - for( y = k = 0; y < N; y++ ) - for( x = 0; x < N; x++ ) - { - CvPoint2D64f p = pts[k++]; - oX0 = MIN(oX0, p.x); - oX1 = MAX(oX1, p.x); - oY0 = MIN(oY0, p.y); - oY1 = MAX(oY1, p.y); - - if( x == 0 ) - iX0 = MAX(iX0, p.x); - if( x == N-1 ) - iX1 = MIN(iX1, p.x); - if( y == 0 ) - iY0 = MAX(iY0, p.y); - if( y == N-1 ) - iY1 = MIN(iY1, p.y); - } - inner = cv::Rect_(iX0, iY0, iX1-iX0, iY1-iY0); - outer = cv::Rect_(oX0, oY0, oX1-oX0, oY1-oY0); -} - - -void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, - const CvMat* _distCoeffs1, const CvMat* _distCoeffs2, - CvSize imageSize, const CvMat* matR, const CvMat* matT, - CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, - CvMat* matQ, int flags, double alpha, CvSize newImgSize, - CvRect* roi1, CvRect* roi2 ) -{ - double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4]; - double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3]; - cv::Rect_ inner1, inner2, outer1, outer2; - - CvMat om = cvMat(3, 1, CV_64F, _om); - CvMat t = cvMat(3, 1, CV_64F, _t); - CvMat uu = cvMat(3, 1, CV_64F, _uu); - CvMat r_r = cvMat(3, 3, CV_64F, _r_r); - CvMat pp = cvMat(3, 4, CV_64F, _pp); - CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps - CvMat wR = cvMat(3, 3, CV_64F, _wr); - CvMat Z = cvMat(3, 1, CV_64F, _z); - CvMat Ri = cvMat(3, 3, CV_64F, _ri); - double nx = imageSize.width, ny = imageSize.height; - int i, k; - - if( matR->rows == 3 && matR->cols == 3 ) - Rodrigues(cvarrToMat(matR), cvarrToMat(&om)); // get vector rotation - else - cvConvert(matR, &om); // it's already a rotation vector - cvConvertScale(&om, &om, -0.5); // get average rotation - Rodrigues(cvarrToMat(&om), cvarrToMat(&r_r)); // rotate cameras to same orientation by averaging - cvMatMul(&r_r, matT, &t); - - int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; - double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); - _uu[idx] = c > 0 ? 1 : -1; - - CV_Assert(nt > 0.0); - - // calculate global Z rotation - cvCrossProduct(&t,&uu,&ww); - double nw = cvNorm(&ww, 0, CV_L2); - if (nw > 0.0) - cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw); - Rodrigues(cvarrToMat(&ww), cvarrToMat(&wR)); - - // apply to both views - cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); - cvConvert( &Ri, _R1 ); - cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0); - cvConvert( &Ri, _R2 ); - cvMatMul(&Ri, matT, &t); - - // calculate projection/camera matrices - // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) - double fc_new = DBL_MAX; - CvPoint2D64f cc_new[2] = {}; - - newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize; - const double ratio_x = (double)newImgSize.width / imageSize.width / 2; - const double ratio_y = (double)newImgSize.height / imageSize.height / 2; - const double ratio = idx == 1 ? ratio_x : ratio_y; - fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio; - - for( k = 0; k < 2; k++ ) - { - const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2; - const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2; - CvPoint2D32f _pts[4] = {}; - CvPoint3D32f _pts_3[4] = {}; - CvMat pts = cvMat(1, 4, CV_32FC2, _pts); - CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); - - for( i = 0; i < 4; i++ ) - { - int j = (i<2) ? 0 : 1; - _pts[i].x = (float)((i % 2)*(nx-1)); - _pts[i].y = (float)(j*(ny-1)); - } - cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 ); - cvConvertPointsHomogeneous( &pts, &pts_3 ); - - //Change camera matrix to have cc=[0,0] and fc = fc_new - double _a_tmp[3][3]; - CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp); - _a_tmp[0][0]=fc_new; - _a_tmp[1][1]=fc_new; - _a_tmp[0][2]=0.0; - _a_tmp[1][2]=0.0; - projectPoints( cvarrToMat(&pts_3), cvarrToMat(k == 0 ? _R1 : _R2), cvarrToMat(&Z), - cvarrToMat(&A_tmp), noArray(), cvarrToMat(&pts) ); - CvScalar avg = cvAvg(&pts); - cc_new[k].x = (nx-1)/2 - avg.val[0]; - cc_new[k].y = (ny-1)/2 - avg.val[1]; - } - - // vertical focal length must be the same for both images to keep the epipolar constraint - // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines) - // use fy for fx also, for simplicity - - // 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 & CALIB_ZERO_DISPARITY ) - { - cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; - cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; - } - else if( idx == 0 ) // horizontal stereo - cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; - else // vertical stereo - cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; - - cvZero( &pp ); - _pp[0][0] = _pp[1][1] = fc_new; - _pp[0][2] = cc_new[0].x; - _pp[1][2] = cc_new[0].y; - _pp[2][2] = 1; - cvConvert(&pp, _P1); - - _pp[0][2] = cc_new[1].x; - _pp[1][2] = cc_new[1].y; - _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length - cvConvert(&pp, _P2); - - alpha = MIN(alpha, 1.); - - icvGetRectangles( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 ); - icvGetRectangles( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 ); - - { - newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize; - double cx1_0 = cc_new[0].x; - double cy1_0 = cc_new[0].y; - double cx2_0 = cc_new[1].x; - double cy2_0 = cc_new[1].y; - double cx1 = newImgSize.width*cx1_0/imageSize.width; - double cy1 = newImgSize.height*cy1_0/imageSize.height; - double cx2 = newImgSize.width*cx2_0/imageSize.width; - double cy2 = newImgSize.height*cy2_0/imageSize.height; - double s = 1.; - - if( alpha >= 0 ) - { - double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)), - (double)(newImgSize.width - 1 - cx1)/(inner1.x + inner1.width - cx1_0)), - (double)(newImgSize.height - 1 - cy1)/(inner1.y + inner1.height - cy1_0)); - s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)), - (double)(newImgSize.width - 1 - cx2)/(inner2.x + inner2.width - cx2_0)), - (double)(newImgSize.height - 1 - cy2)/(inner2.y + inner2.height - cy2_0)), - s0); - - double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)), - (double)(newImgSize.width - 1 - cx1)/(outer1.x + outer1.width - cx1_0)), - (double)(newImgSize.height - 1 - cy1)/(outer1.y + outer1.height - cy1_0)); - s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)), - (double)(newImgSize.width - 1 - cx2)/(outer2.x + outer2.width - cx2_0)), - (double)(newImgSize.height - 1 - cy2)/(outer2.y + outer2.height - cy2_0)), - s1); - - s = s0*(1 - alpha) + s1*alpha; - } - - fc_new *= s; - cc_new[0] = cvPoint2D64f(cx1, cy1); - cc_new[1] = cvPoint2D64f(cx2, cy2); - - cvmSet(_P1, 0, 0, fc_new); - cvmSet(_P1, 1, 1, fc_new); - cvmSet(_P1, 0, 2, cx1); - cvmSet(_P1, 1, 2, cy1); - - cvmSet(_P2, 0, 0, fc_new); - cvmSet(_P2, 1, 1, fc_new); - cvmSet(_P2, 0, 2, cx2); - cvmSet(_P2, 1, 2, cy2); - cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3)); - - if(roi1) - { - *roi1 = cvRect( - cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1), - cvCeil((inner1.y - cy1_0)*s + cy1), - cvFloor(inner1.width*s), cvFloor(inner1.height*s)) - & cv::Rect(0, 0, newImgSize.width, newImgSize.height) - ); - } - - if(roi2) - { - *roi2 = cvRect( - cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2), - cvCeil((inner2.y - cy2_0)*s + cy2), - cvFloor(inner2.width*s), cvFloor(inner2.height*s)) - & cv::Rect(0, 0, newImgSize.width, newImgSize.height) - ); - } - } - - if( matQ ) - { - double q[] = - { - 1, 0, 0, -cc_new[0].x, - 0, 1, 0, -cc_new[0].y, - 0, 0, 0, fc_new, - 0, 0, -1./_t[idx], - (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx] - }; - CvMat Q = cvMat(4, 4, CV_64F, q); - cvConvert( &Q, matQ ); - } -} - - -CV_IMPL int cvStereoRectifyUncalibrated( - const CvMat* _points1, const CvMat* _points2, - const CvMat* F0, CvSize imgSize, - CvMat* _H1, CvMat* _H2, double threshold ) -{ - Ptr _m1, _m2, _lines1, _lines2; - - int i, j, npoints; - double cx, cy; - double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3] = {0}; - CvMat E2 = cvMat( 3, 1, CV_64F, e2 ); - CvMat U = cvMat( 3, 3, CV_64F, u ); - CvMat V = cvMat( 3, 3, CV_64F, v ); - CvMat W = cvMat( 3, 3, CV_64F, w ); - CvMat F = cvMat( 3, 3, CV_64F, f ); - CvMat H1 = cvMat( 3, 3, CV_64F, h1 ); - CvMat H2 = cvMat( 3, 3, CV_64F, h2 ); - CvMat H0 = cvMat( 3, 3, CV_64F, h0 ); - - CvPoint2D64f* m1; - CvPoint2D64f* m2; - CvPoint3D64f* lines1; - CvPoint3D64f* lines2; - - CV_Assert( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) && - CV_ARE_SIZES_EQ(_points1, _points2) ); - - npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2; - - _m1.reset(cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) )); - _m2.reset(cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) )); - _lines1.reset(cvCreateMat( 1, npoints, CV_64FC3 )); - _lines2.reset(cvCreateMat( 1, npoints, CV_64FC3 )); - - cvConvert( F0, &F ); - - cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T ); - W.data.db[8] = 0.; - cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T ); - cvMatMul( &W, &V, &F ); - - cx = cvRound( (imgSize.width-1)*0.5 ); - cy = cvRound( (imgSize.height-1)*0.5 ); - - cvZero( _H1 ); - cvZero( _H2 ); - - cvConvert( _points1, _m1 ); - cvConvert( _points2, _m2 ); - cvReshape( _m1, _m1, 2, 1 ); - cvReshape( _m2, _m2, 2, 1 ); - - m1 = (CvPoint2D64f*)_m1->data.ptr; - m2 = (CvPoint2D64f*)_m2->data.ptr; - lines1 = (CvPoint3D64f*)_lines1->data.ptr; - lines2 = (CvPoint3D64f*)_lines2->data.ptr; - - if( threshold > 0 ) - { - cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 ); - cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 ); - - // measure distance from points to the corresponding epilines, mark outliers - for( i = j = 0; i < npoints; i++ ) - { - if( fabs(m1[i].x*lines2[i].x + - m1[i].y*lines2[i].y + - lines2[i].z) <= threshold && - fabs(m2[i].x*lines1[i].x + - m2[i].y*lines1[i].y + - lines1[i].z) <= threshold ) - { - if( j < i ) - { - m1[j] = m1[i]; - m2[j] = m2[i]; - } - j++; - } - } - - npoints = j; - if( npoints == 0 ) - return 0; - } - - _m1->cols = _m2->cols = npoints; - memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); - cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); - - double t[] = - { - 1, 0, -cx, - 0, 1, -cy, - 0, 0, 1 - }; - CvMat T = cvMat(3, 3, CV_64F, t); - cvMatMul( &T, &E2, &E2 ); - - int mirror = e2[0] < 0; - double d = MAX(std::sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON); - double alpha = e2[0]/d; - double beta = e2[1]/d; - double r[] = - { - alpha, beta, 0, - -beta, alpha, 0, - 0, 0, 1 - }; - CvMat R = cvMat(3, 3, CV_64F, r); - cvMatMul( &R, &T, &T ); - cvMatMul( &R, &E2, &E2 ); - double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0]; - double k[] = - { - 1, 0, 0, - 0, 1, 0, - invf, 0, 1 - }; - CvMat K = cvMat(3, 3, CV_64F, k); - cvMatMul( &K, &T, &H2 ); - cvMatMul( &K, &E2, &E2 ); - - double it[] = - { - 1, 0, cx, - 0, 1, cy, - 0, 0, 1 - }; - CvMat iT = cvMat( 3, 3, CV_64F, it ); - cvMatMul( &iT, &H2, &H2 ); - - memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); - cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); - - double e2_x[] = - { - 0, -e2[2], e2[1], - e2[2], 0, -e2[0], - -e2[1], e2[0], 0 - }; - double e2_111[] = - { - e2[0], e2[0], e2[0], - e2[1], e2[1], e2[1], - e2[2], e2[2], e2[2], - }; - CvMat E2_x = cvMat(3, 3, CV_64F, e2_x); - CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111); - cvMatMulAdd(&E2_x, &F, &E2_111, &H0 ); - cvMatMul(&H2, &H0, &H0); - CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6); - cvMatMul(&H0, &E1, &E1); - - cvPerspectiveTransform( _m1, _m1, &H0 ); - cvPerspectiveTransform( _m2, _m2, &H2 ); - CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B; - double x[3] = {0}; - CvMat X = cvMat( 3, 1, CV_64F, x ); - cvConvertPointsHomogeneous( _m1, &A ); - cvReshape( &A, &A, 1, npoints ); - cvReshape( _m2, &BxBy, 1, npoints ); - cvGetCol( &BxBy, &B, 0 ); - cvSolve( &A, &B, &X, CV_SVD ); - - double ha[] = - { - x[0], x[1], x[2], - 0, 1, 0, - 0, 0, 1 - }; - CvMat Ha = cvMat(3, 3, CV_64F, ha); - cvMatMul( &Ha, &H0, &H1 ); - cvPerspectiveTransform( _m1, _m1, &Ha ); - - if( mirror ) - { - double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 }; - CvMat MM = cvMat(3, 3, CV_64F, mm); - cvMatMul( &MM, &H1, &H1 ); - cvMatMul( &MM, &H2, &H2 ); - } - - cvConvert( &H1, _H1 ); - cvConvert( &H2, _H2 ); - - return 1; -} - - -void cv::reprojectImageTo3D( InputArray _disparity, - OutputArray __3dImage, InputArray _Qmat, - bool handleMissingValues, int dtype ) -{ - CV_INSTRUMENT_REGION(); - - Mat disparity = _disparity.getMat(), Q = _Qmat.getMat(); - int stype = disparity.type(); - - CV_Assert( stype == CV_8UC1 || stype == CV_16SC1 || - stype == CV_32SC1 || stype == CV_32FC1 ); - CV_Assert( Q.size() == Size(4,4) ); - - if( dtype >= 0 ) - dtype = CV_MAKETYPE(CV_MAT_DEPTH(dtype), 3); - - if( __3dImage.fixedType() ) - { - int dtype_ = __3dImage.type(); - CV_Assert( dtype == -1 || dtype == dtype_ ); - dtype = dtype_; - } - - if( dtype < 0 ) - dtype = CV_32FC3; - else - CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 ); - - __3dImage.create(disparity.size(), dtype); - Mat _3dImage = __3dImage.getMat(); - - const float bigZ = 10000.f; - Matx44d _Q; - Q.convertTo(_Q, CV_64F); - - int x, cols = disparity.cols; - CV_Assert( cols >= 0 ); - - std::vector _sbuf(cols); - std::vector _dbuf(cols); - float* sbuf = &_sbuf[0]; - Vec3f* dbuf = &_dbuf[0]; - double minDisparity = FLT_MAX; - - // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined. - // and we set the corresponding Z's to some fixed big value. - if( handleMissingValues ) - cv::minMaxIdx( disparity, &minDisparity, 0, 0, 0 ); - - for( int y = 0; y < disparity.rows; y++ ) - { - float* sptr = sbuf; - Vec3f* dptr = dbuf; - - if( stype == CV_8UC1 ) - { - const uchar* sptr0 = disparity.ptr(y); - for( x = 0; x < cols; x++ ) - sptr[x] = (float)sptr0[x]; - } - else if( stype == CV_16SC1 ) - { - const short* sptr0 = disparity.ptr(y); - for( x = 0; x < cols; x++ ) - sptr[x] = (float)sptr0[x]; - } - else if( stype == CV_32SC1 ) - { - const int* sptr0 = disparity.ptr(y); - for( x = 0; x < cols; x++ ) - sptr[x] = (float)sptr0[x]; - } - else - sptr = disparity.ptr(y); - - if( dtype == CV_32FC3 ) - dptr = _3dImage.ptr(y); - - for( x = 0; x < cols; x++) - { - double d = sptr[x]; - Vec4d homg_pt = _Q*Vec4d(x, y, d, 1.0); - dptr[x] = Vec3d(homg_pt.val); - dptr[x] /= homg_pt[3]; - - if( fabs(d-minDisparity) <= FLT_EPSILON ) - dptr[x][2] = bigZ; - } - - if( dtype == CV_16SC3 ) - { - Vec3s* dptr0 = _3dImage.ptr(y); - for( x = 0; x < cols; x++ ) - { - dptr0[x] = dptr[x]; - } - } - else if( dtype == CV_32SC3 ) - { - Vec3i* dptr0 = _3dImage.ptr(y); - for( x = 0; x < cols; x++ ) - { - dptr0[x] = dptr[x]; - } - } - } -} - - namespace cv { @@ -2351,195 +1782,4 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, return err; } - -void cv::stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1, - InputArray _cameraMatrix2, InputArray _distCoeffs2, - Size imageSize, InputArray _Rmat, InputArray _Tmat, - OutputArray _Rmat1, OutputArray _Rmat2, - OutputArray _Pmat1, OutputArray _Pmat2, - OutputArray _Qmat, int flags, - double alpha, Size newImageSize, - Rect* validPixROI1, Rect* validPixROI2 ) -{ - Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat(); - Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat(); - Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat(); - CvMat c_cameraMatrix1 = cvMat(cameraMatrix1); - CvMat c_cameraMatrix2 = cvMat(cameraMatrix2); - CvMat c_distCoeffs1 = cvMat(distCoeffs1); - CvMat c_distCoeffs2 = cvMat(distCoeffs2); - CvMat c_R = cvMat(Rmat), c_T = cvMat(Tmat); - - int rtype = CV_64F; - _Rmat1.create(3, 3, rtype); - _Rmat2.create(3, 3, rtype); - _Pmat1.create(3, 4, rtype); - _Pmat2.create(3, 4, rtype); - Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q; - CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2); - CvMat c_Q, *p_Q = 0; - - if( _Qmat.needed() ) - { - _Qmat.create(4, 4, rtype); - p_Q = &(c_Q = cvMat(Q = _Qmat.getMat())); - } - - CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1; - CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2; - cvStereoRectify( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2, - cvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha, - cvSize(newImageSize), (CvRect*)validPixROI1, (CvRect*)validPixROI2); -} - -bool cv::stereoRectifyUncalibrated( InputArray _points1, InputArray _points2, - InputArray _Fmat, Size imgSize, - OutputArray _Hmat1, OutputArray _Hmat2, double threshold ) -{ - CV_INSTRUMENT_REGION(); - - int rtype = CV_64F; - _Hmat1.create(3, 3, rtype); - _Hmat2.create(3, 3, rtype); - Mat F = _Fmat.getMat(); - Mat points1 = _points1.getMat(), points2 = _points2.getMat(); - CvMat c_pt1 = cvMat(points1), c_pt2 = cvMat(points2); - Mat H1 = _Hmat1.getMat(), H2 = _Hmat2.getMat(); - CvMat c_F, *p_F=0, c_H1 = cvMat(H1), c_H2 = cvMat(H2); - if( F.size() == Size(3, 3) ) - p_F = &(c_F = cvMat(F)); - return cvStereoRectifyUncalibrated(&c_pt1, &c_pt2, p_F, cvSize(imgSize), &c_H1, &c_H2, threshold) > 0; -} - -namespace cv -{ - -static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0, - InputArrayOfArrays _imgpt3_0, - const Mat& cameraMatrix1, const Mat& distCoeffs1, - const Mat& cameraMatrix3, const Mat& distCoeffs3, - const Mat& R1, const Mat& R3, const Mat& P1, Mat& P3 ) -{ - size_t n1 = _imgpt1_0.total(), n3 = _imgpt3_0.total(); - std::vector imgpt1, imgpt3; - - for( int i = 0; i < (int)std::min(n1, n3); i++ ) - { - Mat pt1 = _imgpt1_0.getMat(i), pt3 = _imgpt3_0.getMat(i); - int ni1 = pt1.checkVector(2, CV_32F), ni3 = pt3.checkVector(2, CV_32F); - CV_Assert( ni1 > 0 && ni1 == ni3 ); - const Point2f* pt1data = pt1.ptr(); - const Point2f* pt3data = pt3.ptr(); - std::copy(pt1data, pt1data + ni1, std::back_inserter(imgpt1)); - std::copy(pt3data, pt3data + ni3, std::back_inserter(imgpt3)); - } - - undistortPoints(imgpt1, imgpt1, cameraMatrix1, distCoeffs1, R1, P1); - undistortPoints(imgpt3, imgpt3, cameraMatrix3, distCoeffs3, R3, P3); - - double y1_ = 0, y2_ = 0, y1y1_ = 0, y1y2_ = 0; - size_t n = imgpt1.size(); - CV_DbgAssert(n > 0); - - for( size_t i = 0; i < n; i++ ) - { - double y1 = imgpt3[i].y, y2 = imgpt1[i].y; - - y1_ += y1; y2_ += y2; - y1y1_ += y1*y1; y1y2_ += y1*y2; - } - - y1_ /= n; - y2_ /= n; - y1y1_ /= n; - y1y2_ /= n; - - double a = (y1y2_ - y1_*y2_)/(y1y1_ - y1_*y1_); - double b = y2_ - a*y1_; - - P3.at(0,0) *= a; - P3.at(1,1) *= a; - P3.at(0,2) = P3.at(0,2)*a; - P3.at(1,2) = P3.at(1,2)*a + b; - P3.at(0,3) *= a; - P3.at(1,3) *= a; -} - -} - -float cv::rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1, - InputArray _cameraMatrix2, InputArray _distCoeffs2, - InputArray _cameraMatrix3, InputArray _distCoeffs3, - InputArrayOfArrays _imgpt1, - InputArrayOfArrays _imgpt3, - Size imageSize, InputArray _Rmat12, InputArray _Tmat12, - InputArray _Rmat13, InputArray _Tmat13, - OutputArray _Rmat1, OutputArray _Rmat2, OutputArray _Rmat3, - OutputArray _Pmat1, OutputArray _Pmat2, OutputArray _Pmat3, - OutputArray _Qmat, - double alpha, Size newImgSize, - Rect* roi1, Rect* roi2, int flags ) -{ - // first, rectify the 1-2 stereo pair - stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2, - imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat, - flags, alpha, newImgSize, roi1, roi2 ); - - Mat R12 = _Rmat12.getMat(), R13 = _Rmat13.getMat(), T12 = _Tmat12.getMat(), T13 = _Tmat13.getMat(); - - _Rmat3.create(3, 3, CV_64F); - _Pmat3.create(3, 4, CV_64F); - - Mat P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(); - Mat R3 = _Rmat3.getMat(), P3 = _Pmat3.getMat(); - - // recompute rectification transforms for cameras 1 & 2. - Mat om, r_r, r_r13; - - if( R13.size() != Size(3,3) ) - Rodrigues(R13, r_r13); - else - R13.copyTo(r_r13); - - if( R12.size() == Size(3,3) ) - Rodrigues(R12, om); - else - R12.copyTo(om); - - om *= -0.5; - Rodrigues(om, r_r); // rotate cameras to same orientation by averaging - Mat_ t12 = r_r * T12; - - int idx = fabs(t12(0,0)) > fabs(t12(1,0)) ? 0 : 1; - double c = t12(idx,0), nt = norm(t12, CV_L2); - CV_Assert(fabs(nt) > 0); - Mat_ uu = Mat_::zeros(3,1); - uu(idx, 0) = c > 0 ? 1 : -1; - - // calculate global Z rotation - Mat_ ww = t12.cross(uu), wR; - double nw = norm(ww, CV_L2); - CV_Assert(fabs(nw) > 0); - ww *= acos(fabs(c)/nt)/nw; - Rodrigues(ww, wR); - - // now rotate camera 3 to make its optical axis parallel to cameras 1 and 2. - R3 = wR*r_r.t()*r_r13.t(); - Mat_ t13 = R3 * T13; - - P2.copyTo(P3); - Mat t = P3.col(3); - t13.copyTo(t); - P3.at(0,3) *= P3.at(0,0); - P3.at(1,3) *= P3.at(1,1); - - if( !_imgpt1.empty() && !_imgpt3.empty() ) - adjust3rdMatrix(_imgpt1, _imgpt3, _cameraMatrix1.getMat(), _distCoeffs1.getMat(), - _cameraMatrix3.getMat(), _distCoeffs3.getMat(), _Rmat1.getMat(), R3, P1, P3); - - return (float)((P3.at(idx,3)/P3.at(idx,idx))/ - (P2.at(idx,3)/P2.at(idx,idx))); -} - - /* End of file. */ diff --git a/modules/calib3d/src/calibration_base.cpp b/modules/calib3d/src/calibration_base.cpp index c047a45e8e..166f297917 100644 --- a/modules/calib3d/src/calibration_base.cpp +++ b/modules/calib3d/src/calibration_base.cpp @@ -43,7 +43,8 @@ #include "precomp.hpp" #include "hal_replacement.hpp" #include "distortion_model.hpp" -#include "calib3d_c_api.h" +#include "opencv2/calib3d/calib3d_c.h" +#include "opencv2/core/core_c.h" #include #include @@ -1520,7 +1521,7 @@ void cv::projectPoints( InputArray _opoints, } } -static void getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, +void cv::getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray R, InputArray newCameraMatrix, Size imgSize, Rect_& inner, Rect_& outer ) { diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index ba103d7b8d..fec3f3a420 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -357,80 +357,3 @@ CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, H0.convertTo(H, H.type()); return 1; } - - -CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID, - const CvMat* fmatrix, CvMat* _lines ) -{ - cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix); - cv::Mat lines = cv::cvarrToMat(_lines); - const cv::Mat lines0 = lines; - - if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 ) - cv::transpose(pt, pt); - - cv::computeCorrespondEpilines(pt, pointImageID, fm, lines); - - bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3; - lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows)); - - if( tflag ) - { - CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows ); - if( lines0.type() == lines.type() ) - transpose( lines, lines0 ); - else - { - transpose( lines, lines ); - lines.convertTo( lines0, lines0.type() ); - } - } - else - { - CV_Assert( lines.size() == lines0.size() ); - if( lines.data != lines0.data ) - lines.convertTo(lines0, lines0.type()); - } -} - - -CV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst ) -{ - cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst); - const cv::Mat dst0 = dst; - - int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows); - - if( src.channels() == 1 && src.cols > d0 ) - cv::transpose(src, src); - - int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows); - - if( d0 == d1 ) - src.copyTo(dst); - else if( d0 < d1 ) - cv::convertPointsToHomogeneous(src, dst); - else - cv::convertPointsFromHomogeneous(src, dst); - - bool tflag = dst0.channels() == 1 && dst0.cols > d1; - dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows)); - - if( tflag ) - { - CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows ); - if( dst0.type() == dst.type() ) - transpose( dst, dst0 ); - else - { - transpose( dst, dst ); - dst.convertTo( dst0, dst0.type() ); - } - } - else - { - CV_Assert( dst.size() == dst0.size() ); - if( dst.data != dst0.data ) - dst.convertTo(dst0, dst0.type()); - } -} diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 4aec4324e0..7bbddf2838 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -698,90 +698,6 @@ void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, Input } -////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// 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_INSTRUMENT_REGION(); - - 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 diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index 05811e0907..118e9dc974 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -150,6 +150,10 @@ void projectPoints( InputArray objectPoints, OutputArray dpdc=noArray(), OutputArray dpdk=noArray(), OutputArray dpdo=noArray(), double aspectRatio=0.); +void getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, + InputArray R, InputArray newCameraMatrix, Size imgSize, + Rect_& inner, Rect_& outer ); + } // namespace cv int checkChessboardBinary(const cv::Mat & img, const cv::Size & size); diff --git a/modules/calib3d/src/stereo_geom.cpp b/modules/calib3d/src/stereo_geom.cpp new file mode 100644 index 0000000000..4c91522bdc --- /dev/null +++ b/modules/calib3d/src/stereo_geom.cpp @@ -0,0 +1,713 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "precomp.hpp" + +namespace cv { + +void reprojectImageTo3D( InputArray _disparity, OutputArray __3dImage, + InputArray _Qmat, bool handleMissingValues, int dtype ) +{ + CV_INSTRUMENT_REGION(); + + Mat disparity = _disparity.getMat(), Q = _Qmat.getMat(); + int stype = disparity.type(); + + CV_Assert( stype == CV_8UC1 || stype == CV_16SC1 || + stype == CV_32SC1 || stype == CV_32FC1 ); + CV_Assert( Q.size() == Size(4,4) ); + + if( dtype >= 0 ) + dtype = CV_MAKETYPE(CV_MAT_DEPTH(dtype), 3); + + if( __3dImage.fixedType() ) + { + int dtype_ = __3dImage.type(); + CV_Assert( dtype == -1 || dtype == dtype_ ); + dtype = dtype_; + } + + if( dtype < 0 ) + dtype = CV_32FC3; + else + CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 ); + + __3dImage.create(disparity.size(), dtype); + Mat _3dImage = __3dImage.getMat(); + + const float bigZ = 10000.f; + Matx44d _Q; + Q.convertTo(_Q, CV_64F); + + int x, cols = disparity.cols; + CV_Assert( cols >= 0 ); + + std::vector _sbuf(cols); + std::vector _dbuf(cols); + float* sbuf = &_sbuf[0]; + Vec3f* dbuf = &_dbuf[0]; + double minDisparity = FLT_MAX; + + // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined. + // and we set the corresponding Z's to some fixed big value. + if( handleMissingValues ) + cv::minMaxIdx( disparity, &minDisparity, 0, 0, 0 ); + + for( int y = 0; y < disparity.rows; y++ ) + { + float* sptr = sbuf; + Vec3f* dptr = dbuf; + + if( stype == CV_8UC1 ) + { + const uchar* sptr0 = disparity.ptr(y); + for( x = 0; x < cols; x++ ) + sptr[x] = (float)sptr0[x]; + } + else if( stype == CV_16SC1 ) + { + const short* sptr0 = disparity.ptr(y); + for( x = 0; x < cols; x++ ) + sptr[x] = (float)sptr0[x]; + } + else if( stype == CV_32SC1 ) + { + const int* sptr0 = disparity.ptr(y); + for( x = 0; x < cols; x++ ) + sptr[x] = (float)sptr0[x]; + } + else + sptr = disparity.ptr(y); + + if( dtype == CV_32FC3 ) + dptr = _3dImage.ptr(y); + + for( x = 0; x < cols; x++) + { + double d = sptr[x]; + Vec4d homg_pt = _Q*Vec4d(x, y, d, 1.0); + dptr[x] = Vec3d(homg_pt.val); + dptr[x] /= homg_pt[3]; + + if( fabs(d-minDisparity) <= FLT_EPSILON ) + dptr[x][2] = bigZ; + } + + if( dtype == CV_16SC3 ) + { + Vec3s* dptr0 = _3dImage.ptr(y); + for( x = 0; x < cols; x++ ) + { + dptr0[x] = dptr[x]; + } + } + else if( dtype == CV_32SC3 ) + { + Vec3i* dptr0 = _3dImage.ptr(y); + for( x = 0; x < cols; x++ ) + { + dptr0[x] = dptr[x]; + } + } + } +} + +void stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1, + InputArray _cameraMatrix2, InputArray _distCoeffs2, + Size imageSize, InputArray R, InputArray T, + OutputArray _R1, OutputArray _R2, + OutputArray _P1, OutputArray _P2, + OutputArray _Qmat, int flags, + double alpha, Size newImgSize, + Rect* roi1, Rect* roi2 ) +{ + Mat matR = Mat_(R.getMat()), matT = Mat_(T.getMat()); + + Mat om, r_r; + Mat Z = Mat::zeros(3, 1, CV_64F); + double nx = imageSize.width, ny = imageSize.height; + + if( matR.rows == 3 && matR.cols == 3 ) + Rodrigues(matR, om); // get vector rotation + else + matR.copyTo(om); + om *= -0.5; // get average rotation + Rodrigues(om, r_r); + Mat t = r_r * matT; // rotate cameras to same orientation by averaging + + int idx = fabs(t.at(0)) > fabs(t.at(1)) ? 0 : 1; + double c = t.at(idx), nt = norm(t, NORM_L2); + double _uu[3]={0, 0, 0}; + _uu[idx] = c > 0 ? 1 : -1; + + CV_Assert(nt > 0.0); + + // calculate global Z rotation + Mat ww = t.cross(Mat(3, 1, CV_64F, _uu)), wR; + double nw = norm(ww, NORM_L2); + if (nw > 0.0) + ww *= std::acos(fabs(c)/nt)/nw; + Rodrigues(ww, wR); + + Mat Ri; + // apply to both views + gemm(wR, r_r, 1, Mat(), 0, Ri, GEMM_2_T); + Ri.copyTo(_R1); + gemm(wR, r_r, 1, Mat(), 0, Ri, 0); + Ri.copyTo(_R2); + t = Ri * matT; + + // calculate projection/camera matrices + // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) + Point2d cc_new[2]={}; + + newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize; + const double ratio_x = (double)newImgSize.width / imageSize.width / 2; + const double ratio_y = (double)newImgSize.height / imageSize.height / 2; + const double ratio = idx == 1 ? ratio_x : ratio_y; + + Mat cameraMatrix1 = Mat_(_cameraMatrix1.getMat()); + Mat cameraMatrix2 = Mat_(_cameraMatrix2.getMat()); + Mat distCoeffs1, distCoeffs2; + if (!_distCoeffs1.empty()) + distCoeffs1 = Mat_(_distCoeffs1.getMat()); + if (!_distCoeffs2.empty()) + distCoeffs2 = Mat_(_distCoeffs2.getMat()); + + double fc_new = (cameraMatrix1.at(idx ^ 1, idx ^ 1) + cameraMatrix2.at(idx ^ 1, idx ^ 1)) * ratio; + + for( int k = 0; k < 2; k++ ) + { + const Mat& A = k == 0 ? cameraMatrix1 : cameraMatrix2; + const Mat& Dk = k == 0 ? distCoeffs1 : distCoeffs2; + Point2f _pts[4] = {}; + Point3f _pts_3[4] = {}; + Mat pts(1, 4, CV_32FC2, _pts); + Mat pts_3(1, 4, CV_32FC3, _pts_3); + + for( int i = 0; i < 4; i++ ) + { + int j = (i<2) ? 0 : 1; + _pts[i].x = (float)((i % 2)*(nx-1)); + _pts[i].y = (float)(j*(ny-1)); + } + undistortPoints(pts, pts, A, Dk, Mat(), Mat()); + convertPointsToHomogeneous(pts, pts_3); + + // Change the camera matrix to have cc=[0,0] and fc = fc_new + double _a_tmp[3][3] = {{fc_new, 0, 0}, {0, fc_new, 0}, {0, 0, 1}}; + Mat A_tmp(3, 3, CV_64F, _a_tmp); + projectPoints(pts_3, (k == 0 ? _R1 : _R2), Z, A_tmp, Mat(), pts); + Scalar avg = mean(pts); + cc_new[k].x = (nx-1)/2 - avg.val[0]; + cc_new[k].y = (ny-1)/2 - avg.val[1]; + } + + // vertical focal length must be the same for both images to keep the epipolar constraint + // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines) + // use fy for fx also, for simplicity + + // 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 & CALIB_ZERO_DISPARITY ) + { + cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; + cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + } + else if( idx == 0 ) // horizontal stereo + cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + else // vertical stereo + cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; + + double t_idx = t.at(idx); + + Mat pp = Mat::zeros(3, 4, CV_64F); + pp.at(0, 0) = pp.at(1, 1) = fc_new; + pp.at(0, 2) = cc_new[0].x; + pp.at(1, 2) = cc_new[0].y; + pp.at(2, 2) = 1.; + pp.copyTo(_P1); + + pp.at(0, 2) = cc_new[1].x; + pp.at(1, 2) = cc_new[1].y; + pp.at(idx, 3) = t_idx*fc_new; // baseline * focal length + pp.copyTo(_P2); + + alpha = MIN(alpha, 1.); + + cv::Rect_ inner1, inner2, outer1, outer2; + getUndistortRectangles(cameraMatrix1, distCoeffs1, _R1, _P1, imageSize, inner1, outer1); + getUndistortRectangles(cameraMatrix2, distCoeffs2, _R2, _P2, imageSize, inner2, outer2); + + { + newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize; + double cx1_0 = cc_new[0].x; + double cy1_0 = cc_new[0].y; + double cx2_0 = cc_new[1].x; + double cy2_0 = cc_new[1].y; + double cx1 = newImgSize.width*cx1_0/imageSize.width; + double cy1 = newImgSize.height*cy1_0/imageSize.height; + double cx2 = newImgSize.width*cx2_0/imageSize.width; + double cy2 = newImgSize.height*cy2_0/imageSize.height; + double s = 1.; + + if( alpha >= 0 ) + { + double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)), + (double)(newImgSize.width - 1 - cx1)/(inner1.x + inner1.width - cx1_0)), + (double)(newImgSize.height - 1 - cy1)/(inner1.y + inner1.height - cy1_0)); + s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)), + (double)(newImgSize.width - 1 - cx2)/(inner2.x + inner2.width - cx2_0)), + (double)(newImgSize.height - 1 - cy2)/(inner2.y + inner2.height - cy2_0)), + s0); + + double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)), + (double)(newImgSize.width - 1 - cx1)/(outer1.x + outer1.width - cx1_0)), + (double)(newImgSize.height - 1 - cy1)/(outer1.y + outer1.height - cy1_0)); + s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)), + (double)(newImgSize.width - 1 - cx2)/(outer2.x + outer2.width - cx2_0)), + (double)(newImgSize.height - 1 - cy2)/(outer2.y + outer2.height - cy2_0)), + s1); + + s = s0*(1 - alpha) + s1*alpha; + } + + fc_new *= s; + cc_new[0] = Point2d(cx1, cy1); + cc_new[1] = Point2d(cx2, cy2); + + pp.at(0, 0) = pp.at(1, 1) = fc_new; + pp.at(0, 2) = cx2; + pp.at(1, 2) = cy2; + pp.at(idx, 3) *= s; + pp.copyTo(_P2); + + pp.at(0, 2) = cx1; + pp.at(1, 2) = cy1; + pp.at(idx, 3) = 0.; + pp.copyTo(_P1); + + if(roi1) + { + *roi1 = + cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1), + cvCeil((inner1.y - cy1_0)*s + cy1), + cvFloor(inner1.width*s), cvFloor(inner1.height*s)) + & cv::Rect(0, 0, newImgSize.width, newImgSize.height) + ; + } + + if(roi2) + { + *roi2 = + cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2), + cvCeil((inner2.y - cy2_0)*s + cy2), + cvFloor(inner2.width*s), cvFloor(inner2.height*s)) + & cv::Rect(0, 0, newImgSize.width, newImgSize.height) + ; + } + } + + if( _Qmat.needed() ) + { + double q[] = + { + 1, 0, 0, -cc_new[0].x, + 0, 1, 0, -cc_new[0].y, + 0, 0, 0, fc_new, + 0, 0, -1./t_idx, + (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/t_idx + }; + Mat Q(4, 4, CV_64F, q); + Q.copyTo(_Qmat); + } +} + +/* +CV_IMPL int cvStereoRectifyUncalibrated( + const CvMat* _points1, const CvMat* _points2, + const CvMat* F0, CvSize imgSize, + CvMat* _H1, CvMat* _H2, double threshold ) +*/ +bool stereoRectifyUncalibrated( InputArray _points1, InputArray _points2, + InputArray _Fmat, Size imgSize, + OutputArray _Hmat1, OutputArray _Hmat2, double threshold ) +{ + Mat points1 = _points1.getMat(), points2 = _points2.getMat(); + CV_Assert( points1.size() == points2.size() ); + + int npoints = points1.checkVector(2); + CV_Assert(npoints > 0); + + Mat _m1, _m2; + + points1.convertTo(_m1, CV_64F); + points2.convertTo(_m2, CV_64F); + _m1 = _m1.reshape(2, 1); + _m2 = _m2.reshape(2, 1); + + Mat F0 = _Fmat.getMat(), F, Wdiag, U, Vt; + F0.convertTo(F, CV_64F); + + SVDecomp(F, Wdiag, U, Vt, 0); + Wdiag.at(2) = 0.; + Mat W = Mat::diag(Wdiag), UW; + gemm(U, W, 1, Mat(), 0, UW); + gemm(UW, Vt, 1, Mat(), 0, F); + + double cx = cvRound( (imgSize.width-1)*0.5 ); + double cy = cvRound( (imgSize.height-1)*0.5 ); + + if( threshold > 0 ) + { + Mat _lines1, _lines2; + computeCorrespondEpilines(_m1, 1, F, _lines1); + computeCorrespondEpilines(_m2, 2, F, _lines2); + CV_Assert(_m1.isContinuous() && _m2.isContinuous() && + _lines1.isContinuous() && _lines2.isContinuous()); + Point2d* m1 = (Point2d*)_m1.data; + Point2d* m2 = (Point2d*)_m2.data; + Point3d* lines1 = (Point3d*)_lines1.data; + Point3d* lines2 = (Point3d*)_lines2.data; + + // measure distance from points to the corresponding epilines, mark outliers + int i, j; + for( i = j = 0; i < npoints; i++ ) + { + if( fabs(m1[i].x*lines2[i].x + + m1[i].y*lines2[i].y + + lines2[i].z) <= threshold && + fabs(m2[i].x*lines1[i].x + + m2[i].y*lines1[i].y + + lines1[i].z) <= threshold ) + { + if( j < i ) + { + m1[j] = m1[i]; + m2[j] = m2[i]; + } + j++; + } + } + + npoints = j; + if( npoints == 0 ) + return false; + _m1.cols = _m2.cols = npoints; + } + + Mat E2 = U.col(2).clone(); + if (E2.at(2) < 0) + E2 *= -1.0; + + double t[] = + { + 1, 0, -cx, + 0, 1, -cy, + 0, 0, 1 + }; + Mat T(3, 3, CV_64F, t); + E2 = T*E2; + + double* e2 = (double*)E2.data; + int mirror = e2[0] < 0; + double d = std::sqrt(e2[0]*e2[0] + e2[1]*e2[1]); + d = MAX(d, DBL_EPSILON); + double alpha = e2[0]/d; + double beta = e2[1]/d; + double r[] = + { + alpha, beta, 0, + -beta, alpha, 0, + 0, 0, 1 + }; + Mat R(3, 3, CV_64F, r); + T = R*T; + E2 = R*E2; + double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0]; + double k[] = + { + 1, 0, 0, + 0, 1, 0, + invf, 0, 1 + }; + Mat K(3, 3, CV_64F, k); + Mat H2 = K*T; + E2 = K*E2; + + double it[] = + { + 1, 0, cx, + 0, 1, cy, + 0, 0, 1 + }; + Mat iT( 3, 3, CV_64F, it ); + H2 = iT*H2; + + U.col(2).copyTo(E2); + if (E2.at(2) < 0) + E2 *= -1.0; + + double e2_x[] = + { + 0, -e2[2], e2[1], + e2[2], 0, -e2[0], + -e2[1], e2[0], 0 + }; + double e2_111[] = + { + e2[0], e2[0], e2[0], + e2[1], e2[1], e2[1], + e2[2], e2[2], e2[2], + }; + Mat E2_x(3, 3, CV_64F, e2_x); + Mat E2_111(3, 3, CV_64F, e2_111); + Mat H0 = E2_x*F + E2_111; + H0 = H2*H0; + Mat E1(3, 1, CV_64F, (double*)Vt.data+6); + E1 = H0*E1; + + perspectiveTransform( _m1, _m1, H0 ); + perspectiveTransform( _m2, _m2, H2 ); + Mat A, X; + convertPointsToHomogeneous(_m1, A); + A.convertTo(A, CV_64F); + A = A.reshape(1, npoints); + Mat BxBy = _m2.reshape(1, npoints); + Mat B = BxBy.col(0); + solve(A, B, X, DECOMP_SVD); + CV_Assert(X.isContinuous()); + double* x = X.ptr(); + + double ha[] = + { + x[0], x[1], x[2], + 0, 1, 0, + 0, 0, 1 + }; + Mat Ha(3, 3, CV_64F, ha); + Mat H1 = Ha*H0; + perspectiveTransform( _m1, _m1, Ha ); + + if( mirror ) + { + double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 }; + Mat MM(3, 3, CV_64F, mm); + H1 = MM*H1; + H2 = MM*H2; + } + + H1.copyTo(_Hmat1); + H2.copyTo(_Hmat2); + return true; +} + + +static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0, + InputArrayOfArrays _imgpt3_0, + const Mat& cameraMatrix1, const Mat& distCoeffs1, + const Mat& cameraMatrix3, const Mat& distCoeffs3, + const Mat& R1, const Mat& R3, const Mat& P1, Mat& P3 ) +{ + size_t n1 = _imgpt1_0.total(), n3 = _imgpt3_0.total(); + std::vector imgpt1, imgpt3; + + for( int i = 0; i < (int)std::min(n1, n3); i++ ) + { + Mat pt1 = _imgpt1_0.getMat(i), pt3 = _imgpt3_0.getMat(i); + int ni1 = pt1.checkVector(2, CV_32F), ni3 = pt3.checkVector(2, CV_32F); + CV_Assert( ni1 > 0 && ni1 == ni3 ); + const Point2f* pt1data = pt1.ptr(); + const Point2f* pt3data = pt3.ptr(); + std::copy(pt1data, pt1data + ni1, std::back_inserter(imgpt1)); + std::copy(pt3data, pt3data + ni3, std::back_inserter(imgpt3)); + } + + undistortPoints(imgpt1, imgpt1, cameraMatrix1, distCoeffs1, R1, P1); + undistortPoints(imgpt3, imgpt3, cameraMatrix3, distCoeffs3, R3, P3); + + double y1_ = 0, y2_ = 0, y1y1_ = 0, y1y2_ = 0; + size_t n = imgpt1.size(); + CV_DbgAssert(n > 0); + + for( size_t i = 0; i < n; i++ ) + { + double y1 = imgpt3[i].y, y2 = imgpt1[i].y; + + y1_ += y1; y2_ += y2; + y1y1_ += y1*y1; y1y2_ += y1*y2; + } + + y1_ /= n; + y2_ /= n; + y1y1_ /= n; + y1y2_ /= n; + + double a = (y1y2_ - y1_*y2_)/(y1y1_ - y1_*y1_); + double b = y2_ - a*y1_; + + P3.at(0,0) *= a; + P3.at(1,1) *= a; + P3.at(0,2) = P3.at(0,2)*a; + P3.at(1,2) = P3.at(1,2)*a + b; + P3.at(0,3) *= a; + P3.at(1,3) *= a; +} + +float rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1, + InputArray _cameraMatrix2, InputArray _distCoeffs2, + InputArray _cameraMatrix3, InputArray _distCoeffs3, + InputArrayOfArrays _imgpt1, + InputArrayOfArrays _imgpt3, + Size imageSize, InputArray _Rmat12, InputArray _Tmat12, + InputArray _Rmat13, InputArray _Tmat13, + OutputArray _Rmat1, OutputArray _Rmat2, OutputArray _Rmat3, + OutputArray _Pmat1, OutputArray _Pmat2, OutputArray _Pmat3, + OutputArray _Qmat, + double alpha, Size newImgSize, + Rect* roi1, Rect* roi2, int flags ) +{ + // first, rectify the 1-2 stereo pair + stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2, + imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat, + flags, alpha, newImgSize, roi1, roi2 ); + + Mat R12 = _Rmat12.getMat(), R13 = _Rmat13.getMat(), T12 = _Tmat12.getMat(), T13 = _Tmat13.getMat(); + + _Rmat3.create(3, 3, CV_64F); + _Pmat3.create(3, 4, CV_64F); + + Mat P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(); + Mat R3 = _Rmat3.getMat(), P3 = _Pmat3.getMat(); + + // recompute rectification transforms for cameras 1 & 2. + Mat om, r_r, r_r13; + + if( R13.size() != Size(3,3) ) + Rodrigues(R13, r_r13); + else + R13.copyTo(r_r13); + + if( R12.size() == Size(3,3) ) + Rodrigues(R12, om); + else + R12.copyTo(om); + + om *= -0.5; + Rodrigues(om, r_r); // rotate cameras to same orientation by averaging + Mat_ t12 = r_r * T12; + + int idx = fabs(t12(0,0)) > fabs(t12(1,0)) ? 0 : 1; + double c = t12(idx,0), nt = norm(t12, NORM_L2); + CV_Assert(fabs(nt) > 0); + Mat_ uu = Mat_::zeros(3,1); + uu(idx, 0) = c > 0 ? 1 : -1; + + // calculate global Z rotation + Mat_ ww = t12.cross(uu), wR; + double nw = norm(ww, NORM_L2); + CV_Assert(fabs(nw) > 0); + ww *= std::acos(fabs(c)/nt)/nw; + Rodrigues(ww, wR); + + // now rotate camera 3 to make its optical axis parallel to cameras 1 and 2. + R3 = wR*r_r.t()*r_r13.t(); + Mat_ t13 = R3 * T13; + + P2.copyTo(P3); + Mat t = P3.col(3); + t13.copyTo(t); + P3.at(0,3) *= P3.at(0,0); + P3.at(1,3) *= P3.at(1,1); + + if( !_imgpt1.empty() && !_imgpt3.empty() ) + adjust3rdMatrix(_imgpt1, _imgpt3, _cameraMatrix1.getMat(), _distCoeffs1.getMat(), + _cameraMatrix3.getMat(), _distCoeffs3.getMat(), _Rmat1.getMat(), R3, P1, P3); + + return (float)((P3.at(idx,3)/P3.at(idx,idx))/ + (P2.at(idx,3)/P2.at(idx,idx))); +} + +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_INSTRUMENT_REGION(); + + 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)); + + + Mat aaa = _tvec.getMat().reshape(3, 1); + + Vec3d rvec; // Rodrigues vector + if (_R.size() == Size(3, 3)) + { + 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 *= std::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; + fisheye::estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, R1, newK1, balance, newImageSize, fov_scale); + fisheye::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 & 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()); +} + +} diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp index 830dc63aa6..7e09bd26a4 100644 --- a/modules/calib3d/src/undistort.dispatch.cpp +++ b/modules/calib3d/src/undistort.dispatch.cpp @@ -40,12 +40,11 @@ // //M*/ +#include "opencv2/core/core_c.h" #include "opencv2/core/types.hpp" #include "precomp.hpp" #include "distortion_model.hpp" -#include "calib3d_c_api.h" - #include "undistort.simd.hpp" #include "undistort.simd_declarations.hpp" // defines CV_CPU_DISPATCH_MODES_ALL=AVX2,...,BASELINE based on CMakeLists.txt content @@ -508,14 +507,6 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM } } -void cvUndistortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, - const CvMat* _distCoeffs, - const CvMat* matR, const CvMat* matP) -{ - cvUndistortPointsInternal(_src, _dst, _cameraMatrix, _distCoeffs, matR, matP, - cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01)); -} - namespace cv { void undistortPoints(InputArray _src, OutputArray _dst,