diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 217d75e81f..6fd44a83d6 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -276,12 +276,11 @@ namespace cv class PnPRansacCallback : public PointSetRegistrator::Callback { public: - bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const - { - // which kind of checking?? - return true; - } + PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE, + bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() ) + : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), + rvec(_rvec), tvec(_tvec) {} /* Pre: True */ /* Post: compute _model with given points an eturn number of found models */ @@ -289,18 +288,20 @@ public: { Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); - Mat cameraMatrix = _model.getMat(0); - Mat distCoeffs = _model.getMat(1); - Mat rvec = _model.getMat(2); - Mat tvec = _model.getMat(3); - int flags = _model.getMat(4).at(0); + bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs, + rvec, tvec, useExtrinsicGuess, flags ); + - bool useExtrinsicGuess = true; + if(correspondence) + { + Mat _local_model; + _local_model.push_back(rvec); + _local_model.push_back(tvec); - bool correspondence = cv::solvePnP( opoints, ipoints, - cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags ); + _local_model.copyTo(_model); + } - return 1; + return correspondence; } /* Pre: True */ @@ -308,28 +309,33 @@ public: void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const { - Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); - - Mat cameraMatrix = _model.getMat(0); - Mat distCoeffs = _model.getMat(1); - Mat rvec = _model.getMat(2); - Mat tvec = _model.getMat(3); + Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat(); int i, count = opoints.cols; + Mat _rvec = model.rowRange(0,3); + Mat _tvec = model.rowRange(3, 6); - Mat projpoints(count, 2, CV_64FC1); - cv::projectPoints(opoints, rvec, tvec, cameraMatrix, distCoeffs, projpoints); + Mat projpoints(count, 2, CV_32FC1); + cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); const Point2f* ipoints_ptr = ipoints.ptr(); const Point2f* projpoints_ptr = projpoints.ptr(); - _err.create(count, 1, CV_64FC1); + _err.create(count, 1, CV_32FC1); float* err = _err.getMat().ptr(); for ( i = 0; i < count; ++i) err[i] = cv::norm( ipoints_ptr[i] - projpoints_ptr[i] ); } + + + Mat cameraMatrix; + Mat distCoeffs; + int flags; + bool useExtrinsicGuess; + Mat rvec; + Mat tvec; }; void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, @@ -338,7 +344,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, int iterationsCount, float reprojectionError, int minInliersCount, OutputArray _inliers, int flags) { - // NO CHANGES + Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); @@ -349,11 +355,6 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, CV_Assert(ipoints.depth() == CV_32F); CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2); - _rvec.create(3, 1, CV_64FC1); - _tvec.create(3, 1, CV_64FC1); - Mat rvec = _rvec.getMat(); - Mat tvec = _tvec.getMat(); - Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1); if (minInliersCount <= 0) @@ -365,47 +366,59 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, params.useExtrinsicGuess = useExtrinsicGuess; params.camera.init(cameraMatrix, distCoeffs); params.flags = flags; - // END NO CHANGES - cv::Mat flag(1, 1, CV_8UC1); - flag.at(0) = params.flags; + Ptr cb; // pointer to callback - // Embed input model to a Mat - std::vector _model; + _rvec.create(3, 1, CV_64FC1); + _tvec.create(3, 1, CV_64FC1); - _model.push_back(_cameraMatrix.getMat()); // 3x3 - _model.push_back(_distCoeffs.getMat()); // 4x1 - _model.push_back(_rvec.getMat()); // 3x1 - _model.push_back(_tvec.getMat()); // 3x1 - _model.push_back(flag); // 1x1 + Mat rvec, tvec; - cv::Mat local_inliers; + if (useExtrinsicGuess) // use given rvec & tvec + { + rvec = _rvec.getMat(); + tvec = _tvec.getMat(); + + cb = makePtr( params.camera.intrinsics, params.camera.distortion, + params.flags, params.useExtrinsicGuess, rvec, tvec); + } + else + { + rvec = Mat(3, 1, CV_64FC1); + tvec = Mat(3, 1, CV_64FC1); - Ptr cb = makePtr(); // pointer to callback + cb = makePtr( params.camera.intrinsics, params.camera.distortion, + params.flags, params.useExtrinsicGuess, rvec, tvec); + } - int model_points = 7; // number of model points. From fundamentalMatrix, must change + int model_points = 7; // number of model points double param1 = params.reprojectionError ; // reprojection error double param2 = 0.99; // confidence int param3 = params.iterationsCount; // number maximum iterations + cv::Mat _local_model(3, 2, CV_64FC1); + cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); + // call Ransac + int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(_opoints, _ipoints, _local_model, _mask_local_inliers); - // NO COMPILE, IT DOESN'T LIKE vector in run - int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(objectPoints, imagePoints, _model, local_inliers); + _rvec.assign(_local_model.rowRange(0,3)); // output rotation vector + _tvec.assign(_local_model.rowRange(3,6)); // output translation vector - _rvec.assign(_model.at(2)); // output rotation vector - _tvec.assign(_model.at(3)); // output translation vector + //std::cout << _mask_local_inliers.type() << std::endl; - // output inliers vector + Mat _local_inliers; int count = 0; - for (int i = 0; i < local_inliers.rows; ++i) + for (int i = 0; i < _mask_local_inliers.rows; ++i) { - if(local_inliers.at(i) == 1) + if((int)_mask_local_inliers.at(i) == 1) // inliers mask { - cv::Mat & inliers = _inliers.getMat().at(count) = i; + _local_inliers.push_back(count); // output inliers vector count++; } } + _local_inliers.copyTo(_inliers); + // OLD IMPLEMENTATION