diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index 28a8f645f1..62361d4df3 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -2412,6 +2412,32 @@ float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs, m1type, map1, map2, (UndistortTypes)projType, alpha); } +/** @brief Computes useful camera characteristics from the camera intrinsic matrix. + * + * @param cameraMatrix Input camera intrinsic matrix that can be estimated by #calibrateCamera or + * #stereoCalibrate . + * @param imageSize Input image size in pixels. + * @param apertureWidth Physical width in mm of the sensor. + * @param apertureHeight Physical height in mm of the sensor. + * @param fovx Output field of view in degrees along the horizontal sensor axis. + * @param fovy Output field of view in degrees along the vertical sensor axis. + * @param focalLength Focal length of the lens in mm. + * @param principalPoint Principal point in mm. + * @param aspectRatio \f$f_y/f_x\f$ + * + * The function computes various useful camera characteristics from the previously estimated camera + * matrix. + * + * @note + * Do keep in mind that the unity measure 'mm' stands for whatever unit of measure one chooses for + * the chessboard pitch (it can thus be any value). + */ +CV_EXPORTS_W void calibrationMatrixValues( InputArray cameraMatrix, Size imageSize, + double apertureWidth, double apertureHeight, + CV_OUT double& fovx, CV_OUT double& fovy, + CV_OUT double& focalLength, CV_OUT Point2d& principalPoint, + CV_OUT double& aspectRatio ); + /** @brief Returns the default new camera matrix. The function returns the camera matrix that is either an exact copy of the input cameraMatrix (when diff --git a/modules/3d/src/undistort.dispatch.cpp b/modules/3d/src/undistort.dispatch.cpp index fbdea7f13e..fc6ee6a271 100644 --- a/modules/3d/src/undistort.dispatch.cpp +++ b/modules/3d/src/undistort.dispatch.cpp @@ -66,6 +66,46 @@ Mat getDefaultNewCameraMatrix( InputArray _cameraMatrix, Size imgsize, return newCameraMatrix; } +void calibrationMatrixValues( InputArray _cameraMatrix, Size imageSize, + double apertureWidth, double apertureHeight, + double& fovx, double& fovy, double& focalLength, + Point2d& principalPoint, double& aspectRatio ) +{ + CV_INSTRUMENT_REGION(); + + if(_cameraMatrix.size() != Size(3, 3)) + CV_Error(cv::Error::StsUnmatchedSizes, "Size of cameraMatrix must be 3x3!"); + + Matx33d A; + _cameraMatrix.getMat().convertTo(A, CV_64F); + CV_DbgAssert(imageSize.width != 0 && imageSize.height != 0 && A(0, 0) != 0.0 && A(1, 1) != 0.0); + + /* Calculate pixel aspect ratio. */ + aspectRatio = A(1, 1) / A(0, 0); + + /* Calculate number of pixel per realworld unit. */ + double mx, my; + if(apertureWidth != 0.0 && apertureHeight != 0.0) { + mx = imageSize.width / apertureWidth; + my = imageSize.height / apertureHeight; + } else { + mx = 1.0; + my = aspectRatio; + } + + /* Calculate fovx and fovy. */ + fovx = atan2(A(0, 2), A(0, 0)) + atan2(imageSize.width - A(0, 2), A(0, 0)); + fovy = atan2(A(1, 2), A(1, 1)) + atan2(imageSize.height - A(1, 2), A(1, 1)); + fovx *= 180.0 / CV_PI; + fovy *= 180.0 / CV_PI; + + /* Calculate focal length. */ + focalLength = A(0, 0) / mx; + + /* Calculate principle point. */ + principalPoint = Point2d(A(0, 2) / mx, A(1, 2) / my); +} + static Ptr getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type, const double* _ir, Matx33d &_matTilt, double _u0, double _v0, double _fx, double _fy, diff --git a/modules/3d/test/test_cameras.cpp b/modules/3d/test/test_cameras.cpp new file mode 100644 index 0000000000..966ecdc4c7 --- /dev/null +++ b/modules/3d/test/test_cameras.cpp @@ -0,0 +1,1398 @@ +// 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 "test_precomp.hpp" +#include "opencv2/3d.hpp" +#include "opencv2/ts/cuda_test.hpp" // EXPECT_MAT_NEAR + +namespace opencv_test { namespace { + +#if 0 +class CV_ProjectPointsTest : public cvtest::ArrayTest + { +public: + CV_ProjectPointsTest(); + +protected: + int read_params( const cv::FileStorage& fs ); + void fill_array( int test_case_idx, int i, int j, Mat& arr ); + int prepare_test_case( int test_case_idx ); + void get_test_array_types_and_sizes( int test_case_idx, vector >& sizes, vector >& types ); + double get_success_error_level( int test_case_idx, int i, int j ); + void run_func(); + void prepare_to_validation( int ); + + bool calc_jacobians; +}; + + +CV_ProjectPointsTest::CV_ProjectPointsTest() + : cvtest::ArrayTest( "3d-ProjectPoints", "cvProjectPoints2", "" ) +{ + test_array[INPUT].push_back(NULL); // rotation vector + test_array[OUTPUT].push_back(NULL); // rotation matrix + test_array[OUTPUT].push_back(NULL); // jacobian (J) + test_array[OUTPUT].push_back(NULL); // rotation vector (backward transform result) + test_array[OUTPUT].push_back(NULL); // inverse transform jacobian (J1) + test_array[OUTPUT].push_back(NULL); // J*J1 (or J1*J) == I(3x3) + test_array[REF_OUTPUT].push_back(NULL); + test_array[REF_OUTPUT].push_back(NULL); + test_array[REF_OUTPUT].push_back(NULL); + test_array[REF_OUTPUT].push_back(NULL); + test_array[REF_OUTPUT].push_back(NULL); + + element_wise_relative_error = false; + calc_jacobians = false; +} + + +int CV_ProjectPointsTest::read_params( const cv::FileStorage& fs ) +{ + int code = cvtest::ArrayTest::read_params( fs ); + return code; +} + + +void CV_ProjectPointsTest::get_test_array_types_and_sizes( + int /*test_case_idx*/, vector >& sizes, vector >& types ) +{ + RNG& rng = ts->get_rng(); + int depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; + int i, code; + + code = cvtest::randInt(rng) % 3; + types[INPUT][0] = CV_MAKETYPE(depth, 1); + + if( code == 0 ) + { + sizes[INPUT][0] = cvSize(1,1); + types[INPUT][0] = CV_MAKETYPE(depth, 3); + } + else if( code == 1 ) + sizes[INPUT][0] = cvSize(3,1); + else + sizes[INPUT][0] = cvSize(1,3); + + sizes[OUTPUT][0] = cvSize(3, 3); + types[OUTPUT][0] = CV_MAKETYPE(depth, 1); + + types[OUTPUT][1] = CV_MAKETYPE(depth, 1); + + if( cvtest::randInt(rng) % 2 ) + sizes[OUTPUT][1] = cvSize(3,9); + else + sizes[OUTPUT][1] = cvSize(9,3); + + types[OUTPUT][2] = types[INPUT][0]; + sizes[OUTPUT][2] = sizes[INPUT][0]; + + types[OUTPUT][3] = types[OUTPUT][1]; + sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width); + + types[OUTPUT][4] = types[OUTPUT][1]; + sizes[OUTPUT][4] = cvSize(3,3); + + calc_jacobians = 1;//cvtest::randInt(rng) % 3 != 0; + if( !calc_jacobians ) + sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0); + + for( i = 0; i < 5; i++ ) + { + types[REF_OUTPUT][i] = types[OUTPUT][i]; + sizes[REF_OUTPUT][i] = sizes[OUTPUT][i]; + } +} + + +double CV_ProjectPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j ) +{ + return j == 4 ? 1e-2 : 1e-2; +} + + +void CV_ProjectPointsTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr ) +{ + double r[3], theta0, theta1, f; + CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r ); + RNG& rng = ts->get_rng(); + + r[0] = cvtest::randReal(rng)*CV_PI*2; + r[1] = cvtest::randReal(rng)*CV_PI*2; + r[2] = cvtest::randReal(rng)*CV_PI*2; + + theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); + theta1 = fmod(theta0, CV_PI*2); + + if( theta1 > CV_PI ) + theta1 = -(CV_PI*2 - theta1); + + f = theta1/(theta0 ? theta0 : 1); + r[0] *= f; + r[1] *= f; + r[2] *= f; + + cvTsConvert( &_r, arr ); +} + + +int CV_ProjectPointsTest::prepare_test_case( int test_case_idx ) +{ + int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); + return code; +} + + +void CV_ProjectPointsTest::run_func() +{ + CvMat *v2m_jac = 0, *m2v_jac = 0; + if( calc_jacobians ) + { + v2m_jac = &test_mat[OUTPUT][1]; + m2v_jac = &test_mat[OUTPUT][3]; + } + + cvProjectPoints2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac ); + cvProjectPoints2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac ); +} + + +void CV_ProjectPointsTest::prepare_to_validation( int /*test_case_idx*/ ) +{ + const CvMat* vec = &test_mat[INPUT][0]; + CvMat* m = &test_mat[REF_OUTPUT][0]; + CvMat* vec2 = &test_mat[REF_OUTPUT][2]; + CvMat* v2m_jac = 0, *m2v_jac = 0; + double theta0, theta1; + + if( calc_jacobians ) + { + v2m_jac = &test_mat[REF_OUTPUT][1]; + m2v_jac = &test_mat[REF_OUTPUT][3]; + } + + + cvTsProjectPoints( vec, m, v2m_jac ); + cvTsProjectPoints( m, vec2, m2v_jac ); + cvTsCopy( vec, vec2 ); + + theta0 = cvtest::norm( cvarrtomat(vec2), 0, NORM_L2 ); + theta1 = fmod( theta0, CV_PI*2 ); + + if( theta1 > CV_PI ) + theta1 = -(CV_PI*2 - theta1); + cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) ); + + if( calc_jacobians ) + { + //cvInvert( v2m_jac, m2v_jac, CV_SVD ); + if( cvtest::norm(cvarrtomat(&test_mat[OUTPUT][3]), 0, NORM_INF) < 1000 ) + { + cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3], + 1, 0, 0, &test_mat[OUTPUT][4], + v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T ); + } + else + { + cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) ); + cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] ); + } + cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) ); + } +} + +CV_ProjectPointsTest ProjectPoints_test; + +#endif + +//----------------------------------------- CV_ProjectPointsTest -------------------------------- +void calcdfdx( const vector >& leftF, const vector >& rightF, double eps, Mat& dfdx ) +{ + const int fdim = 2; + CV_Assert( !leftF.empty() && !rightF.empty() && !leftF[0].empty() && !rightF[0].empty() ); + CV_Assert( leftF[0].size() == rightF[0].size() ); + CV_Assert( fabs(eps) > std::numeric_limits::epsilon() ); + int fcount = (int)leftF[0].size(), xdim = (int)leftF.size(); + + dfdx.create( fcount*fdim, xdim, CV_64FC1 ); + + vector >::const_iterator arrLeftIt = leftF.begin(); + vector >::const_iterator arrRightIt = rightF.begin(); + for( int xi = 0; xi < xdim; xi++, ++arrLeftIt, ++arrRightIt ) + { + CV_Assert( (int)arrLeftIt->size() == fcount ); + CV_Assert( (int)arrRightIt->size() == fcount ); + vector::const_iterator lIt = arrLeftIt->begin(); + vector::const_iterator rIt = arrRightIt->begin(); + for( int fi = 0; fi < dfdx.rows; fi+=fdim, ++lIt, ++rIt ) + { + dfdx.at(fi, xi ) = 0.5 * ((double)(rIt->x - lIt->x)) / eps; + dfdx.at(fi+1, xi ) = 0.5 * ((double)(rIt->y - lIt->y)) / eps; + } + } +} + + +class CV_ProjectPointsTest : public cvtest::BaseTest +{ +public: + CV_ProjectPointsTest() {} +protected: + void run(int); + virtual void project( const Mat& objectPoints, + const Mat& rvec, const Mat& tvec, + const Mat& cameraMatrix, + const Mat& distCoeffs, + vector& imagePoints, + Mat& dpdrot, Mat& dpdt, Mat& dpdf, + Mat& dpdc, Mat& dpddist, + double aspectRatio=0 ) = 0; +}; + +void CV_ProjectPointsTest::run(int) +{ + //typedef float matType; + + int code = cvtest::TS::OK; + const int pointCount = 100; + + const float zMinVal = 10.0f, zMaxVal = 100.0f, + rMinVal = -0.3f, rMaxVal = 0.3f, + tMinVal = -2.0f, tMaxVal = 2.0f; + + const float imgPointErr = 1e-3f, + dEps = 1e-3f; + + double err; + + Size imgSize( 600, 800 ); + Mat_ objPoints( pointCount, 3), rvec( 1, 3), rmat, tvec( 1, 3 ), cameraMatrix( 3, 3 ), distCoeffs( 1, 4 ), + leftRvec, rightRvec, leftTvec, rightTvec, leftCameraMatrix, rightCameraMatrix, leftDistCoeffs, rightDistCoeffs; + + RNG rng = ts->get_rng(); + + // generate data + cameraMatrix << 300.f, 0.f, imgSize.width/2.f, + 0.f, 300.f, imgSize.height/2.f, + 0.f, 0.f, 1.f; + distCoeffs << 0.1, 0.01, 0.001, 0.001; + + rvec(0,0) = rng.uniform( rMinVal, rMaxVal ); + rvec(0,1) = rng.uniform( rMinVal, rMaxVal ); + rvec(0,2) = rng.uniform( rMinVal, rMaxVal ); + rmat = cv::Mat_::zeros(3, 3); + Rodrigues( rvec, rmat ); + + tvec(0,0) = rng.uniform( tMinVal, tMaxVal ); + tvec(0,1) = rng.uniform( tMinVal, tMaxVal ); + tvec(0,2) = rng.uniform( tMinVal, tMaxVal ); + + for( int y = 0; y < objPoints.rows; y++ ) + { + Mat point(1, 3, CV_32FC1, objPoints.ptr(y) ); + float z = rng.uniform( zMinVal, zMaxVal ); + point.at(0,2) = z; + point.at(0,0) = (rng.uniform(2.f,(float)(imgSize.width-2)) - cameraMatrix(0,2)) / cameraMatrix(0,0) * z; + point.at(0,1) = (rng.uniform(2.f,(float)(imgSize.height-2)) - cameraMatrix(1,2)) / cameraMatrix(1,1) * z; + point = (point - tvec) * rmat; + } + + vector imgPoints; + vector > leftImgPoints; + vector > rightImgPoints; + Mat dpdrot, dpdt, dpdf, dpdc, dpddist, + valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist; + + project( objPoints, rvec, tvec, cameraMatrix, distCoeffs, + imgPoints, dpdrot, dpdt, dpdf, dpdc, dpddist, 0 ); + + // calculate and check image points + CV_Assert( (int)imgPoints.size() == pointCount ); + vector::const_iterator it = imgPoints.begin(); + for( int i = 0; i < pointCount; i++, ++it ) + { + Point3d p( objPoints(i,0), objPoints(i,1), objPoints(i,2) ); + double z = p.x*rmat(2,0) + p.y*rmat(2,1) + p.z*rmat(2,2) + tvec(0,2), + x = (p.x*rmat(0,0) + p.y*rmat(0,1) + p.z*rmat(0,2) + tvec(0,0)) / z, + y = (p.x*rmat(1,0) + p.y*rmat(1,1) + p.z*rmat(1,2) + tvec(0,1)) / z, + r2 = x*x + y*y, + r4 = r2*r2; + Point2f validImgPoint; + double a1 = 2*x*y, + a2 = r2 + 2*x*x, + a3 = r2 + 2*y*y, + cdist = 1+distCoeffs(0,0)*r2+distCoeffs(0,1)*r4; + validImgPoint.x = static_cast((double)cameraMatrix(0,0)*(x*cdist + (double)distCoeffs(0,2)*a1 + (double)distCoeffs(0,3)*a2) + + (double)cameraMatrix(0,2)); + validImgPoint.y = static_cast((double)cameraMatrix(1,1)*(y*cdist + (double)distCoeffs(0,2)*a3 + distCoeffs(0,3)*a1) + + (double)cameraMatrix(1,2)); + + if( fabs(it->x - validImgPoint.x) > imgPointErr || + fabs(it->y - validImgPoint.y) > imgPointErr ) + { + ts->printf( cvtest::TS::LOG, "bad image point\n" ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + goto _exit_; + } + } + + // check derivatives + // 1. rotation + leftImgPoints.resize(3); + rightImgPoints.resize(3); + for( int i = 0; i < 3; i++ ) + { + rvec.copyTo( leftRvec ); leftRvec(0,i) -= dEps; + project( objPoints, leftRvec, tvec, cameraMatrix, distCoeffs, + leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + rvec.copyTo( rightRvec ); rightRvec(0,i) += dEps; + project( objPoints, rightRvec, tvec, cameraMatrix, distCoeffs, + rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + } + calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdrot ); + err = cvtest::norm( dpdrot, valDpdrot, NORM_INF ); + if( err > 3 ) + { + ts->printf( cvtest::TS::LOG, "bad dpdrot: too big difference = %g\n", err ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + } + + // 2. translation + for( int i = 0; i < 3; i++ ) + { + tvec.copyTo( leftTvec ); leftTvec(0,i) -= dEps; + project( objPoints, rvec, leftTvec, cameraMatrix, distCoeffs, + leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + tvec.copyTo( rightTvec ); rightTvec(0,i) += dEps; + project( objPoints, rvec, rightTvec, cameraMatrix, distCoeffs, + rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + } + calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdt ); + if( cvtest::norm( dpdt, valDpdt, NORM_INF ) > 0.2 ) + { + ts->printf( cvtest::TS::LOG, "bad dpdtvec\n" ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + } + + // 3. camera matrix + // 3.1. focus + leftImgPoints.resize(2); + rightImgPoints.resize(2); + cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,0) -= dEps; + project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, + leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,1) -= dEps; + project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, + leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,0) += dEps; + project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, + rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,1) += dEps; + project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, + rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdf ); + if ( cvtest::norm( dpdf, valDpdf, NORM_L2 ) > 0.2 ) + { + ts->printf( cvtest::TS::LOG, "bad dpdf\n" ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + } + // 3.2. principal point + leftImgPoints.resize(2); + rightImgPoints.resize(2); + cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,2) -= dEps; + project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, + leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,2) -= dEps; + project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, + leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,2) += dEps; + project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, + rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,2) += dEps; + project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, + rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdc ); + if ( cvtest::norm( dpdc, valDpdc, NORM_L2 ) > 0.2 ) + { + ts->printf( cvtest::TS::LOG, "bad dpdc\n" ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + } + + // 4. distortion + leftImgPoints.resize(distCoeffs.cols); + rightImgPoints.resize(distCoeffs.cols); + for( int i = 0; i < distCoeffs.cols; i++ ) + { + distCoeffs.copyTo( leftDistCoeffs ); leftDistCoeffs(0,i) -= dEps; + project( objPoints, rvec, tvec, cameraMatrix, leftDistCoeffs, + leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + distCoeffs.copyTo( rightDistCoeffs ); rightDistCoeffs(0,i) += dEps; + project( objPoints, rvec, tvec, cameraMatrix, rightDistCoeffs, + rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); + } + calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpddist ); + if( cvtest::norm( dpddist, valDpddist, NORM_L2 ) > 0.3 ) + { + ts->printf( cvtest::TS::LOG, "bad dpddist\n" ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + } + + _exit_: + RNG& _rng = ts->get_rng(); + _rng = rng; + ts->set_failed_test_info( code ); +} + +//----------------------------------------- CV_ProjectPointsTest_CPP -------------------------------- +class CV_ProjectPointsTest_CPP : public CV_ProjectPointsTest +{ +public: + CV_ProjectPointsTest_CPP() {} +protected: + virtual void project( const Mat& objectPoints, + const Mat& rvec, const Mat& tvec, + const Mat& cameraMatrix, + const Mat& distCoeffs, + vector& imagePoints, + Mat& dpdrot, Mat& dpdt, Mat& dpdf, + Mat& dpdc, Mat& dpddist, + double aspectRatio=0 ); +}; + +void CV_ProjectPointsTest_CPP::project( const Mat& objectPoints, const Mat& rvec, const Mat& tvec, + const Mat& cameraMatrix, const Mat& distCoeffs, vector& imagePoints, + Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio) +{ + Mat J; + projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, J, aspectRatio); + J.colRange(0, 3).copyTo(dpdrot); + J.colRange(3, 6).copyTo(dpdt); + J.colRange(6, 8).copyTo(dpdf); + J.colRange(8, 10).copyTo(dpdc); + J.colRange(10, J.cols).copyTo(dpddist); +} + +TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); } + +TEST(Calib3d_ProjectPoints_CPP, inputShape) +{ + Matx31d rvec = Matx31d::zeros(); + Matx31d tvec(0, 0, 1); + Matx33d cameraMatrix = Matx33d::eye(); + const float L = 0.1f; + { + //3xN 1-channel + Mat objectPoints = (Mat_(3, 2) << -L, L, + L, L, + 0, 0); + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.cols, static_cast(imagePoints.size())); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } + { + //Nx2 1-channel + Mat objectPoints = (Mat_(2, 3) << -L, L, 0, + L, L, 0); + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.rows, static_cast(imagePoints.size())); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } + { + //1xN 3-channel + Mat objectPoints(1, 2, CV_32FC3); + objectPoints.at(0,0) = Vec3f(-L, L, 0); + objectPoints.at(0,1) = Vec3f(L, L, 0); + + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.cols, static_cast(imagePoints.size())); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } + { + //Nx1 3-channel + Mat objectPoints(2, 1, CV_32FC3); + objectPoints.at(0,0) = Vec3f(-L, L, 0); + objectPoints.at(1,0) = Vec3f(L, L, 0); + + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.rows, static_cast(imagePoints.size())); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } + { + //vector + vector objectPoints; + objectPoints.push_back(Point3f(-L, L, 0)); + objectPoints.push_back(Point3f(L, L, 0)); + + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.size(), imagePoints.size()); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } + { + //vector + vector objectPoints; + objectPoints.push_back(Point3d(-L, L, 0)); + objectPoints.push_back(Point3d(L, L, 0)); + + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.size(), imagePoints.size()); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } +} + +TEST(Calib3d_ProjectPoints_CPP, outputShape) +{ + Matx31d rvec = Matx31d::zeros(); + Matx31d tvec(0, 0, 1); + Matx33d cameraMatrix = Matx33d::eye(); + const float L = 0.1f; + { + vector objectPoints; + objectPoints.push_back(Point3f(-L, L, 0)); + objectPoints.push_back(Point3f( L, L, 0)); + objectPoints.push_back(Point3f( L, -L, 0)); + + //Mat --> will be Nx1 2-channel + Mat imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(static_cast(objectPoints.size()), imagePoints.rows); + EXPECT_NEAR(imagePoints.at(0,0)(0), -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,0)(1), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(1,0)(0), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(1,0)(1), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(2,0)(0), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(2,0)(1), -L, std::numeric_limits::epsilon()); + } + { + vector objectPoints; + objectPoints.push_back(Point3f(-L, L, 0)); + objectPoints.push_back(Point3f( L, L, 0)); + objectPoints.push_back(Point3f( L, -L, 0)); + + //Nx1 2-channel + Mat imagePoints(3,1,CV_32FC2); + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(static_cast(objectPoints.size()), imagePoints.rows); + EXPECT_NEAR(imagePoints.at(0,0)(0), -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,0)(1), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(1,0)(0), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(1,0)(1), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(2,0)(0), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(2,0)(1), -L, std::numeric_limits::epsilon()); + } + { + vector objectPoints; + objectPoints.push_back(Point3f(-L, L, 0)); + objectPoints.push_back(Point3f( L, L, 0)); + objectPoints.push_back(Point3f( L, -L, 0)); + + //1xN 2-channel + Mat imagePoints(1,3,CV_32FC2); + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(static_cast(objectPoints.size()), imagePoints.cols); + EXPECT_NEAR(imagePoints.at(0,0)(0), -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,0)(1), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,1)(0), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,1)(1), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,2)(0), L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints.at(0,2)(1), -L, std::numeric_limits::epsilon()); + } + { + vector objectPoints; + objectPoints.push_back(Point3f(-L, L, 0)); + objectPoints.push_back(Point3f(L, L, 0)); + + //vector + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.size(), imagePoints.size()); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } + { + vector objectPoints; + objectPoints.push_back(Point3d(-L, L, 0)); + objectPoints.push_back(Point3d(L, L, 0)); + + //vector + vector imagePoints; + projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); + EXPECT_EQ(objectPoints.size(), imagePoints.size()); + EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); + EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); + } +} + +#define NUM_DIST_COEFF_TILT 14 + +class cameraCalibrationTiltTest : public ::testing::Test { + +protected: + + static const cv::Size m_imageSize; + static const double m_pixelSize; + static const double m_circleConfusionPixel; + static const double m_lensFocalLength; + static const double m_lensFNumber; + static const double m_objectDistance; + static const double m_planeTiltDegree; + static const double m_pointTargetDist; + static const int m_pointTargetNum; + + /** image distance corresponding to working distance */ + double m_imageDistance; + /** image tilt angle corresponding to the tilt of the object plane */ + double m_imageTiltDegree; + /** center of the field of view, near and far plane */ + std::vector m_fovCenter; + /** normal of the field of view, near and far plane */ + std::vector m_fovNormal; + /** points on a plane calibration target */ + std::vector m_pointTarget; + /** rotations for the calibration target */ + std::vector m_pointTargetRvec; + /** translations for the calibration target */ + std::vector m_pointTargetTvec; + /** camera matrix */ + cv::Matx33d m_cameraMatrix; + /** distortion coefficients */ + cv::Vec m_distortionCoeff; + + /** random generator */ + cv::RNG m_rng; + /** degree to radian conversion factor */ + const double m_toRadian; + /** radian to degree conversion factor */ + const double m_toDegree; + + cameraCalibrationTiltTest() + : m_toRadian(acos(-1.0)/180.0) + , m_toDegree(180.0/acos(-1.0)) + {} + + /** + C *hanges given distortion coefficients randomly by adding + a uniformly distributed random variable in [-max max] + \param coeff input + \param max limits for the random variables + */ + void randomDistortionCoeff( + cv::Vec& coeff, + const cv::Vec& max) + { + for (int i = 0; i < coeff.rows; ++i) + coeff(i) += m_rng.uniform(-max(i), max(i)); + } + + void removeInvalidPoints( + std::vector& imagePoints, + std::vector& objectPoints) + { + // remove object and imgage points out of range + std::vector::iterator itImg = imagePoints.begin(); + std::vector::iterator itObj = objectPoints.begin(); + while (itImg != imagePoints.end()) + { + bool ok = + itImg->x >= 0 && + itImg->x <= m_imageSize.width - 1.0 && + itImg->y >= 0 && + itImg->y <= m_imageSize.height - 1.0; + if (ok) + { + ++itImg; + ++itObj; + } + else + { + itImg = imagePoints.erase(itImg); + itObj = objectPoints.erase(itObj); + } + } + } + + void numericalDerivative( + cv::Mat& jac, + double eps, + const std::vector& obj, + const cv::Vec3d& rvec, + const cv::Vec3d& tvec, + const cv::Matx33d& camera, + const cv::Vec& distor) + { + cv::Vec3d r(rvec); + cv::Vec3d t(tvec); + cv::Matx33d cm(camera); + cv::Vec dc(distor); + double* param[10+NUM_DIST_COEFF_TILT] = { + &r(0), &r(1), &r(2), + &t(0), &t(1), &t(2), + &cm(0,0), &cm(1,1), &cm(0,2), &cm(1,2), + &dc(0), &dc(1), &dc(2), &dc(3), &dc(4), &dc(5), &dc(6), + &dc(7), &dc(8), &dc(9), &dc(10), &dc(11), &dc(12), &dc(13)}; + std::vector pix0, pix1; + double invEps = .5/eps; + + for (int col = 0; col < 10+NUM_DIST_COEFF_TILT; ++col) + { + double save = *(param[col]); + *(param[col]) = save + eps; + cv::projectPoints(obj, r, t, cm, dc, pix0); + *(param[col]) = save - eps; + cv::projectPoints(obj, r, t, cm, dc, pix1); + *(param[col]) = save; + + std::vector::const_iterator it0 = pix0.begin(); + std::vector::const_iterator it1 = pix1.begin(); + int row = 0; + for (;it0 != pix0.end(); ++it0, ++it1) + { + cv::Point2d d = invEps*(*it0 - *it1); + jac.at(row, col) = d.x; + ++row; + jac.at(row, col) = d.y; + ++row; + } + } + } + + /** + c *omputes for a given distance of an image or object point + the distance of the corresponding object or image point + */ + double opticalMap(double dist) { + return m_lensFocalLength*dist/(dist - m_lensFocalLength); + } + + /** magnification of the optical map */ + double magnification(double dist) { + return m_lensFocalLength/(dist - m_lensFocalLength); + } + + void SetUp() + { + m_imageDistance = opticalMap(m_objectDistance); + m_imageTiltDegree = m_toDegree * atan2( + m_imageDistance * tan(m_toRadian * m_planeTiltDegree), + m_objectDistance); + // half sensor height + double tmp = .5 * (m_imageSize.height - 1) * m_pixelSize + * cos(m_toRadian * m_imageTiltDegree); + // y-Value of tilted sensor + double yImage[2] = {tmp, -tmp}; + // change in z because of the tilt + tmp *= sin(m_toRadian * m_imageTiltDegree); + // z-values of the sensor lower and upper corner + double zImage[2] = { + m_imageDistance + tmp, + m_imageDistance - tmp}; + // circle of confusion + double circleConfusion = m_circleConfusionPixel*m_pixelSize; + // aperture of the lense + double aperture = m_lensFocalLength/m_lensFNumber; + // near and far factor on the image side + double nearFarFactorImage[2] = { + aperture/(aperture - circleConfusion), + aperture/(aperture + circleConfusion)}; + // on the object side - points that determine the field of + // view + std::vector fovBottomTop(6); + std::vector::iterator itFov = fovBottomTop.begin(); + for (size_t iBottomTop = 0; iBottomTop < 2; ++iBottomTop) + { + // mapping sensor to field of view + *itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]); + *itFov *= magnification((*itFov)(2)); + ++itFov; + for (size_t iNearFar = 0; iNearFar < 2; ++iNearFar, ++itFov) + { + // scaling to the near and far distance on the + // image side + *itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]) * + nearFarFactorImage[iNearFar]; + // scaling to the object side + *itFov *= magnification((*itFov)(2)); + } + } + m_fovCenter.resize(3); + m_fovNormal.resize(3); + for (size_t i = 0; i < 3; ++i) + { + m_fovCenter[i] = .5*(fovBottomTop[i] + fovBottomTop[i+3]); + m_fovNormal[i] = fovBottomTop[i+3] - fovBottomTop[i]; + m_fovNormal[i] = cv::normalize(m_fovNormal[i]); + m_fovNormal[i] = cv::Vec3d( + m_fovNormal[i](0), + -m_fovNormal[i](2), + m_fovNormal[i](1)); + // one target position in each plane + m_pointTargetTvec.push_back(m_fovCenter[i]); + cv::Vec3d rvec = cv::Vec3d(0,0,1).cross(m_fovNormal[i]); + rvec = cv::normalize(rvec); + rvec *= acos(m_fovNormal[i](2)); + m_pointTargetRvec.push_back(rvec); + } + // calibration target + size_t num = 2*m_pointTargetNum + 1; + m_pointTarget.resize(num*num); + std::vector::iterator itTarget = m_pointTarget.begin(); + for (int iY = -m_pointTargetNum; iY <= m_pointTargetNum; ++iY) + { + for (int iX = -m_pointTargetNum; iX <= m_pointTargetNum; ++iX, ++itTarget) + { + *itTarget = cv::Point3d(iX, iY, 0) * m_pointTargetDist; + } + } + // oblique target positions + // approximate distance to the near and far plane + double dist = std::max( + std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[1])), + std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[2]))); + // maximal angle such that target border "reaches" near and far plane + double maxAngle = atan2(dist, m_pointTargetNum*m_pointTargetDist); + std::vector angle; + angle.push_back(-maxAngle); + angle.push_back(maxAngle); + cv::Matx33d baseMatrix; + cv::Rodrigues(m_pointTargetRvec.front(), baseMatrix); + for (std::vector::const_iterator itAngle = angle.begin(); itAngle != angle.end(); ++itAngle) + { + cv::Matx33d rmat; + for (int i = 0; i < 2; ++i) + { + cv::Vec3d rvec(0,0,0); + rvec(i) = *itAngle; + cv::Rodrigues(rvec, rmat); + rmat = baseMatrix*rmat; + cv::Rodrigues(rmat, rvec); + m_pointTargetTvec.push_back(m_fovCenter.front()); + m_pointTargetRvec.push_back(rvec); + } + } + // camera matrix + double cx = .5 * (m_imageSize.width - 1); + double cy = .5 * (m_imageSize.height - 1); + double f = m_imageDistance/m_pixelSize; + m_cameraMatrix = cv::Matx33d( + f,0,cx, + 0,f,cy, + 0,0,1); + // distortion coefficients + m_distortionCoeff = cv::Vec::all(0); + // tauX + m_distortionCoeff(12) = -m_toRadian*m_imageTiltDegree; + } +}; + +/** Number of Pixel of the sensor */ +const cv::Size cameraCalibrationTiltTest::m_imageSize(1600, 1200); +/** Size of a pixel in mm */ +const double cameraCalibrationTiltTest::m_pixelSize(.005); +/** Diameter of the circle of confusion */ +const double cameraCalibrationTiltTest::m_circleConfusionPixel(3); +/** Focal length of the lens */ +const double cameraCalibrationTiltTest::m_lensFocalLength(16.4); +/** F-Number */ +const double cameraCalibrationTiltTest::m_lensFNumber(8); +/** Working distance */ +const double cameraCalibrationTiltTest::m_objectDistance(200); +/** Angle between optical axis and object plane normal */ +const double cameraCalibrationTiltTest::m_planeTiltDegree(55); +/** the calibration target are points on a square grid with this side length */ +const double cameraCalibrationTiltTest::m_pointTargetDist(5); +/** the calibration target has (2*n + 1) x (2*n + 1) points */ +const int cameraCalibrationTiltTest::m_pointTargetNum(15); + +TEST_F(cameraCalibrationTiltTest, projectPoints) +{ + std::vector imagePoints; + std::vector objectPoints = m_pointTarget; + cv::Vec3d rvec = m_pointTargetRvec.front(); + cv::Vec3d tvec = m_pointTargetTvec.front(); + + cv::Vec coeffNoiseHalfWidth( + .1, .1, // k1 k2 + .01, .01, // p1 p2 + .001, .001, .001, .001, // k3 k4 k5 k6 + .001, .001, .001, .001, // s1 s2 s3 s4 + .01, .01); // tauX tauY + for (size_t numTest = 0; numTest < 10; ++numTest) + { + // create random distortion coefficients + cv::Vec distortionCoeff = m_distortionCoeff; + randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth); + + // projection + cv::projectPoints( + objectPoints, + rvec, + tvec, + m_cameraMatrix, + distortionCoeff, + imagePoints); + + // remove object and imgage points out of range + removeInvalidPoints(imagePoints, objectPoints); + + int numPoints = (int)imagePoints.size(); + int numParams = 10 + distortionCoeff.rows; + cv::Mat jacobian(2*numPoints, numParams, CV_64FC1); + + // projection and jacobian + cv::projectPoints( + objectPoints, + rvec, + tvec, + m_cameraMatrix, + distortionCoeff, + imagePoints, + jacobian); + + // numerical derivatives + cv::Mat numericJacobian(2*numPoints, numParams, CV_64FC1); + double eps = 1e-7; + numericalDerivative( + numericJacobian, + eps, + objectPoints, + rvec, + tvec, + m_cameraMatrix, + distortionCoeff); + +#if 0 + for (size_t row = 0; row < 2; ++row) + { + std::cout << "------ Row = " << row << " ------\n"; + for (size_t i = 0; i < 10+NUM_DIST_COEFF_TILT; ++i) + { + std::cout << i + << " jac = " << jacobian.at(row,i) + << " num = " << numericJacobian.at(row,i) + << " rel. diff = " << abs(numericJacobian.at(row,i) - jacobian.at(row,i))/abs(numericJacobian.at(row,i)) + << "\n"; + } + } +#endif + // relative difference for large values (rvec and tvec) + cv::Mat check = abs(jacobian(cv::Range::all(), cv::Range(0,6)) - numericJacobian(cv::Range::all(), cv::Range(0,6)))/ + (1 + abs(jacobian(cv::Range::all(), cv::Range(0,6)))); + double minVal, maxVal; + cv::minMaxIdx(check, &minVal, &maxVal); + EXPECT_LE(maxVal, .01); + // absolute difference for distortion and camera matrix + EXPECT_MAT_NEAR(jacobian(cv::Range::all(), cv::Range(6,numParams)), numericJacobian(cv::Range::all(), cv::Range(6,numParams)), 1e-5); + } +} + +TEST_F(cameraCalibrationTiltTest, undistortPoints) +{ + cv::Vec coeffNoiseHalfWidth( + .2, .1, // k1 k2 + .01, .01, // p1 p2 + .01, .01, .01, .01, // k3 k4 k5 k6 + .001, .001, .001, .001, // s1 s2 s3 s4 + .001, .001); // tauX tauY + double step = 99; + double toleranceBackProjection = 1e-5; + + for (size_t numTest = 0; numTest < 10; ++numTest) + { + cv::Vec distortionCoeff = m_distortionCoeff; + randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth); + + // distorted points + std::vector distorted; + for (double x = 0; x <= m_imageSize.width-1; x += step) + for (double y = 0; y <= m_imageSize.height-1; y += step) + distorted.push_back(cv::Point2d(x,y)); + std::vector normalizedUndistorted; + + // undistort + cv::undistortPoints(distorted, + normalizedUndistorted, + m_cameraMatrix, + distortionCoeff); + + // copy normalized points to 3D + std::vector objectPoints; + for (std::vector::const_iterator itPnt = normalizedUndistorted.begin(); + itPnt != normalizedUndistorted.end(); ++itPnt) + objectPoints.push_back(cv::Point3d(itPnt->x, itPnt->y, 1)); + + // project + std::vector imagePoints(objectPoints.size()); + cv::projectPoints(objectPoints, + cv::Vec3d(0,0,0), + cv::Vec3d(0,0,0), + m_cameraMatrix, + distortionCoeff, + imagePoints); + + EXPECT_MAT_NEAR(distorted, imagePoints, toleranceBackProjection); + } +} + +TEST(Calib3d_Triangulate, accuracy) +{ + // the testcase from http://code.opencv.org/issues/4334 + { + double P1data[] = { 250, 0, 200, 0, 0, 250, 150, 0, 0, 0, 1, 0 }; + double P2data[] = { 250, 0, 200, -250, 0, 250, 150, 0, 0, 0, 1, 0 }; + Mat P1(3, 4, CV_64F, P1data), P2(3, 4, CV_64F, P2data); + + float x1data[] = { 200.f, 0.f }; + float x2data[] = { 170.f, 1.f }; + float Xdata[] = { 0.f, -5.f, 25/3.f }; + Mat x1(2, 1, CV_32F, x1data); + Mat x2(2, 1, CV_32F, x2data); + Mat res0(1, 3, CV_32F, Xdata); + Mat res_, res; + + triangulatePoints(P1, P2, x1, x2, res_); + cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace) + convertPointsFromHomogeneous(res_, res); + res = res.reshape(1, 1); + + cout << "[1]:" << endl; + cout << "\tres0: " << res0 << endl; + cout << "\tres: " << res << endl; + + ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 1e-1); + } + + // another testcase http://code.opencv.org/issues/3461 + { + Matx33d K1(6137.147949, 0.000000, 644.974609, + 0.000000, 6137.147949, 573.442749, + 0.000000, 0.000000, 1.000000); + Matx33d K2(6137.147949, 0.000000, 644.674438, + 0.000000, 6137.147949, 573.079834, + 0.000000, 0.000000, 1.000000); + + Matx34d RT1(1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0); + + Matx34d RT2(0.998297, 0.0064108, -0.0579766, 143.614334, + -0.0065818, 0.999975, -0.00275888, -5.160085, + 0.0579574, 0.00313577, 0.998314, 96.066109); + + Matx34d P1 = K1*RT1; + Matx34d P2 = K2*RT2; + + float x1data[] = { 438.f, 19.f }; + float x2data[] = { 452.363600f, 16.452225f }; + float Xdata[] = { -81.049530f, -215.702804f, 2401.645449f }; + Mat x1(2, 1, CV_32F, x1data); + Mat x2(2, 1, CV_32F, x2data); + Mat res0(1, 3, CV_32F, Xdata); + Mat res_, res; + + triangulatePoints(P1, P2, x1, x2, res_); + cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace) + convertPointsFromHomogeneous(res_, res); + res = res.reshape(1, 1); + + cout << "[2]:" << endl; + cout << "\tres0: " << res0 << endl; + cout << "\tres: " << res << endl; + + ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 2); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +TEST(CV_RecoverPoseTest, regression_15341) +{ + // initialize test data + const int invalid_point_count = 2; + const float _points1_[] = { + 1537.7f, 166.8f, + 1599.1f, 179.6f, + 1288.0f, 207.5f, + 1507.1f, 193.2f, + 1742.7f, 210.0f, + 1041.6f, 271.7f, + 1591.8f, 247.2f, + 1524.0f, 261.3f, + 1330.3f, 285.0f, + 1403.1f, 284.0f, + 1506.6f, 342.9f, + 1502.8f, 347.3f, + 1344.9f, 364.9f, + 0.0f, 0.0f // last point is initial invalid + }; + + const float _points2_[] = { + 1533.4f, 532.9f, + 1596.6f, 552.4f, + 1277.0f, 556.4f, + 1502.1f, 557.6f, + 1744.4f, 601.3f, + 1023.0f, 612.6f, + 1589.2f, 621.6f, + 1519.4f, 629.0f, + 1320.3f, 637.3f, + 1395.2f, 642.2f, + 1501.5f, 710.3f, + 1497.6f, 714.2f, + 1335.1f, 719.61f, + 1000.0f, 1000.0f // last point is initial invalid + }; + + vector _points1; Mat(14, 1, CV_32FC2, (void*)_points1_).copyTo(_points1); + vector _points2; Mat(14, 1, CV_32FC2, (void*)_points2_).copyTo(_points2); + + const int point_count = (int) _points1.size(); + CV_Assert(point_count == (int) _points2.size()); + + // camera matrix with both focal lengths = 1, and principal point = (0, 0) + const Mat cameraMatrix = Mat::eye(3, 3, CV_64F); + + // camera matrix with focal lengths 0.5 and 0.6 respectively and principal point = (100, 200) + double cameraMatrix2Data[] = { 0.5, 0, 100, + 0, 0.6, 200, + 0, 0, 1 }; + const Mat cameraMatrix2( 3, 3, CV_64F, cameraMatrix2Data ); + + // zero and nonzero distortion coefficients + double nonZeroDistCoeffsData[] = { 0.01, 0.0001, 0, 0, 1e-04, 0.2, 0.02, 0.0002 }; // k1, k2, p1, p2, k3, k4, k5, k6 + vector distCoeffsList = {Mat::zeros(1, 5, CV_64F), Mat{1, 8, CV_64F, nonZeroDistCoeffsData}}; + const auto &zeroDistCoeffs = distCoeffsList[0]; + + int Inliers = 0; + + const int ntests = 3; + for (int testcase = 1; testcase <= ntests; ++testcase) + { + if (testcase == 1) // testcase with vector input data + { + // init temporary test data + vector mask(point_count); + vector points1(_points1); + vector points2(_points2); + + // Estimation of fundamental matrix using the RANSAC algorithm + Mat E, E2, R, t; + + // Check pose when camera matrices are different. + for (const auto &distCoeffs: distCoeffsList) + { + E = findEssentialMat(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, RANSAC, 0.999, 1.0, mask); + recoverPose(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, E2, R, t, RANSAC, 0.999, 1.0, mask); + EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << + "Two big difference between the same essential matrices computed using different functions with different cameras, testcase " << testcase; + EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function failed with different cameras, testcase " << testcase; + } + + // Check pose when camera matrices are the same. + E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, 1000/*maxIters*/, mask); + E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask); + EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << + "Two big difference between the same essential matrices computed using different functions with same cameras, testcase " << testcase; + EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function findEssentialMat failed with same cameras, testcase " << testcase; + points2[12] = Point2f(0.0f, 0.0f); // provoke another outlier detection for recover Pose + Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask); + EXPECT_EQ(0, (int)mask[12]) << "Detecting outliers in function failed with same cameras, testcase " << testcase; + } + else // testcase with mat input data + { + Mat points1(_points1, true); + Mat points2(_points2, true); + Mat mask; + + if (testcase == 2) + { + // init temporary testdata + mask = Mat::zeros(point_count, 1, CV_8UC1); + } + else // testcase == 3 - with transposed mask + { + mask = Mat::zeros(1, point_count, CV_8UC1); + } + + // Estimation of fundamental matrix using the RANSAC algorithm + Mat E, E2, R, t; + + // Check pose when camera matrices are different. + for (const auto &distCoeffs: distCoeffsList) + { + E = findEssentialMat(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, RANSAC, 0.999, 1.0, mask); + recoverPose(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, E2, R, t, RANSAC, 0.999, 1.0, mask); + EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << + "Two big difference between the same essential matrices computed using different functions with different cameras, testcase " << testcase; + EXPECT_EQ(0, (int)mask.at(13)) << "Detecting outliers in function failed with different cameras, testcase " << testcase; + } + + // Check pose when camera matrices are the same. + E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, 1000/*maxIters*/, mask); + E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask); + EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << + "Two big difference between the same essential matrices computed using different functions with same cameras, testcase " << testcase; + EXPECT_EQ(0, (int)mask.at(13)) << "Detecting outliers in function findEssentialMat failed with same cameras, testcase " << testcase; + points2.at(12) = Point2f(0.0f, 0.0f); // provoke an outlier detection + Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask); + EXPECT_EQ(0, (int)mask.at(12)) << "Detecting outliers in function failed with same cameras, testcase " << testcase; + } + EXPECT_EQ(Inliers, point_count - invalid_point_count) << + "Number of inliers differs from expected number of inliers, testcase " << testcase; + } +} + +//----------------------------------------- CV_CalibrationMatrixValuesTest -------------------------------- + +class CV_CalibrationMatrixValuesTest : public cvtest::BaseTest +{ +public: + CV_CalibrationMatrixValuesTest() {} +protected: + void run(int); + virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize, + double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, + Point2d& principalPoint, double& aspectRatio ) = 0; +}; + +void CV_CalibrationMatrixValuesTest::run(int) +{ + int code = cvtest::TS::OK; + const double fcMinVal = 1e-5; + const double fcMaxVal = 1000; + const double apertureMaxVal = 0.01; + + RNG rng = ts->get_rng(); + + double fx, fy, cx, cy, nx, ny; + Mat cameraMatrix( 3, 3, CV_64FC1 ); + cameraMatrix.setTo( Scalar(0) ); + fx = cameraMatrix.at(0,0) = rng.uniform( fcMinVal, fcMaxVal ); + fy = cameraMatrix.at(1,1) = rng.uniform( fcMinVal, fcMaxVal ); + cx = cameraMatrix.at(0,2) = rng.uniform( fcMinVal, fcMaxVal ); + cy = cameraMatrix.at(1,2) = rng.uniform( fcMinVal, fcMaxVal ); + cameraMatrix.at(2,2) = 1; + + Size imageSize( 600, 400 ); + + double apertureWidth = (double)rng * apertureMaxVal, + apertureHeight = (double)rng * apertureMaxVal; + + double fovx, fovy, focalLength, aspectRatio, + goodFovx, goodFovy, goodFocalLength, goodAspectRatio; + Point2d principalPoint, goodPrincipalPoint; + + + calibMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight, + fovx, fovy, focalLength, principalPoint, aspectRatio ); + + // calculate calibration matrix values + goodAspectRatio = fy / fx; + + if( apertureWidth != 0.0 && apertureHeight != 0.0 ) + { + nx = imageSize.width / apertureWidth; + ny = imageSize.height / apertureHeight; + } + else + { + nx = 1.0; + ny = goodAspectRatio; + } + + goodFovx = (atan2(cx, fx) + atan2(imageSize.width - cx, fx)) * 180.0 / CV_PI; + goodFovy = (atan2(cy, fy) + atan2(imageSize.height - cy, fy)) * 180.0 / CV_PI; + + goodFocalLength = fx / nx; + + goodPrincipalPoint.x = cx / nx; + goodPrincipalPoint.y = cy / ny; + + // check results + if( fabs(fovx - goodFovx) > FLT_EPSILON ) + { + ts->printf( cvtest::TS::LOG, "bad fovx (real=%f, good = %f\n", fovx, goodFovx ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + goto _exit_; + } + if( fabs(fovy - goodFovy) > FLT_EPSILON ) + { + ts->printf( cvtest::TS::LOG, "bad fovy (real=%f, good = %f\n", fovy, goodFovy ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + goto _exit_; + } + if( fabs(focalLength - goodFocalLength) > FLT_EPSILON ) + { + ts->printf( cvtest::TS::LOG, "bad focalLength (real=%f, good = %f\n", focalLength, goodFocalLength ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + goto _exit_; + } + if( fabs(aspectRatio - goodAspectRatio) > FLT_EPSILON ) + { + ts->printf( cvtest::TS::LOG, "bad aspectRatio (real=%f, good = %f\n", aspectRatio, goodAspectRatio ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + goto _exit_; + } + if( cv::norm(principalPoint - goodPrincipalPoint) > FLT_EPSILON ) // Point2d + { + ts->printf( cvtest::TS::LOG, "bad principalPoint\n" ); + code = cvtest::TS::FAIL_BAD_ACCURACY; + goto _exit_; + } + + _exit_: + RNG& _rng = ts->get_rng(); + _rng = rng; + ts->set_failed_test_info( code ); +} + +//----------------------------------------- CV_CalibrationMatrixValuesTest_CPP -------------------------------- + +class CV_CalibrationMatrixValuesTest_CPP : public CV_CalibrationMatrixValuesTest +{ +public: + CV_CalibrationMatrixValuesTest_CPP() {} +protected: + virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize, + double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, + Point2d& principalPoint, double& aspectRatio ); +}; + +void CV_CalibrationMatrixValuesTest_CPP::calibMatrixValues( const Mat& cameraMatrix, Size imageSize, + double apertureWidth, double apertureHeight, + double& fovx, double& fovy, double& focalLength, + Point2d& principalPoint, double& aspectRatio ) +{ + calibrationMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight, + fovx, fovy, focalLength, principalPoint, aspectRatio ); +} + +TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); } + +}} // namespace diff --git a/modules/calib/include/opencv2/calib.hpp b/modules/calib/include/opencv2/calib.hpp index 97f6249ca7..dcb4fff98c 100644 --- a/modules/calib/include/opencv2/calib.hpp +++ b/modules/calib/include/opencv2/calib.hpp @@ -1051,32 +1051,6 @@ CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints, int flags = 0, TermCriteria criteria = TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) ); -/** @brief Computes useful camera characteristics from the camera intrinsic matrix. - -@param cameraMatrix Input camera intrinsic matrix that can be estimated by #calibrateCamera or -#stereoCalibrate . -@param imageSize Input image size in pixels. -@param apertureWidth Physical width in mm of the sensor. -@param apertureHeight Physical height in mm of the sensor. -@param fovx Output field of view in degrees along the horizontal sensor axis. -@param fovy Output field of view in degrees along the vertical sensor axis. -@param focalLength Focal length of the lens in mm. -@param principalPoint Principal point in mm. -@param aspectRatio \f$f_y/f_x\f$ - -The function computes various useful camera characteristics from the previously estimated camera -matrix. - -@note - Do keep in mind that the unity measure 'mm' stands for whatever unit of measure one chooses for - the chessboard pitch (it can thus be any value). - */ -CV_EXPORTS_W void calibrationMatrixValues( InputArray cameraMatrix, Size imageSize, - double apertureWidth, double apertureHeight, - CV_OUT double& fovx, CV_OUT double& fovy, - CV_OUT double& focalLength, CV_OUT Point2d& principalPoint, - CV_OUT double& aspectRatio ); - /** @brief Calibrates a stereo camera set up. This function finds the intrinsic parameters for each of the two cameras and the extrinsic parameters between the two cameras. diff --git a/modules/calib/src/calibration.cpp b/modules/calib/src/calibration.cpp index 299f0c589a..0e423fe1ad 100644 --- a/modules/calib/src/calibration.cpp +++ b/modules/calib/src/calibration.cpp @@ -3014,47 +3014,6 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints, return reprojErr; } - -void calibrationMatrixValues( InputArray _cameraMatrix, Size imageSize, - double apertureWidth, double apertureHeight, - double& fovx, double& fovy, double& focalLength, - Point2d& principalPoint, double& aspectRatio ) -{ - CV_INSTRUMENT_REGION(); - - if(_cameraMatrix.size() != Size(3, 3)) - CV_Error(cv::Error::StsUnmatchedSizes, "Size of cameraMatrix must be 3x3!"); - - Matx33d A; - _cameraMatrix.getMat().convertTo(A, CV_64F); - CV_DbgAssert(imageSize.width != 0 && imageSize.height != 0 && A(0, 0) != 0.0 && A(1, 1) != 0.0); - - /* Calculate pixel aspect ratio. */ - aspectRatio = A(1, 1) / A(0, 0); - - /* Calculate number of pixel per realworld unit. */ - double mx, my; - if(apertureWidth != 0.0 && apertureHeight != 0.0) { - mx = imageSize.width / apertureWidth; - my = imageSize.height / apertureHeight; - } else { - mx = 1.0; - my = aspectRatio; - } - - /* Calculate fovx and fovy. */ - fovx = atan2(A(0, 2), A(0, 0)) + atan2(imageSize.width - A(0, 2), A(0, 0)); - fovy = atan2(A(1, 2), A(1, 1)) + atan2(imageSize.height - A(1, 2), A(1, 1)); - fovx *= 180.0 / CV_PI; - fovy *= 180.0 / CV_PI; - - /* Calculate focal length. */ - focalLength = A(0, 0) / mx; - - /* Calculate principle point. */ - principalPoint = Point2d(A(0, 2) / mx, A(1, 2) / my); -} - double stereoCalibrate( InputArrayOfArrays _objectPoints, InputArrayOfArrays _imagePoints1, InputArrayOfArrays _imagePoints2, diff --git a/modules/calib/test/test_cameracalibration.cpp b/modules/calib/test/test_cameracalibration.cpp index c5bf3a3ddc..a9e8bd3df7 100644 --- a/modules/calib/test/test_cameracalibration.cpp +++ b/modules/calib/test/test_cameracalibration.cpp @@ -44,204 +44,6 @@ namespace opencv_test { namespace { -#if 0 -class CV_ProjectPointsTest : public cvtest::ArrayTest -{ -public: - CV_ProjectPointsTest(); - -protected: - int read_params( const cv::FileStorage& fs ); - void fill_array( int test_case_idx, int i, int j, Mat& arr ); - int prepare_test_case( int test_case_idx ); - void get_test_array_types_and_sizes( int test_case_idx, vector >& sizes, vector >& types ); - double get_success_error_level( int test_case_idx, int i, int j ); - void run_func(); - void prepare_to_validation( int ); - - bool calc_jacobians; -}; - - -CV_ProjectPointsTest::CV_ProjectPointsTest() - : cvtest::ArrayTest( "3d-ProjectPoints", "cvProjectPoints2", "" ) -{ - test_array[INPUT].push_back(NULL); // rotation vector - test_array[OUTPUT].push_back(NULL); // rotation matrix - test_array[OUTPUT].push_back(NULL); // jacobian (J) - test_array[OUTPUT].push_back(NULL); // rotation vector (backward transform result) - test_array[OUTPUT].push_back(NULL); // inverse transform jacobian (J1) - test_array[OUTPUT].push_back(NULL); // J*J1 (or J1*J) == I(3x3) - test_array[REF_OUTPUT].push_back(NULL); - test_array[REF_OUTPUT].push_back(NULL); - test_array[REF_OUTPUT].push_back(NULL); - test_array[REF_OUTPUT].push_back(NULL); - test_array[REF_OUTPUT].push_back(NULL); - - element_wise_relative_error = false; - calc_jacobians = false; -} - - -int CV_ProjectPointsTest::read_params( const cv::FileStorage& fs ) -{ - int code = cvtest::ArrayTest::read_params( fs ); - return code; -} - - -void CV_ProjectPointsTest::get_test_array_types_and_sizes( - int /*test_case_idx*/, vector >& sizes, vector >& types ) -{ - RNG& rng = ts->get_rng(); - int depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; - int i, code; - - code = cvtest::randInt(rng) % 3; - types[INPUT][0] = CV_MAKETYPE(depth, 1); - - if( code == 0 ) - { - sizes[INPUT][0] = cvSize(1,1); - types[INPUT][0] = CV_MAKETYPE(depth, 3); - } - else if( code == 1 ) - sizes[INPUT][0] = cvSize(3,1); - else - sizes[INPUT][0] = cvSize(1,3); - - sizes[OUTPUT][0] = cvSize(3, 3); - types[OUTPUT][0] = CV_MAKETYPE(depth, 1); - - types[OUTPUT][1] = CV_MAKETYPE(depth, 1); - - if( cvtest::randInt(rng) % 2 ) - sizes[OUTPUT][1] = cvSize(3,9); - else - sizes[OUTPUT][1] = cvSize(9,3); - - types[OUTPUT][2] = types[INPUT][0]; - sizes[OUTPUT][2] = sizes[INPUT][0]; - - types[OUTPUT][3] = types[OUTPUT][1]; - sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width); - - types[OUTPUT][4] = types[OUTPUT][1]; - sizes[OUTPUT][4] = cvSize(3,3); - - calc_jacobians = 1;//cvtest::randInt(rng) % 3 != 0; - if( !calc_jacobians ) - sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0); - - for( i = 0; i < 5; i++ ) - { - types[REF_OUTPUT][i] = types[OUTPUT][i]; - sizes[REF_OUTPUT][i] = sizes[OUTPUT][i]; - } -} - - -double CV_ProjectPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j ) -{ - return j == 4 ? 1e-2 : 1e-2; -} - - -void CV_ProjectPointsTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr ) -{ - double r[3], theta0, theta1, f; - CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r ); - RNG& rng = ts->get_rng(); - - r[0] = cvtest::randReal(rng)*CV_PI*2; - r[1] = cvtest::randReal(rng)*CV_PI*2; - r[2] = cvtest::randReal(rng)*CV_PI*2; - - theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); - theta1 = fmod(theta0, CV_PI*2); - - if( theta1 > CV_PI ) - theta1 = -(CV_PI*2 - theta1); - - f = theta1/(theta0 ? theta0 : 1); - r[0] *= f; - r[1] *= f; - r[2] *= f; - - cvTsConvert( &_r, arr ); -} - - -int CV_ProjectPointsTest::prepare_test_case( int test_case_idx ) -{ - int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); - return code; -} - - -void CV_ProjectPointsTest::run_func() -{ - CvMat *v2m_jac = 0, *m2v_jac = 0; - if( calc_jacobians ) - { - v2m_jac = &test_mat[OUTPUT][1]; - m2v_jac = &test_mat[OUTPUT][3]; - } - - cvProjectPoints2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac ); - cvProjectPoints2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac ); -} - - -void CV_ProjectPointsTest::prepare_to_validation( int /*test_case_idx*/ ) -{ - const CvMat* vec = &test_mat[INPUT][0]; - CvMat* m = &test_mat[REF_OUTPUT][0]; - CvMat* vec2 = &test_mat[REF_OUTPUT][2]; - CvMat* v2m_jac = 0, *m2v_jac = 0; - double theta0, theta1; - - if( calc_jacobians ) - { - v2m_jac = &test_mat[REF_OUTPUT][1]; - m2v_jac = &test_mat[REF_OUTPUT][3]; - } - - - cvTsProjectPoints( vec, m, v2m_jac ); - cvTsProjectPoints( m, vec2, m2v_jac ); - cvTsCopy( vec, vec2 ); - - theta0 = cvtest::norm( cvarrtomat(vec2), 0, NORM_L2 ); - theta1 = fmod( theta0, CV_PI*2 ); - - if( theta1 > CV_PI ) - theta1 = -(CV_PI*2 - theta1); - cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) ); - - if( calc_jacobians ) - { - //cvInvert( v2m_jac, m2v_jac, CV_SVD ); - if( cvtest::norm(cvarrtomat(&test_mat[OUTPUT][3]), 0, NORM_INF) < 1000 ) - { - cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3], - 1, 0, 0, &test_mat[OUTPUT][4], - v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T ); - } - else - { - cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) ); - cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] ); - } - cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) ); - } -} - - -CV_ProjectPointsTest ProjectPoints_test; - -#endif - // --------------------------------- CV_CameraCalibrationTest -------------------------------------------- typedef Matx33d RotMat; @@ -824,399 +626,6 @@ void CV_CameraCalibrationTest_CPP::project( const std::vector& objectPo }*/ } - -//----------------------------------------- CV_CalibrationMatrixValuesTest -------------------------------- - -class CV_CalibrationMatrixValuesTest : public cvtest::BaseTest -{ -public: - CV_CalibrationMatrixValuesTest() {} -protected: - void run(int); - virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize, - double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, - Point2d& principalPoint, double& aspectRatio ) = 0; -}; - -void CV_CalibrationMatrixValuesTest::run(int) -{ - int code = cvtest::TS::OK; - const double fcMinVal = 1e-5; - const double fcMaxVal = 1000; - const double apertureMaxVal = 0.01; - - RNG rng = ts->get_rng(); - - double fx, fy, cx, cy, nx, ny; - Mat cameraMatrix( 3, 3, CV_64FC1 ); - cameraMatrix.setTo( Scalar(0) ); - fx = cameraMatrix.at(0,0) = rng.uniform( fcMinVal, fcMaxVal ); - fy = cameraMatrix.at(1,1) = rng.uniform( fcMinVal, fcMaxVal ); - cx = cameraMatrix.at(0,2) = rng.uniform( fcMinVal, fcMaxVal ); - cy = cameraMatrix.at(1,2) = rng.uniform( fcMinVal, fcMaxVal ); - cameraMatrix.at(2,2) = 1; - - Size imageSize( 600, 400 ); - - double apertureWidth = (double)rng * apertureMaxVal, - apertureHeight = (double)rng * apertureMaxVal; - - double fovx, fovy, focalLength, aspectRatio, - goodFovx, goodFovy, goodFocalLength, goodAspectRatio; - Point2d principalPoint, goodPrincipalPoint; - - - calibMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight, - fovx, fovy, focalLength, principalPoint, aspectRatio ); - - // calculate calibration matrix values - goodAspectRatio = fy / fx; - - if( apertureWidth != 0.0 && apertureHeight != 0.0 ) - { - nx = imageSize.width / apertureWidth; - ny = imageSize.height / apertureHeight; - } - else - { - nx = 1.0; - ny = goodAspectRatio; - } - - goodFovx = (atan2(cx, fx) + atan2(imageSize.width - cx, fx)) * 180.0 / CV_PI; - goodFovy = (atan2(cy, fy) + atan2(imageSize.height - cy, fy)) * 180.0 / CV_PI; - - goodFocalLength = fx / nx; - - goodPrincipalPoint.x = cx / nx; - goodPrincipalPoint.y = cy / ny; - - // check results - if( fabs(fovx - goodFovx) > FLT_EPSILON ) - { - ts->printf( cvtest::TS::LOG, "bad fovx (real=%f, good = %f\n", fovx, goodFovx ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - goto _exit_; - } - if( fabs(fovy - goodFovy) > FLT_EPSILON ) - { - ts->printf( cvtest::TS::LOG, "bad fovy (real=%f, good = %f\n", fovy, goodFovy ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - goto _exit_; - } - if( fabs(focalLength - goodFocalLength) > FLT_EPSILON ) - { - ts->printf( cvtest::TS::LOG, "bad focalLength (real=%f, good = %f\n", focalLength, goodFocalLength ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - goto _exit_; - } - if( fabs(aspectRatio - goodAspectRatio) > FLT_EPSILON ) - { - ts->printf( cvtest::TS::LOG, "bad aspectRatio (real=%f, good = %f\n", aspectRatio, goodAspectRatio ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - goto _exit_; - } - if( cv::norm(principalPoint - goodPrincipalPoint) > FLT_EPSILON ) // Point2d - { - ts->printf( cvtest::TS::LOG, "bad principalPoint\n" ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - goto _exit_; - } - -_exit_: - RNG& _rng = ts->get_rng(); - _rng = rng; - ts->set_failed_test_info( code ); -} - -//----------------------------------------- CV_CalibrationMatrixValuesTest_CPP -------------------------------- - -class CV_CalibrationMatrixValuesTest_CPP : public CV_CalibrationMatrixValuesTest -{ -public: - CV_CalibrationMatrixValuesTest_CPP() {} -protected: - virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize, - double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, - Point2d& principalPoint, double& aspectRatio ); -}; - -void CV_CalibrationMatrixValuesTest_CPP::calibMatrixValues( const Mat& cameraMatrix, Size imageSize, - double apertureWidth, double apertureHeight, - double& fovx, double& fovy, double& focalLength, - Point2d& principalPoint, double& aspectRatio ) -{ - calibrationMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight, - fovx, fovy, focalLength, principalPoint, aspectRatio ); -} - - -//----------------------------------------- CV_ProjectPointsTest -------------------------------- -void calcdfdx( const vector >& leftF, const vector >& rightF, double eps, Mat& dfdx ) -{ - const int fdim = 2; - CV_Assert( !leftF.empty() && !rightF.empty() && !leftF[0].empty() && !rightF[0].empty() ); - CV_Assert( leftF[0].size() == rightF[0].size() ); - CV_Assert( fabs(eps) > std::numeric_limits::epsilon() ); - int fcount = (int)leftF[0].size(), xdim = (int)leftF.size(); - - dfdx.create( fcount*fdim, xdim, CV_64FC1 ); - - vector >::const_iterator arrLeftIt = leftF.begin(); - vector >::const_iterator arrRightIt = rightF.begin(); - for( int xi = 0; xi < xdim; xi++, ++arrLeftIt, ++arrRightIt ) - { - CV_Assert( (int)arrLeftIt->size() == fcount ); - CV_Assert( (int)arrRightIt->size() == fcount ); - vector::const_iterator lIt = arrLeftIt->begin(); - vector::const_iterator rIt = arrRightIt->begin(); - for( int fi = 0; fi < dfdx.rows; fi+=fdim, ++lIt, ++rIt ) - { - dfdx.at(fi, xi ) = 0.5 * ((double)(rIt->x - lIt->x)) / eps; - dfdx.at(fi+1, xi ) = 0.5 * ((double)(rIt->y - lIt->y)) / eps; - } - } -} - -class CV_ProjectPointsTest : public cvtest::BaseTest -{ -public: - CV_ProjectPointsTest() {} -protected: - void run(int); - virtual void project( const Mat& objectPoints, - const Mat& rvec, const Mat& tvec, - const Mat& cameraMatrix, - const Mat& distCoeffs, - vector& imagePoints, - Mat& dpdrot, Mat& dpdt, Mat& dpdf, - Mat& dpdc, Mat& dpddist, - double aspectRatio=0 ) = 0; -}; - -void CV_ProjectPointsTest::run(int) -{ - //typedef float matType; - - int code = cvtest::TS::OK; - const int pointCount = 100; - - const float zMinVal = 10.0f, zMaxVal = 100.0f, - rMinVal = -0.3f, rMaxVal = 0.3f, - tMinVal = -2.0f, tMaxVal = 2.0f; - - const float imgPointErr = 1e-3f, - dEps = 1e-3f; - - double err; - - Size imgSize( 600, 800 ); - Mat_ objPoints( pointCount, 3), rvec( 1, 3), rmat, tvec( 1, 3 ), cameraMatrix( 3, 3 ), distCoeffs( 1, 4 ), - leftRvec, rightRvec, leftTvec, rightTvec, leftCameraMatrix, rightCameraMatrix, leftDistCoeffs, rightDistCoeffs; - - RNG rng = ts->get_rng(); - - // generate data - cameraMatrix << 300.f, 0.f, imgSize.width/2.f, - 0.f, 300.f, imgSize.height/2.f, - 0.f, 0.f, 1.f; - distCoeffs << 0.1, 0.01, 0.001, 0.001; - - rvec(0,0) = rng.uniform( rMinVal, rMaxVal ); - rvec(0,1) = rng.uniform( rMinVal, rMaxVal ); - rvec(0,2) = rng.uniform( rMinVal, rMaxVal ); - rmat = cv::Mat_::zeros(3, 3); - Rodrigues( rvec, rmat ); - - tvec(0,0) = rng.uniform( tMinVal, tMaxVal ); - tvec(0,1) = rng.uniform( tMinVal, tMaxVal ); - tvec(0,2) = rng.uniform( tMinVal, tMaxVal ); - - for( int y = 0; y < objPoints.rows; y++ ) - { - Mat point(1, 3, CV_32FC1, objPoints.ptr(y) ); - float z = rng.uniform( zMinVal, zMaxVal ); - point.at(0,2) = z; - point.at(0,0) = (rng.uniform(2.f,(float)(imgSize.width-2)) - cameraMatrix(0,2)) / cameraMatrix(0,0) * z; - point.at(0,1) = (rng.uniform(2.f,(float)(imgSize.height-2)) - cameraMatrix(1,2)) / cameraMatrix(1,1) * z; - point = (point - tvec) * rmat; - } - - vector imgPoints; - vector > leftImgPoints; - vector > rightImgPoints; - Mat dpdrot, dpdt, dpdf, dpdc, dpddist, - valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist; - - project( objPoints, rvec, tvec, cameraMatrix, distCoeffs, - imgPoints, dpdrot, dpdt, dpdf, dpdc, dpddist, 0 ); - - // calculate and check image points - CV_Assert( (int)imgPoints.size() == pointCount ); - vector::const_iterator it = imgPoints.begin(); - for( int i = 0; i < pointCount; i++, ++it ) - { - Point3d p( objPoints(i,0), objPoints(i,1), objPoints(i,2) ); - double z = p.x*rmat(2,0) + p.y*rmat(2,1) + p.z*rmat(2,2) + tvec(0,2), - x = (p.x*rmat(0,0) + p.y*rmat(0,1) + p.z*rmat(0,2) + tvec(0,0)) / z, - y = (p.x*rmat(1,0) + p.y*rmat(1,1) + p.z*rmat(1,2) + tvec(0,1)) / z, - r2 = x*x + y*y, - r4 = r2*r2; - Point2f validImgPoint; - double a1 = 2*x*y, - a2 = r2 + 2*x*x, - a3 = r2 + 2*y*y, - cdist = 1+distCoeffs(0,0)*r2+distCoeffs(0,1)*r4; - validImgPoint.x = static_cast((double)cameraMatrix(0,0)*(x*cdist + (double)distCoeffs(0,2)*a1 + (double)distCoeffs(0,3)*a2) - + (double)cameraMatrix(0,2)); - validImgPoint.y = static_cast((double)cameraMatrix(1,1)*(y*cdist + (double)distCoeffs(0,2)*a3 + distCoeffs(0,3)*a1) - + (double)cameraMatrix(1,2)); - - if( fabs(it->x - validImgPoint.x) > imgPointErr || - fabs(it->y - validImgPoint.y) > imgPointErr ) - { - ts->printf( cvtest::TS::LOG, "bad image point\n" ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - goto _exit_; - } - } - - // check derivatives - // 1. rotation - leftImgPoints.resize(3); - rightImgPoints.resize(3); - for( int i = 0; i < 3; i++ ) - { - rvec.copyTo( leftRvec ); leftRvec(0,i) -= dEps; - project( objPoints, leftRvec, tvec, cameraMatrix, distCoeffs, - leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - rvec.copyTo( rightRvec ); rightRvec(0,i) += dEps; - project( objPoints, rightRvec, tvec, cameraMatrix, distCoeffs, - rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - } - calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdrot ); - err = cvtest::norm( dpdrot, valDpdrot, NORM_INF ); - if( err > 3 ) - { - ts->printf( cvtest::TS::LOG, "bad dpdrot: too big difference = %g\n", err ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - } - - // 2. translation - for( int i = 0; i < 3; i++ ) - { - tvec.copyTo( leftTvec ); leftTvec(0,i) -= dEps; - project( objPoints, rvec, leftTvec, cameraMatrix, distCoeffs, - leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - tvec.copyTo( rightTvec ); rightTvec(0,i) += dEps; - project( objPoints, rvec, rightTvec, cameraMatrix, distCoeffs, - rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - } - calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdt ); - if( cvtest::norm( dpdt, valDpdt, NORM_INF ) > 0.2 ) - { - ts->printf( cvtest::TS::LOG, "bad dpdtvec\n" ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - } - - // 3. camera matrix - // 3.1. focus - leftImgPoints.resize(2); - rightImgPoints.resize(2); - cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,0) -= dEps; - project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, - leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,1) -= dEps; - project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, - leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,0) += dEps; - project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, - rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,1) += dEps; - project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, - rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdf ); - if ( cvtest::norm( dpdf, valDpdf, NORM_L2 ) > 0.2 ) - { - ts->printf( cvtest::TS::LOG, "bad dpdf\n" ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - } - // 3.2. principal point - leftImgPoints.resize(2); - rightImgPoints.resize(2); - cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,2) -= dEps; - project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, - leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,2) -= dEps; - project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs, - leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,2) += dEps; - project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, - rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,2) += dEps; - project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs, - rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdc ); - if ( cvtest::norm( dpdc, valDpdc, NORM_L2 ) > 0.2 ) - { - ts->printf( cvtest::TS::LOG, "bad dpdc\n" ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - } - - // 4. distortion - leftImgPoints.resize(distCoeffs.cols); - rightImgPoints.resize(distCoeffs.cols); - for( int i = 0; i < distCoeffs.cols; i++ ) - { - distCoeffs.copyTo( leftDistCoeffs ); leftDistCoeffs(0,i) -= dEps; - project( objPoints, rvec, tvec, cameraMatrix, leftDistCoeffs, - leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - distCoeffs.copyTo( rightDistCoeffs ); rightDistCoeffs(0,i) += dEps; - project( objPoints, rvec, tvec, cameraMatrix, rightDistCoeffs, - rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 ); - } - calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpddist ); - if( cvtest::norm( dpddist, valDpddist, NORM_L2 ) > 0.3 ) - { - ts->printf( cvtest::TS::LOG, "bad dpddist\n" ); - code = cvtest::TS::FAIL_BAD_ACCURACY; - } - -_exit_: - RNG& _rng = ts->get_rng(); - _rng = rng; - ts->set_failed_test_info( code ); -} - -//----------------------------------------- CV_ProjectPointsTest_CPP -------------------------------- -class CV_ProjectPointsTest_CPP : public CV_ProjectPointsTest -{ -public: - CV_ProjectPointsTest_CPP() {} -protected: - virtual void project( const Mat& objectPoints, - const Mat& rvec, const Mat& tvec, - const Mat& cameraMatrix, - const Mat& distCoeffs, - vector& imagePoints, - Mat& dpdrot, Mat& dpdt, Mat& dpdf, - Mat& dpdc, Mat& dpddist, - double aspectRatio=0 ); -}; - -void CV_ProjectPointsTest_CPP::project( const Mat& objectPoints, const Mat& rvec, const Mat& tvec, - const Mat& cameraMatrix, const Mat& distCoeffs, vector& imagePoints, - Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio) -{ - Mat J; - projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, J, aspectRatio); - J.colRange(0, 3).copyTo(dpdrot); - J.colRange(3, 6).copyTo(dpdt); - J.colRange(6, 8).copyTo(dpdf); - J.colRange(8, 10).copyTo(dpdc); - J.colRange(10, J.cols).copyTo(dpddist); -} - ///////////////////////////////// Stereo Calibration ///////////////////////////////////// class CV_StereoCalibrationTest : public cvtest::BaseTest @@ -2238,188 +1647,7 @@ double CV_CameraRegistrationTest_CPP::calibrateStereoCamera( const vector(3, 2) << -L, L, - L, L, - 0, 0); - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.cols, static_cast(imagePoints.size())); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } - { - //Nx2 1-channel - Mat objectPoints = (Mat_(2, 3) << -L, L, 0, - L, L, 0); - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.rows, static_cast(imagePoints.size())); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } - { - //1xN 3-channel - Mat objectPoints(1, 2, CV_32FC3); - objectPoints.at(0,0) = Vec3f(-L, L, 0); - objectPoints.at(0,1) = Vec3f(L, L, 0); - - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.cols, static_cast(imagePoints.size())); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } - { - //Nx1 3-channel - Mat objectPoints(2, 1, CV_32FC3); - objectPoints.at(0,0) = Vec3f(-L, L, 0); - objectPoints.at(1,0) = Vec3f(L, L, 0); - - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.rows, static_cast(imagePoints.size())); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } - { - //vector - vector objectPoints; - objectPoints.push_back(Point3f(-L, L, 0)); - objectPoints.push_back(Point3f(L, L, 0)); - - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.size(), imagePoints.size()); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } - { - //vector - vector objectPoints; - objectPoints.push_back(Point3d(-L, L, 0)); - objectPoints.push_back(Point3d(L, L, 0)); - - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.size(), imagePoints.size()); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } -} - -TEST(Calib3d_ProjectPoints_CPP, outputShape) -{ - Matx31d rvec = Matx31d::zeros(); - Matx31d tvec(0, 0, 1); - Matx33d cameraMatrix = Matx33d::eye(); - const float L = 0.1f; - { - vector objectPoints; - objectPoints.push_back(Point3f(-L, L, 0)); - objectPoints.push_back(Point3f( L, L, 0)); - objectPoints.push_back(Point3f( L, -L, 0)); - - //Mat --> will be Nx1 2-channel - Mat imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(static_cast(objectPoints.size()), imagePoints.rows); - EXPECT_NEAR(imagePoints.at(0,0)(0), -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,0)(1), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(1,0)(0), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(1,0)(1), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(2,0)(0), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(2,0)(1), -L, std::numeric_limits::epsilon()); - } - { - vector objectPoints; - objectPoints.push_back(Point3f(-L, L, 0)); - objectPoints.push_back(Point3f( L, L, 0)); - objectPoints.push_back(Point3f( L, -L, 0)); - - //Nx1 2-channel - Mat imagePoints(3,1,CV_32FC2); - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(static_cast(objectPoints.size()), imagePoints.rows); - EXPECT_NEAR(imagePoints.at(0,0)(0), -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,0)(1), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(1,0)(0), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(1,0)(1), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(2,0)(0), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(2,0)(1), -L, std::numeric_limits::epsilon()); - } - { - vector objectPoints; - objectPoints.push_back(Point3f(-L, L, 0)); - objectPoints.push_back(Point3f( L, L, 0)); - objectPoints.push_back(Point3f( L, -L, 0)); - - //1xN 2-channel - Mat imagePoints(1,3,CV_32FC2); - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(static_cast(objectPoints.size()), imagePoints.cols); - EXPECT_NEAR(imagePoints.at(0,0)(0), -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,0)(1), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,1)(0), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,1)(1), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,2)(0), L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints.at(0,2)(1), -L, std::numeric_limits::epsilon()); - } - { - vector objectPoints; - objectPoints.push_back(Point3f(-L, L, 0)); - objectPoints.push_back(Point3f(L, L, 0)); - - //vector - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.size(), imagePoints.size()); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } - { - vector objectPoints; - objectPoints.push_back(Point3d(-L, L, 0)); - objectPoints.push_back(Point3d(L, L, 0)); - - //vector - vector imagePoints; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints); - EXPECT_EQ(objectPoints.size(), imagePoints.size()); - EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits::epsilon()); - EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits::epsilon()); - } -} - TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); } @@ -2669,212 +1897,4 @@ TEST(Calib_StereoCalibrate, regression_22421) EXPECT_LE(terr, 0.0000001); } -TEST(Calib3d_Triangulate, accuracy) -{ - // the testcase from http://code.opencv.org/issues/4334 - { - double P1data[] = { 250, 0, 200, 0, 0, 250, 150, 0, 0, 0, 1, 0 }; - double P2data[] = { 250, 0, 200, -250, 0, 250, 150, 0, 0, 0, 1, 0 }; - Mat P1(3, 4, CV_64F, P1data), P2(3, 4, CV_64F, P2data); - - float x1data[] = { 200.f, 0.f }; - float x2data[] = { 170.f, 1.f }; - float Xdata[] = { 0.f, -5.f, 25/3.f }; - Mat x1(2, 1, CV_32F, x1data); - Mat x2(2, 1, CV_32F, x2data); - Mat res0(1, 3, CV_32F, Xdata); - Mat res_, res; - - triangulatePoints(P1, P2, x1, x2, res_); - cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace) - convertPointsFromHomogeneous(res_, res); - res = res.reshape(1, 1); - - cout << "[1]:" << endl; - cout << "\tres0: " << res0 << endl; - cout << "\tres: " << res << endl; - - ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 1e-1); - } - - // another testcase http://code.opencv.org/issues/3461 - { - Matx33d K1(6137.147949, 0.000000, 644.974609, - 0.000000, 6137.147949, 573.442749, - 0.000000, 0.000000, 1.000000); - Matx33d K2(6137.147949, 0.000000, 644.674438, - 0.000000, 6137.147949, 573.079834, - 0.000000, 0.000000, 1.000000); - - Matx34d RT1(1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0); - - Matx34d RT2(0.998297, 0.0064108, -0.0579766, 143.614334, - -0.0065818, 0.999975, -0.00275888, -5.160085, - 0.0579574, 0.00313577, 0.998314, 96.066109); - - Matx34d P1 = K1*RT1; - Matx34d P2 = K2*RT2; - - float x1data[] = { 438.f, 19.f }; - float x2data[] = { 452.363600f, 16.452225f }; - float Xdata[] = { -81.049530f, -215.702804f, 2401.645449f }; - Mat x1(2, 1, CV_32F, x1data); - Mat x2(2, 1, CV_32F, x2data); - Mat res0(1, 3, CV_32F, Xdata); - Mat res_, res; - - triangulatePoints(P1, P2, x1, x2, res_); - cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace) - convertPointsFromHomogeneous(res_, res); - res = res.reshape(1, 1); - - cout << "[2]:" << endl; - cout << "\tres0: " << res0 << endl; - cout << "\tres: " << res << endl; - - ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 2); - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////// - -TEST(CV_RecoverPoseTest, regression_15341) -{ - // initialize test data - const int invalid_point_count = 2; - const float _points1_[] = { - 1537.7f, 166.8f, - 1599.1f, 179.6f, - 1288.0f, 207.5f, - 1507.1f, 193.2f, - 1742.7f, 210.0f, - 1041.6f, 271.7f, - 1591.8f, 247.2f, - 1524.0f, 261.3f, - 1330.3f, 285.0f, - 1403.1f, 284.0f, - 1506.6f, 342.9f, - 1502.8f, 347.3f, - 1344.9f, 364.9f, - 0.0f, 0.0f // last point is initial invalid - }; - - const float _points2_[] = { - 1533.4f, 532.9f, - 1596.6f, 552.4f, - 1277.0f, 556.4f, - 1502.1f, 557.6f, - 1744.4f, 601.3f, - 1023.0f, 612.6f, - 1589.2f, 621.6f, - 1519.4f, 629.0f, - 1320.3f, 637.3f, - 1395.2f, 642.2f, - 1501.5f, 710.3f, - 1497.6f, 714.2f, - 1335.1f, 719.61f, - 1000.0f, 1000.0f // last point is initial invalid - }; - - vector _points1; Mat(14, 1, CV_32FC2, (void*)_points1_).copyTo(_points1); - vector _points2; Mat(14, 1, CV_32FC2, (void*)_points2_).copyTo(_points2); - - const int point_count = (int) _points1.size(); - CV_Assert(point_count == (int) _points2.size()); - - // camera matrix with both focal lengths = 1, and principal point = (0, 0) - const Mat cameraMatrix = Mat::eye(3, 3, CV_64F); - - // camera matrix with focal lengths 0.5 and 0.6 respectively and principal point = (100, 200) - double cameraMatrix2Data[] = { 0.5, 0, 100, - 0, 0.6, 200, - 0, 0, 1 }; - const Mat cameraMatrix2( 3, 3, CV_64F, cameraMatrix2Data ); - - // zero and nonzero distortion coefficients - double nonZeroDistCoeffsData[] = { 0.01, 0.0001, 0, 0, 1e-04, 0.2, 0.02, 0.0002 }; // k1, k2, p1, p2, k3, k4, k5, k6 - vector distCoeffsList = {Mat::zeros(1, 5, CV_64F), Mat{1, 8, CV_64F, nonZeroDistCoeffsData}}; - const auto &zeroDistCoeffs = distCoeffsList[0]; - - int Inliers = 0; - - const int ntests = 3; - for (int testcase = 1; testcase <= ntests; ++testcase) - { - if (testcase == 1) // testcase with vector input data - { - // init temporary test data - vector mask(point_count); - vector points1(_points1); - vector points2(_points2); - - // Estimation of fundamental matrix using the RANSAC algorithm - Mat E, E2, R, t; - - // Check pose when camera matrices are different. - for (const auto &distCoeffs: distCoeffsList) - { - E = findEssentialMat(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, RANSAC, 0.999, 1.0, mask); - recoverPose(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, E2, R, t, RANSAC, 0.999, 1.0, mask); - EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << - "Two big difference between the same essential matrices computed using different functions with different cameras, testcase " << testcase; - EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function failed with different cameras, testcase " << testcase; - } - - // Check pose when camera matrices are the same. - E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, 1000/*maxIters*/, mask); - E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask); - EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << - "Two big difference between the same essential matrices computed using different functions with same cameras, testcase " << testcase; - EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function findEssentialMat failed with same cameras, testcase " << testcase; - points2[12] = Point2f(0.0f, 0.0f); // provoke another outlier detection for recover Pose - Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask); - EXPECT_EQ(0, (int)mask[12]) << "Detecting outliers in function failed with same cameras, testcase " << testcase; - } - else // testcase with mat input data - { - Mat points1(_points1, true); - Mat points2(_points2, true); - Mat mask; - - if (testcase == 2) - { - // init temporary testdata - mask = Mat::zeros(point_count, 1, CV_8UC1); - } - else // testcase == 3 - with transposed mask - { - mask = Mat::zeros(1, point_count, CV_8UC1); - } - - // Estimation of fundamental matrix using the RANSAC algorithm - Mat E, E2, R, t; - - // Check pose when camera matrices are different. - for (const auto &distCoeffs: distCoeffsList) - { - E = findEssentialMat(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, RANSAC, 0.999, 1.0, mask); - recoverPose(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, E2, R, t, RANSAC, 0.999, 1.0, mask); - EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << - "Two big difference between the same essential matrices computed using different functions with different cameras, testcase " << testcase; - EXPECT_EQ(0, (int)mask.at(13)) << "Detecting outliers in function failed with different cameras, testcase " << testcase; - } - - // Check pose when camera matrices are the same. - E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, 1000/*maxIters*/, mask); - E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask); - EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) << - "Two big difference between the same essential matrices computed using different functions with same cameras, testcase " << testcase; - EXPECT_EQ(0, (int)mask.at(13)) << "Detecting outliers in function findEssentialMat failed with same cameras, testcase " << testcase; - points2.at(12) = Point2f(0.0f, 0.0f); // provoke an outlier detection - Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask); - EXPECT_EQ(0, (int)mask.at(12)) << "Detecting outliers in function failed with same cameras, testcase " << testcase; - } - EXPECT_EQ(Inliers, point_count - invalid_point_count) << - "Number of inliers differs from expected number of inliers, testcase " << testcase; - } -} - }} // namespace