From d10616775bd435c507332d6a447901a635f6c884 Mon Sep 17 00:00:00 2001 From: Marina Kolpakova Date: Sun, 18 Mar 2012 17:28:44 +0000 Subject: [PATCH] Fixed bug #1634 --- modules/calib3d/src/calibration.cpp | 92 ++++++++++++++++++------------------- 1 file changed, 46 insertions(+), 46 deletions(-) diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 9539cc7..4995128 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1047,7 +1047,7 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, { dpdk_p[4] = fx*x*icdist2*r6; dpdk_p[dpdk_step+4] = fy*y*icdist2*r6; - + if( _dpdk->cols > 5 ) { dpdk_p[5] = fx*x*cdist*(-icdist2)*icdist2*r2; @@ -1361,7 +1361,7 @@ CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints, CvSize imageSize, CvMat* cameraMatrix, double aspectRatio ) { - Ptr matA, _b, _allH, _allK; + Ptr matA, _b, _allH; int i, j, pos, nimages, ni = 0; double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 }; @@ -2303,7 +2303,7 @@ icvGetRectangles( const CvMat* cameraMatrix, const CvMat* distCoeffs, if( x == 0 ) iX0 = MAX(iX0, p.x); if( x == N-1 ) - iX1 = MIN(iX1, p.x); + iX1 = MIN(iX1, p.x); if( y == 0 ) iY0 = MAX(iY0, p.y); if( y == N-1 ) @@ -2521,7 +2521,7 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, cvConvert( &Q, matQ ); } } - + void cvGetOptimalNewCameraMatrix( const CvMat* cameraMatrix, const CvMat* distCoeffs, CvSize imgSize, double alpha, @@ -2810,7 +2810,7 @@ void cv::reprojectImageTo3D( InputArray _disparity, 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_32FC3; else @@ -2838,7 +2838,7 @@ void cv::reprojectImageTo3D( InputArray _disparity, // 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, *dptr = dbuf; @@ -2865,7 +2865,7 @@ void cv::reprojectImageTo3D( InputArray _disparity, } else sptr = (float*)disparity.ptr(y); - + if( dtype == CV_32FC3 ) dptr = _3dImage.ptr(y); @@ -3183,7 +3183,7 @@ static void collectCalibrationData( InputArrayOfArrays objectPoints, npoints.at(i) = ni; memcpy( objPtData + j, objpt.data, ni*sizeof(objPtData[0]) ); memcpy( imgPtData1 + j, imgpt1.data, ni*sizeof(imgPtData1[0]) ); - + if( imgPtData2 ) { Mat imgpt2 = imagePoints2.getMat(i); @@ -3361,7 +3361,7 @@ void cv::projectPoints( InputArray _opoints, pdpdc = &(dpdc = jacobian.colRange(8, 10)); pdpddist = &(dpddist = jacobian.colRange(10, 10+ndistCoeffs)); } - + cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs, &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio ); } @@ -3406,9 +3406,9 @@ double cv::calibrateCamera( InputArrayOfArrays _objectPoints, double reprojErr = cvCalibrateCamera2(&c_objPt, &c_imgPt, &c_npoints, imageSize, &c_cameraMatrix, &c_distCoeffs, &c_rvecM, &c_tvecM, flags ); - + bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(); - + if( rvecs_needed ) _rvecs.create((int)nimages, 1, CV_64FC3); if( tvecs_needed ) @@ -3465,13 +3465,13 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype); distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype); distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype); - + if( !(flags & CALIB_RATIONAL_MODEL) ) { distCoeffs1 = distCoeffs1.rows == 1 ? distCoeffs1.colRange(0, 5) : distCoeffs1.rowRange(0, 5); distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5); } - + _Rmat.create(3, 3, rtype); _Tmat.create(3, 1, rtype); @@ -3483,7 +3483,7 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, CvMat c_cameraMatrix1 = cameraMatrix1, c_distCoeffs1 = distCoeffs1; CvMat c_cameraMatrix2 = cameraMatrix2, c_distCoeffs2 = distCoeffs2; CvMat c_matR = _Rmat.getMat(), c_matT = _Tmat.getMat(), c_matE, c_matF, *p_matE = 0, *p_matF = 0; - + if( _Emat.needed() ) { _Emat.create(3, 3, rtype); @@ -3498,12 +3498,12 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, double err = cvStereoCalibrate(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1, &c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, imageSize, &c_matR, &c_matT, p_matE, p_matF, criteria, flags ); - + cameraMatrix1.copyTo(_cameraMatrix1); cameraMatrix2.copyTo(_cameraMatrix2); distCoeffs1.copyTo(_distCoeffs1); distCoeffs2.copyTo(_distCoeffs2); - + return err; } @@ -3525,7 +3525,7 @@ void cv::stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1, CvMat c_distCoeffs1 = distCoeffs1; CvMat c_distCoeffs2 = distCoeffs2; CvMat c_R = Rmat, c_T = Tmat; - + int rtype = CV_64F; _Rmat1.create(3, 3, rtype); _Rmat2.create(3, 3, rtype); @@ -3533,13 +3533,13 @@ void cv::stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1, _Pmat2.create(3, 4, rtype); CvMat c_R1 = _Rmat1.getMat(), c_R2 = _Rmat2.getMat(), c_P1 = _Pmat1.getMat(), c_P2 = _Pmat2.getMat(); CvMat c_Q, *p_Q = 0; - + if( _Qmat.needed() ) { _Qmat.create(4, 4, rtype); p_Q = &(c_Q = _Qmat.getMat()); } - + cvStereoRectify( &c_cameraMatrix1, &c_cameraMatrix2, &c_distCoeffs1, &c_distCoeffs2, imageSize, &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha, newImageSize, (CvRect*)validPixROI1, (CvRect*)validPixROI2); @@ -3568,10 +3568,10 @@ cv::Mat cv::getOptimalNewCameraMatrix( InputArray _cameraMatrix, { Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; - + Mat newCameraMatrix(3, 3, CV_MAT_TYPE(c_cameraMatrix.type)); CvMat c_newCameraMatrix = newCameraMatrix; - + cvGetOptimalNewCameraMatrix(&c_cameraMatrix, &c_distCoeffs, imgSize, alpha, &c_newCameraMatrix, newImgSize, (CvRect*)validPixROI, (int)centerPrincipalPoint); @@ -3628,7 +3628,7 @@ void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraM CvMat c_rotMatrixY, *p_rotMatrixY = 0; CvMat c_rotMatrixZ, *p_rotMatrixZ = 0; CvPoint3D64f *p_eulerAngles = 0; - + if( _rotMatrixX.needed() ) { _rotMatrixX.create(3, 3, type); @@ -3649,7 +3649,7 @@ void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraM _eulerAngles.create(3, 1, CV_64F, -1, true); p_eulerAngles = (CvPoint3D64f*)_eulerAngles.getMat().data; } - + cvDecomposeProjectionMatrix(&c_projMatrix, &c_cameraMatrix, &c_rotMatrix, &c_transVect, p_rotMatrixX, p_rotMatrixY, p_rotMatrixZ, p_eulerAngles); @@ -3658,16 +3658,16 @@ void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraM namespace cv { - + static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0, - InputArrayOfArrays _imgpt3_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(); 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); @@ -3678,29 +3678,29 @@ static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0, 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(); - + 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; @@ -3728,57 +3728,57 @@ float cv::rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1, stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat, flags, alpha, imageSize, 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); 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); 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))); } -- 2.7.4