diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 70f2aafede..98deab6396 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -820,9 +820,9 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, { dpdk_p[5] = fx*x*cdist*(-icdist2)*icdist2*r2; dpdk_p[dpdk_step+5] = fy*y*cdist*(-icdist2)*icdist2*r2; - dpdk_p[6] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r4; + dpdk_p[6] = fx*x*cdist*(-icdist2)*icdist2*r4; dpdk_p[dpdk_step+6] = fy*y*cdist*(-icdist2)*icdist2*r4; - dpdk_p[7] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r6; + dpdk_p[7] = fx*x*cdist*(-icdist2)*icdist2*r6; dpdk_p[dpdk_step+7] = fy*y*cdist*(-icdist2)*icdist2*r6; if( _dpdk->cols > 8 ) { diff --git a/modules/calib3d/src/ptsetreg.cpp b/modules/calib3d/src/ptsetreg.cpp index 2a81a33ff5..75a73a5cb7 100644 --- a/modules/calib3d/src/ptsetreg.cpp +++ b/modules/calib3d/src/ptsetreg.cpp @@ -80,7 +80,7 @@ public: int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000) : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters) { - checkPartialSubsets = true; + checkPartialSubsets = false; } int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const @@ -145,6 +145,9 @@ public: ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k]; if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 )) { + // we may have selected some bad points; + // so, let's remove some of them randomly + i = rng.uniform(0, i+1); iters++; continue; } @@ -206,7 +209,7 @@ public: int i, goodCount, nmodels; if( count > modelPoints ) { - bool found = getSubset( m1, m2, ms1, ms2, rng ); + bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 ); if( !found ) { if( iter == 0 ) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index dd5c5eb8cb..8b20cf7e46 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -59,9 +59,33 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints, Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); - _rvec.create(3, 1, CV_64F); - _tvec.create(3, 1, CV_64F); - Mat cameraMatrix = Mat_(_cameraMatrix.getMat()), distCoeffs = Mat_(_distCoeffs.getMat()); + + Mat rvec, tvec; + if( flags != SOLVEPNP_ITERATIVE ) + useExtrinsicGuess = false; + + if( useExtrinsicGuess ) + { + int rtype = _rvec.type(), ttype = _tvec.type(); + Size rsize = _rvec.size(), tsize = _tvec.size(); + CV_Assert( (rtype == CV_32F || rtype == CV_64F) && + (ttype == CV_32F || ttype == CV_64F) ); + CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) && + (tsize == Size(1, 3) || tsize == Size(3, 1)) ); + } + else + { + _rvec.create(3, 1, CV_64F); + _tvec.create(3, 1, CV_64F); + } + rvec = _rvec.getMat(); + tvec = _tvec.getMat(); + + Mat cameraMatrix0 = _cameraMatrix.getMat(); + Mat distCoeffs0 = _distCoeffs.getMat(); + Mat cameraMatrix = Mat_(cameraMatrix0); + Mat distCoeffs = Mat_(distCoeffs0); + bool result = false; if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) { @@ -69,10 +93,10 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints, undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); epnp PnP(cameraMatrix, opoints, undistortedPoints); - Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R; PnP.compute_pose(R, tvec); Rodrigues(R, rvec); - return true; + result = true; } else if (flags == SOLVEPNP_P3P) { @@ -81,21 +105,20 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints, undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); p3p P3Psolver(cameraMatrix); - Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); - bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); + Mat R; + result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); if (result) Rodrigues(R, rvec); - return result; } else if (flags == SOLVEPNP_ITERATIVE) { CvMat c_objectPoints = opoints, c_imagePoints = ipoints; CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; - CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat(); + CvMat c_rvec = rvec, c_tvec = tvec; cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, &c_rvec, &c_tvec, useExtrinsicGuess ); - return true; + result = true; } /*else if (flags == SOLVEPNP_DLS) { @@ -115,17 +138,13 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints, upnp PnP(cameraMatrix, opoints, ipoints); Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); - double f = PnP.compute_pose(R, tvec); + PnP.compute_pose(R, tvec); Rodrigues(R, rvec); - if(cameraMatrix.type() == CV_32F) - cameraMatrix.at(0,0) = cameraMatrix.at(1,1) = (float)f; - else - cameraMatrix.at(0,0) = cameraMatrix.at(1,1) = f; return true; }*/ else CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); - return false; + return result; } class PnPRansacCallback : public PointSetRegistrator::Callback @@ -196,7 +215,16 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, OutputArray _inliers, int flags) { - Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); + Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat(); + Mat opoints, ipoints; + if( opoints0.depth() == CV_64F || !opoints0.isContinuous() ) + opoints0.convertTo(opoints, CV_32F); + else + opoints = opoints0; + if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() ) + ipoints0.convertTo(ipoints, CV_32F); + else + ipoints = ipoints0; int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); diff --git a/modules/calib3d/test/test_cameracalibration.cpp b/modules/calib3d/test/test_cameracalibration.cpp index bbd363cdf8..2053e0677b 100644 --- a/modules/calib3d/test/test_cameracalibration.cpp +++ b/modules/calib3d/test/test_cameracalibration.cpp @@ -1875,3 +1875,73 @@ TEST(Calib3d_ProjectPoints_C, accuracy) { CV_ProjectPointsTest_C test; test.saf TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); } TEST(Calib3d_StereoCalibrate_C, regression) { CV_StereoCalibrationTest_C test; test.safe_run(); } TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); } + + +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_); + transpose(res_, res_); + convertPointsFromHomogeneous(res_, res); + res = res.reshape(1, 1); + + cout << "[1]:" << endl; + cout << "\tres0: " << res0 << endl; + cout << "\tres: " << res << endl; + + ASSERT_LE(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_); + transpose(res_, res_); + convertPointsFromHomogeneous(res_, res); + res = res.reshape(1, 1); + + cout << "[2]:" << endl; + cout << "\tres0: " << res0 << endl; + cout << "\tres: " << res << endl; + + ASSERT_LE(norm(res, res0, NORM_INF), 2); + } +} diff --git a/modules/calib3d/test/test_fundam.cpp b/modules/calib3d/test/test_fundam.cpp index 5f8d30de40..5b77dc308a 100644 --- a/modules/calib3d/test/test_fundam.cpp +++ b/modules/calib3d/test/test_fundam.cpp @@ -1709,4 +1709,21 @@ TEST(Calib3d_ConvertHomogeneoous, accuracy) { CV_ConvertHomogeneousTest test; te TEST(Calib3d_ComputeEpilines, accuracy) { CV_ComputeEpilinesTest test; test.safe_run(); } TEST(Calib3d_FindEssentialMat, accuracy) { CV_EssentialMatTest test; test.safe_run(); } +TEST(Calib3d_FindFundamentalMat, correctMatches) +{ + double fdata[] = {0, 0, 0, 0, 0, -1, 0, 1, 0}; + double p1data[] = {200, 0, 1}; + double p2data[] = {170, 0, 1}; + + Mat F(3, 3, CV_64F, fdata); + Mat p1(1, 1, CV_64FC2, p1data); + Mat p2(1, 1, CV_64FC2, p2data); + Mat np1, np2; + + correctMatches(F, p1, p2, np1, np2); + + cout << np1 << endl; + cout << np2 << endl; +} + /* End of file. */ diff --git a/modules/calib3d/test/test_homography.cpp b/modules/calib3d/test/test_homography.cpp index a31b75d2b7..1076a7c42a 100644 --- a/modules/calib3d/test/test_homography.cpp +++ b/modules/calib3d/test/test_homography.cpp @@ -568,3 +568,121 @@ void CV_HomographyTest::run(int) } TEST(Calib3d_Homography, accuracy) { CV_HomographyTest test; test.safe_run(); } + +TEST(Calib3d_Homography, EKcase) +{ + float pt1data[] = + { + 2.80073029e+002f, 2.39591217e+002f, 2.21912201e+002f, 2.59783997e+002f, + 2.16053192e+002f, 2.78826569e+002f, 2.22782532e+002f, 2.82330383e+002f, + 2.09924820e+002f, 2.89122559e+002f, 2.11077698e+002f, 2.89384674e+002f, + 2.25287689e+002f, 2.88795532e+002f, 2.11180801e+002f, 2.89653503e+002f, + 2.24126404e+002f, 2.90466064e+002f, 2.10914429e+002f, 2.90886963e+002f, + 2.23439362e+002f, 2.91657715e+002f, 2.24809387e+002f, 2.91891602e+002f, + 2.09809082e+002f, 2.92891113e+002f, 2.08771164e+002f, 2.93093231e+002f, + 2.23160095e+002f, 2.93259460e+002f, 2.07874023e+002f, 2.93989990e+002f, + 2.08963638e+002f, 2.94209839e+002f, 2.23963165e+002f, 2.94479645e+002f, + 2.23241791e+002f, 2.94887817e+002f, 2.09438782e+002f, 2.95233337e+002f, + 2.08901886e+002f, 2.95762878e+002f, 2.21867981e+002f, 2.95747711e+002f, + 2.24195511e+002f, 2.98270905e+002f, 2.09331345e+002f, 3.05958191e+002f, + 2.24727875e+002f, 3.07186035e+002f, 2.26718842e+002f, 3.08095795e+002f, + 2.25363953e+002f, 3.08200226e+002f, 2.19897797e+002f, 3.13845093e+002f, + 2.25013474e+002f, 3.15558777e+002f + }; + + float pt2data[] = + { + 1.84072723e+002f, 1.43591202e+002f, 1.25912483e+002f, 1.63783859e+002f, + 2.06439407e+002f, 2.20573929e+002f, 1.43801437e+002f, 1.80703903e+002f, + 9.77904129e+000f, 2.49660202e+002f, 1.38458405e+001f, 2.14502701e+002f, + 1.50636337e+002f, 2.15597183e+002f, 6.43103180e+001f, 2.51667648e+002f, + 1.54952499e+002f, 2.20780014e+002f, 1.26638412e+002f, 2.43040924e+002f, + 3.67568909e+002f, 1.83624954e+001f, 1.60657944e+002f, 2.21794052e+002f, + -1.29507828e+000f, 3.32472443e+002f, 8.51442242e+000f, 4.15561554e+002f, + 1.27161377e+002f, 1.97260361e+002f, 5.40714645e+000f, 4.90978302e+002f, + 2.25571690e+001f, 3.96912415e+002f, 2.95664978e+002f, 7.36064959e+000f, + 1.27241104e+002f, 1.98887573e+002f, -1.25569367e+000f, 3.87713226e+002f, + 1.04194012e+001f, 4.31495758e+002f, 1.25868874e+002f, 1.99751617e+002f, + 1.28195480e+002f, 2.02270355e+002f, 2.23436356e+002f, 1.80489182e+002f, + 1.28727692e+002f, 2.11185410e+002f, 2.03336639e+002f, 2.52182083e+002f, + 1.29366486e+002f, 2.12201904e+002f, 1.23897598e+002f, 2.17847351e+002f, + 1.29015259e+002f, 2.19560623e+002f + }; + + int npoints = (int)(sizeof(pt1data)/sizeof(pt1data[0])/2); + + Mat p1(1, npoints, CV_32FC2, pt1data); + Mat p2(1, npoints, CV_32FC2, pt2data); + Mat mask; + + Mat h = findHomography(p1, p2, RANSAC, 0.01, mask); + ASSERT_TRUE(!h.empty()); + + transpose(mask, mask); + Mat p3, mask2; + int ninliers = countNonZero(mask); + Mat nmask[] = { mask, mask }; + merge(nmask, 2, mask2); + perspectiveTransform(p1, p3, h); + mask2 = mask2.reshape(1); + p2 = p2.reshape(1); + p3 = p3.reshape(1); + double err = norm(p2, p3, NORM_INF, mask2); + + printf("ninliers: %d, inliers err: %.2g\n", ninliers, err); + ASSERT_GE(ninliers, 10); + ASSERT_LE(err, 0.01); +} + +TEST(Calib3d_Homography, fromImages) +{ + Mat img_1 = imread(cvtest::TS::ptr()->get_data_path() + "cv/optflow/image1.png", 0); + Mat img_2 = imread(cvtest::TS::ptr()->get_data_path() + "cv/optflow/image2.png", 0); + Ptr orb = ORB::create(); + vector keypoints_1, keypoints_2; + Mat descriptors_1, descriptors_2; + orb->detectAndCompute( img_1, Mat(), keypoints_1, descriptors_1, false ); + orb->detectAndCompute( img_2, Mat(), keypoints_2, descriptors_2, false ); + + //-- Step 3: Matching descriptor vectors using Brute Force matcher + BFMatcher matcher(NORM_HAMMING,false); + std::vector< DMatch > matches; + matcher.match( descriptors_1, descriptors_2, matches ); + + double max_dist = 0; double min_dist = 100; + //-- Quick calculation of max and min distances between keypoints + for( int i = 0; i < descriptors_1.rows; i++ ) + { + double dist = matches[i].distance; + if( dist < min_dist ) min_dist = dist; + if( dist > max_dist ) max_dist = dist; + } + + //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist ) + std::vector< DMatch > good_matches; + for( int i = 0; i < descriptors_1.rows; i++ ) + { + if( matches[i].distance <= 42 ) + good_matches.push_back( matches[i]); + } + + //-- Localize the model + std::vector pointframe1; + std::vector pointframe2; + for( int i = 0; i < (int)good_matches.size(); i++ ) + { + //-- Get the keypoints from the good matches + pointframe1.push_back( keypoints_1[ good_matches[i].queryIdx ].pt ); + pointframe2.push_back( keypoints_2[ good_matches[i].trainIdx ].pt ); + } + + Mat inliers; + Mat H = findHomography( pointframe1, pointframe2, RANSAC,3.0,inliers); + int ninliers = countNonZero(inliers); + printf("nfeatures1 = %d, nfeatures2=%d, good matches=%d, ninliers=%d\n", + (int)keypoints_1.size(), (int)keypoints_2.size(), + (int)good_matches.size(), ninliers); + + ASSERT_TRUE(!H.empty()); + ASSERT_GE(ninliers, 80); +} diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 76a3966bba..5e3dbc60db 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -138,7 +138,7 @@ protected: } solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec, - false, 500, 0.5, 0.99, inliers, method); + false, 500, 0.5f, 0.99, inliers, method); bool isTestSuccess = inliers.size() >= points.size()*0.95; @@ -254,10 +254,7 @@ protected: TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); } TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); } - -#ifdef HAVE_TBB - -TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency) +TEST(Calib3d_SolvePnPRansac, concurrency) { int count = 7*13; @@ -287,12 +284,11 @@ TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency) { // limit concurrency to get deterministic result - cv::theRNG().state = 20121010; - tbb::task_scheduler_init one_thread(1); + theRNG().state = 20121010; + setNumThreads(1); solvePnPRansac(object, image, camera_mat, dist_coef, rvec1, tvec1); } - if(1) { Mat rvec; Mat tvec; @@ -306,8 +302,8 @@ TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency) { // single thread again - cv::theRNG().state = 20121010; - tbb::task_scheduler_init one_thread(1); + theRNG().state = 20121010; + setNumThreads(1); solvePnPRansac(object, image, camera_mat, dist_coef, rvec2, tvec2); } @@ -316,6 +312,69 @@ TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency) EXPECT_LT(rnorm, 1e-6); EXPECT_LT(tnorm, 1e-6); +} +TEST(Calib3d_SolvePnP, double_support) +{ + Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0., + 5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.); + std::vector points3d; + std::vector points2d; + std::vector points3dF; + std::vector points2dF; + for (int i = 0; i < 10 ; i++) + { + points3d.push_back(cv::Point3d(i,0,0)); + points3dF.push_back(cv::Point3d(i,0,0)); + points2d.push_back(cv::Point2d(i,0)); + points2dF.push_back(cv::Point2d(i,0)); + } + Mat R,t, RF, tF; + vector inliers; + + solvePnPRansac(points3dF, points2dF, intrinsics, cv::Mat(), RF, tF, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P); + solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R, t, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P); + + ASSERT_LE(norm(R, Mat_(RF), NORM_INF), 1e-3); + ASSERT_LE(norm(t, Mat_(tF), NORM_INF), 1e-3); +} + +TEST(Calib3d_SolvePnP, translation) +{ + Mat cameraIntrinsic = Mat::eye(3,3, CV_32FC1); + vector crvec; + crvec.push_back(0.f); + crvec.push_back(0.f); + crvec.push_back(0.f); + vector ctvec; + ctvec.push_back(100.f); + ctvec.push_back(100.f); + ctvec.push_back(0.f); + vector p3d; + p3d.push_back(Point3f(0,0,0)); + p3d.push_back(Point3f(0,0,10)); + p3d.push_back(Point3f(0,10,10)); + p3d.push_back(Point3f(10,10,10)); + p3d.push_back(Point3f(2,5,5)); + + vector p2d; + projectPoints(p3d, crvec, ctvec, cameraIntrinsic, noArray(), p2d); + Mat rvec; + Mat tvec; + rvec =(Mat_(3,1) << 0, 0, 0); + tvec = (Mat_(3,1) << 100, 100, 0); + + solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true); + ASSERT_TRUE(checkRange(rvec)); + ASSERT_TRUE(checkRange(tvec)); + + rvec =(Mat_(3,1) << 0, 0, 0); + tvec = (Mat_(3,1) << 100, 100, 0); + solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true); + ASSERT_TRUE(checkRange(rvec)); + ASSERT_TRUE(checkRange(tvec)); + + solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, false); + ASSERT_TRUE(checkRange(rvec)); + ASSERT_TRUE(checkRange(tvec)); } -#endif diff --git a/modules/core/src/mathfuncs.cpp b/modules/core/src/mathfuncs.cpp index b8e4b1a2ed..97f36299d8 100644 --- a/modules/core/src/mathfuncs.cpp +++ b/modules/core/src/mathfuncs.cpp @@ -2047,7 +2047,7 @@ double cv::solvePoly( InputArray _coeffs0, OutputArray _roots0, int maxIters ) CV_Assert( CV_MAT_DEPTH(ctype) >= CV_32F && CV_MAT_CN(ctype) <= 2 ); CV_Assert( coeffs0.rows == 1 || coeffs0.cols == 1 ); - int n = coeffs0.cols + coeffs0.rows - 2; + int n0 = coeffs0.cols + coeffs0.rows - 2, n = n0; _roots0.create(n, 1, CV_MAKETYPE(cdepth, 2), -1, true, _OutputArray::DEPTH_MASK_FLT); Mat roots0 = _roots0.getMat(); @@ -2063,6 +2063,12 @@ double cv::solvePoly( InputArray _coeffs0, OutputArray _roots0, int maxIters ) coeffs[i] = C(rcoeffs[i], 0); } + for( ; n > 1; n-- ) + { + if( std::abs(coeffs[n].re) + std::abs(coeffs[n].im) > DBL_EPSILON ) + break; + } + C p(1, 0), r(1, 1); for( i = 0; i < n; i++ ) @@ -2100,6 +2106,9 @@ double cv::solvePoly( InputArray _coeffs0, OutputArray _roots0, int maxIters ) roots[i].im = 0; } + for( ; n < n0; n++ ) + roots[n+1] = roots[n]; + Mat(roots0.size(), CV_64FC2, roots).convertTo(roots0, roots0.type()); return maxDiff; }