void myConvexityDefects( InputArray _points, InputArray _hull, OutputArray _defects ) { Mat points = _points.getMat(); int ptnum = points.checkVector(2, CV_32S); CV_Assert( ptnum > 3 ); Mat hull = _hull.getMat(); CV_Assert( hull.checkVector(1, CV_32S) > 2 ); std::vector<CvConvexityDefect> seq; convexityDefects(_points, _hull, seq); if( seq.size() == 0 ) { _defects.release(); return; } _defects.create(seq.size(), 1, CV_32SC4); Mat defects = _defects.getMat(); auto it = seq.begin(); CvPoint* ptorg = (CvPoint*)points.data; for( unsigned i = 0; i < seq.size(); ++i, ++it ) { CvConvexityDefect& d = *it; int idx0 = (int)(d.start - ptorg); int idx1 = (int)(d.end - ptorg); int idx2 = (int)(d.depth_point - ptorg); CV_Assert( 0 <= idx0 && idx0 < ptnum ); CV_Assert( 0 <= idx1 && idx1 < ptnum ); CV_Assert( 0 <= idx2 && idx2 < ptnum ); CV_Assert( d.depth >= 0 ); int idepth = cvRound(d.depth*256); defects.at<Vec4i>(i) = Vec4i(idx0, idx1, idx2, idepth); } }
void cv::convertPointsHomogeneous( const Mat& src, vector<Point3f>& dst ) { int srccn = src.checkVector(2) >= 0 ? 2 : src.checkVector(4) >= 0 ? 4 : -1; CV_Assert( srccn > 0 && (src.depth() == CV_32F || src.depth() == CV_32S)); dst.resize(src.cols*src.rows*src.channels()/srccn); CvMat _src = src, _dst = Mat(dst); cvConvertPointsHomogeneous(&_src, &_dst); }
int solveP3P( InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) { CV_INSTRUMENT_REGION(); Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); CV_Assert( npoints == 3 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P ); Mat cameraMatrix0 = _cameraMatrix.getMat(); Mat distCoeffs0 = _distCoeffs.getMat(); Mat cameraMatrix = Mat_<double>(cameraMatrix0); Mat distCoeffs = Mat_<double>(distCoeffs0); Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); std::vector<Mat> Rs, ts; int solutions = 0; if (flags == SOLVEPNP_P3P) { p3p P3Psolver(cameraMatrix); solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints); } else if (flags == SOLVEPNP_AP3P) { ap3p P3Psolver(cameraMatrix); solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints); } if (solutions == 0) { return 0; } if (_rvecs.needed()) { _rvecs.create(solutions, 1, CV_64F); } if (_tvecs.needed()) { _tvecs.create(solutions, 1, CV_64F); } for (int i = 0; i < solutions; i++) { Mat rvec; Rodrigues(Rs[i], rvec); _tvecs.getMatRef(i) = ts[i]; _rvecs.getMatRef(i) = rvec; } return solutions; }
void cv::computeCorrespondEpilines( InputArray _points, int whichImage, InputArray _Fmat, OutputArray _lines ) { Mat points = _points.getMat(), F = _Fmat.getMat(); int npoints = points.checkVector(2); if( npoints < 0 ) npoints = points.checkVector(3); CV_Assert( npoints >= 0 && (points.depth() == CV_32F || points.depth() == CV_32S)); _lines.create(npoints, 1, CV_32FC3, -1, true); CvMat c_points = points, c_lines = _lines.getMat(), c_F = F; cvComputeCorrespondEpilines(&c_points, whichImage, &c_F, &c_lines); }
bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) { 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 = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); if (flags == CV_EPNP) { cv::Mat undistortedPoints; cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); epnp PnP(cameraMatrix, opoints, undistortedPoints); cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); PnP.compute_pose(R, tvec); cv::Rodrigues(R, rvec); return true; } else if (flags == CV_P3P) { CV_Assert( npoints == 4); cv::Mat undistortedPoints; cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); p3p P3Psolver(cameraMatrix); cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); if (result) cv::Rodrigues(R, rvec); return result; } else if (flags == CV_ITERATIVE) { CvMat c_objectPoints = opoints, c_imagePoints = ipoints; CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat(); cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, &c_rvec, &c_tvec, useExtrinsicGuess ); return true; } else CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE or CV_EPNP"); return false; }
// calculates length of a curve (e.g. contour perimeter) double cv::arcLength( InputArray _curve, bool is_closed ) { Mat curve = _curve.getMat(); int count = curve.checkVector(2); int depth = curve.depth(); CV_Assert( count >= 0 && (depth == CV_32F || depth == CV_32S)); double perimeter = 0; int i; if( count <= 1 ) return 0.; bool is_float = depth == CV_32F; int last = is_closed ? count-1 : 0; const Point* pti = curve.ptr<Point>(); const Point2f* ptf = curve.ptr<Point2f>(); Point2f prev = is_float ? ptf[last] : Point2f((float)pti[last].x,(float)pti[last].y); for( i = 0; i < count; i++ ) { Point2f p = is_float ? ptf[i] : Point2f((float)pti[i].x,(float)pti[i].y); float dx = p.x - prev.x, dy = p.y - prev.y; perimeter += std::sqrt(dx*dx + dy*dy); prev = p; } return perimeter; }
void cv::fitLine( InputArray _points, OutputArray _line, int distType, double param, double reps, double aeps ) { Mat points = _points.getMat(); float linebuf[6]={0.f}; int npoints2 = points.checkVector(2, -1, false); int npoints3 = points.checkVector(3, -1, false); CV_Assert( npoints2 >= 0 || npoints3 >= 0 ); if( points.depth() != CV_32F || !points.isContinuous() ) { Mat temp; points.convertTo(temp, CV_32F); points = temp; } if( npoints2 >= 0 ) fitLine2D( points.ptr<Point2f>(), npoints2, distType, (float)param, (float)reps, (float)aeps, linebuf); else fitLine3D( points.ptr<Point3f>(), npoints3, distType, (float)param, (float)reps, (float)aeps, linebuf); Mat(npoints2 >= 0 ? 4 : 6, 1, CV_32F, linebuf).copyTo(_line); }
// area of a whole sequence double cv::contourArea( InputArray _contour, bool oriented ) { Mat contour = _contour.getMat(); int npoints = contour.checkVector(2); int depth = contour.depth(); CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_32S)); if( npoints == 0 ) return 0.; double a00 = 0; bool is_float = depth == CV_32F; const Point* ptsi = (const Point*)contour.data; const Point2f* ptsf = (const Point2f*)contour.data; Point2f prev = is_float ? ptsf[npoints-1] : Point2f((float)ptsi[npoints-1].x, (float)ptsi[npoints-1].y); for( int i = 0; i < npoints; i++ ) { Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); a00 += (double)prev.x * p.y - (double)prev.y * p.x; prev = p; } a00 *= 0.5; if( !oriented ) a00 = fabs(a00); return a00; }
void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst ) { Mat src = _src.getMat(); int npoints = src.checkVector(2), cn = 2; if( npoints < 0 ) { npoints = src.checkVector(3); if( npoints >= 0 ) cn = 3; } CV_Assert( npoints >= 0 && (src.depth() == CV_32F || src.depth() == CV_32S)); _dst.create(npoints, 1, CV_MAKETYPE(CV_32F, cn+1)); CvMat c_src = src, c_dst = _dst.getMat(); cvConvertPointsHomogeneous(&c_src, &c_dst); }
int cv::estimateAffine3D(InputArray _from, InputArray _to, OutputArray _out, OutputArray _inliers, double param1, double param2) { Mat from = _from.getMat(), to = _to.getMat(); int count = from.checkVector(3, CV_32F); CV_Assert( count >= 0 && to.checkVector(3, CV_32F) == count ); _out.create(3, 4, CV_64F); Mat out = _out.getMat(); _inliers.create(count, 1, CV_8U, -1, true); Mat inliers = _inliers.getMat(); inliers = Scalar::all(1); Mat dFrom, dTo; from.convertTo(dFrom, CV_64F); to.convertTo(dTo, CV_64F); CvMat F3x4 = out; CvMat mask = inliers; CvMat m1 = dFrom; CvMat m2 = dTo; const double epsilon = numeric_limits<double>::epsilon(); param1 = param1 <= 0 ? 3 : param1; param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2; return Affine3DEstimator().runRANSAC(&m1, &m2, &F3x4, &mask, param1, param2 ); }
static void collectCalibrationData( InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2, Mat& npoints ) { int nimages = (int)objectPoints.total(); int i, j = 0, ni = 0, total = 0; CV_Assert(nimages > 0 && nimages == (int)imagePoints1.total() && (!imgPtMat2 || nimages == (int)imagePoints2.total())); cout << " Number of Frames: " << nimages << endl; for( i = 0; i < nimages; i++ ) { cout << endl << "Object Points: " << endl; printMatOBJ(objectPoints.getMat(i)); cout << endl << "Image Points: " << endl; printMatIMG(imagePoints1.getMat(i)); ni = objectPoints.getMat(i).checkVector(3, CV_32F); CV_Assert( ni >= 0 ); total += ni; } npoints.create(1, (int)nimages, CV_32S); objPtMat.create(1, (int)total, CV_32FC3); imgPtMat1.create(1, (int)total, CV_32FC2); Point2f* imgPtData2 = 0; if( imgPtMat2 ) { imgPtMat2->create(1, (int)total, CV_32FC2); imgPtData2 = imgPtMat2->ptr<Point2f>(); } Point3f* objPtData = objPtMat.ptr<Point3f>(); Point2f* imgPtData1 = imgPtMat1.ptr<Point2f>(); for( i = 0; i < nimages; i++, j += ni ) { Mat objpt = objectPoints.getMat(i); Mat imgpt1 = imagePoints1.getMat(i); ni = objpt.checkVector(3, CV_32F); int ni1 = imgpt1.checkVector(2, CV_32F); CV_Assert( ni > 0 && ni == ni1 ); npoints.at<int>(i) = ni; memcpy( objPtData + j, objpt.data, ni*sizeof(objPtData[0]) ); memcpy( imgPtData1 + j, imgpt1.data, ni*sizeof(imgPtData1[0]) ); if( imgPtData2 ) { Mat imgpt2 = imagePoints2.getMat(i); int ni2 = imgpt2.checkVector(2, CV_32F); CV_Assert( ni == ni2 ); memcpy( imgPtData2 + j, imgpt2.data, ni*sizeof(imgPtData2[0]) ); } } }
void cv::convertPointsHomogeneous( const Mat& src, vector<Point2f>& dst ) { CV_Assert(src.checkVector(3) >= 0 && (src.depth() == CV_32F || src.depth() == CV_32S)); dst.resize(src.cols*src.rows*src.channels()/3); CvMat _src = Mat(src), _dst = Mat(dst); cvConvertPointsHomogeneous(&_src, &_dst); }
void cv::solvePnP( InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess ) { 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 = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); CvMat c_objectPoints = opoints, c_imagePoints = ipoints; CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat(); cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, &c_rvec, &c_tvec, useExtrinsicGuess ); }
void cv::computeCorrespondEpilines( const Mat& points, int whichImage, const Mat& F, vector<Vec3f>& lines ) { CV_Assert(points.checkVector(2) >= 0 && (points.depth() == CV_32F || points.depth() == CV_32S)); lines.resize(points.cols*points.rows*points.channels()/2); CvMat _points = points, _lines = Mat(lines), matF = F; cvComputeCorrespondEpilines(&_points, whichImage, &matF, &_lines); }
static Mat _findFundamentalMat( const Mat& points1, const Mat& points2, int method, double param1, double param2, vector<uchar>* mask ) { CV_Assert(points1.checkVector(2) >= 0 && points2.checkVector(2) >= 0 && (points1.depth() == CV_32F || points1.depth() == CV_32S) && points1.depth() == points2.depth()); Mat F(3, 3, CV_64F); CvMat _pt1 = Mat(points1), _pt2 = Mat(points2); CvMat matF = F, _mask, *pmask = 0; if( mask ) { mask->resize(points1.cols*points1.rows*points1.channels()/2); pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0])); } int n = cvFindFundamentalMat( &_pt1, &_pt2, &matF, method, param1, param2, pmask ); if( n <= 0 ) F = Scalar(0); return F; }
void calcPosition( InputArray _tvecs, InputArray _rvecs, InputArray _pts, InputArray _cameraMatrices, InputArray _distortionMatrices, OutputArray _state, OutputArray _covariance ) { Ptr< PositionCalculator > p_pc = PositionCalculator::create(); std::vector< Mat > tvecs, rvecs; _tvecs.getMatVector( tvecs ); _rvecs.getMatVector( rvecs ); CV_Assert( tvecs.size() >= 2 ); CV_Assert( tvecs.size() == rvecs.size() ); Mat pts = _pts.getMat(); CV_Assert( ( tvecs.size() == pts.checkVector( 2, CV_32F, true ) ) ); std::vector< Mat > camera_m, dist_m; if ( _cameraMatrices.kind() == _InputArray::STD_VECTOR_MAT ) { _cameraMatrices.getMatVector( camera_m ); CV_Assert( tvecs.size() == camera_m.size() ); } else { camera_m.push_back( _cameraMatrices.getMat() ); CV_Assert( ( camera_m[0].rows == 3 ) && ( camera_m[0].cols == 3 ) ); } if ( _distortionMatrices.kind() == _InputArray::STD_VECTOR_MAT ) { _distortionMatrices.getMatVector( dist_m ); CV_Assert( tvecs.size() == dist_m.size() ); } else { dist_m.push_back( _distortionMatrices.getMat() ); CV_Assert( ( ( dist_m[0].rows == 5 ) && ( dist_m[0].cols == 1 ) ) || dist_m[0].empty() ); } Mat camera = camera_m[0]; Mat dist = dist_m[0]; for ( size_t i = 0; i < tvecs.size(); ++i ) { if ( camera_m.size() == tvecs.size() ) camera = camera_m[i]; if ( dist_m.size() == tvecs.size() ) dist = dist_m[i]; p_pc->addMeasurement( tvecs[i], rvecs[i], pts.at< Point2f >( i ), camera, dist ); } p_pc->computeState( _state, _covariance ); }
void cv::cornerSubPix( InputArray _image, InputOutputArray _corners, Size winSize, Size zeroZone, TermCriteria criteria ) { Mat corners = _corners.getMat(); int ncorners = corners.checkVector(2); CV_Assert( ncorners >= 0 && corners.depth() == CV_32F ); Mat image = _image.getMat(); CvMat c_image = image; cvFindCornerSubPix( &c_image, (CvPoint2D32f*)corners.data, ncorners, winSize, zeroZone, criteria ); }
bool isContourConvex( InputArray _contour ) { Mat contour = _contour.getMat(); int total = contour.checkVector(2), depth = contour.depth(); CV_Assert(total >= 0 && (depth == CV_32F || depth == CV_32S)); if( total == 0 ) return false; return depth == CV_32S ? isContourConvex_(contour.ptr<Point>(), total ) : isContourConvex_(contour.ptr<Point2f>(), total ); }
cv::RotatedRect cv::minAreaRect( InputArray _points ) { Mat hull; Point2f out[3]; RotatedRect box; convexHull(_points, hull, true, true); if( hull.depth() != CV_32F ) { Mat temp; hull.convertTo(temp, CV_32F); hull = temp; } int n = hull.checkVector(2); const Point2f* hpoints = hull.ptr<Point2f>(); if( n > 2 ) { rotatingCalipers( hpoints, n, CALIPERS_MINAREARECT, (float*)out ); box.center.x = out[0].x + (out[1].x + out[2].x)*0.5f; box.center.y = out[0].y + (out[1].y + out[2].y)*0.5f; box.size.width = (float)std::sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y); box.size.height = (float)std::sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y); box.angle = (float)atan2( (double)out[1].y, (double)out[1].x ); } else if( n == 2 ) { box.center.x = (hpoints[0].x + hpoints[1].x)*0.5f; box.center.y = (hpoints[0].y + hpoints[1].y)*0.5f; double dx = hpoints[1].x - hpoints[0].x; double dy = hpoints[1].y - hpoints[0].y; box.size.width = (float)std::sqrt(dx*dx + dy*dy); box.size.height = 0; box.angle = (float)atan2( dy, dx ); } else { if( n == 1 ) box.center = hpoints[0]; } box.angle = (float)(box.angle*180/CV_PI); return box; }
// calculates length of a curve (e.g. contour perimeter) double cv::arcLength( InputArray _curve, bool is_closed ) { Mat curve = _curve.getMat(); int count = curve.checkVector(2); int depth = curve.depth(); CV_Assert( count >= 0 && (depth == CV_32F || depth == CV_32S)); double perimeter = 0; int i, j = 0; const int N = 16; float buf[N]; if( count <= 1 ) return 0.; bool is_float = depth == CV_32F; int last = is_closed ? count-1 : 0; const Point* pti = (const Point*)curve.data; const Point2f* ptf = (const Point2f*)curve.data; Point2f prev = is_float ? ptf[last] : Point2f((float)pti[last].x,(float)pti[last].y); for( i = 0; i < count; i++ ) { Point2f p = is_float ? ptf[i] : Point2f((float)pti[i].x,(float)pti[i].y); float dx = p.x - prev.x, dy = p.y - prev.y; buf[j] = dx*dx + dy*dy; if( ++j == N || i == count-1 ) { Mat bufmat(1, j, CV_32F, buf); sqrt(bufmat, bufmat); for( ; j > 0; j-- ) perimeter += buf[j-1]; } prev = p; } return perimeter; }
/* Post: fill _err with projection errors */ void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const { Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat(); int i, count = opoints.checkVector(3); Mat _rvec = model.col(0); Mat _tvec = model.col(1); Mat projpoints(count, 2, CV_32FC1); projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); const Point2f* ipoints_ptr = ipoints.ptr<Point2f>(); const Point2f* projpoints_ptr = projpoints.ptr<Point2f>(); _err.create(count, 1, CV_32FC1); float* err = _err.getMat().ptr<float>(); for ( i = 0; i < count; ++i) err[i] = (float)norm( Matx21f(ipoints_ptr[i] - projpoints_ptr[i]), NORM_L2SQR ); }
bool solvePnP( InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) { CV_INSTRUMENT_REGION() 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)) ); 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 { int mtype = CV_64F; // use CV_32F if all PnP inputs are CV_32F and outputs are empty if (_ipoints.depth() == _cameraMatrix.depth() && _ipoints.depth() == _opoints.depth() && _rvec.empty() && _tvec.empty()) mtype = _opoints.depth(); _rvec.create(3, 1, mtype); _tvec.create(3, 1, mtype); } rvec = _rvec.getMat(); tvec = _tvec.getMat(); Mat cameraMatrix0 = _cameraMatrix.getMat(); Mat distCoeffs0 = _distCoeffs.getMat(); Mat cameraMatrix = Mat_<double>(cameraMatrix0); Mat distCoeffs = Mat_<double>(distCoeffs0); bool result = false; if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) { Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); epnp PnP(cameraMatrix, opoints, undistortedPoints); Mat R; PnP.compute_pose(R, tvec); Rodrigues(R, rvec); result = true; } else if (flags == SOLVEPNP_P3P) { CV_Assert( npoints == 4); Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); p3p P3Psolver(cameraMatrix); Mat R; result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); if (result) Rodrigues(R, rvec); } else if (flags == SOLVEPNP_AP3P) { CV_Assert( npoints == 4); Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); ap3p P3Psolver(cameraMatrix); Mat R; result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); if (result) Rodrigues(R, rvec); } else if (flags == SOLVEPNP_ITERATIVE) { CvMat c_objectPoints = opoints, c_imagePoints = ipoints; CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; 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 ); result = true; } /*else if (flags == SOLVEPNP_DLS) { Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); dls PnP(opoints, undistortedPoints); Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); bool result = PnP.compute_pose(R, tvec); if (result) Rodrigues(R, rvec); return result; } else if (flags == SOLVEPNP_UPNP) { upnp PnP(cameraMatrix, opoints, ipoints); Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); PnP.compute_pose(R, tvec); Rodrigues(R, rvec); return true; }*/ else CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); return result; }
void convexityDefects( InputArray _points, InputArray _hull, OutputArray _defects ) { CV_INSTRUMENT_REGION() Mat points = _points.getMat(); int i, j = 0, npoints = points.checkVector(2, CV_32S); CV_Assert( npoints >= 0 ); if( npoints <= 3 ) { _defects.release(); return; } Mat hull = _hull.getMat(); int hpoints = hull.checkVector(1, CV_32S); CV_Assert( hpoints > 0 ); const Point* ptr = points.ptr<Point>(); const int* hptr = hull.ptr<int>(); std::vector<Vec4i> defects; if ( hpoints < 3 ) //if hull consists of one or two points, contour is always convex { _defects.release(); return; } // 1. recognize co-orientation of the contour and its hull bool rev_orientation = ((hptr[1] > hptr[0]) + (hptr[2] > hptr[1]) + (hptr[0] > hptr[2])) != 2; // 2. cycle through points and hull, compute defects int hcurr = hptr[rev_orientation ? 0 : hpoints-1]; CV_Assert( 0 <= hcurr && hcurr < npoints ); for( i = 0; i < hpoints; i++ ) { int hnext = hptr[rev_orientation ? hpoints - i - 1 : i]; CV_Assert( 0 <= hnext && hnext < npoints ); Point pt0 = ptr[hcurr], pt1 = ptr[hnext]; double dx0 = pt1.x - pt0.x; double dy0 = pt1.y - pt0.y; double scale = dx0 == 0 && dy0 == 0 ? 0. : 1./std::sqrt(dx0*dx0 + dy0*dy0); int defect_deepest_point = -1; double defect_depth = 0; bool is_defect = false; j=hcurr; for(;;) { // go through points to achieve next hull point j++; j &= j >= npoints ? 0 : -1; if( j == hnext ) break; // compute distance from current point to hull edge double dx = ptr[j].x - pt0.x; double dy = ptr[j].y - pt0.y; double dist = fabs(-dy0*dx + dx0*dy) * scale; if( dist > defect_depth ) { defect_depth = dist; defect_deepest_point = j; is_defect = true; } } if( is_defect ) { int idepth = cvRound(defect_depth*256); defects.push_back(Vec4i(hcurr, hnext, defect_deepest_point, idepth)); } hcurr = hnext; } Mat(defects).copyTo(_defects); }
double cv::pointPolygonTest( InputArray _contour, Point2f pt, bool measureDist ) { CV_INSTRUMENT_REGION(); double result = 0; Mat contour = _contour.getMat(); int i, total = contour.checkVector(2), counter = 0; int depth = contour.depth(); CV_Assert( total >= 0 && (depth == CV_32S || depth == CV_32F)); bool is_float = depth == CV_32F; double min_dist_num = FLT_MAX, min_dist_denom = 1; Point ip(cvRound(pt.x), cvRound(pt.y)); if( total == 0 ) return measureDist ? -DBL_MAX : -1; const Point* cnt = contour.ptr<Point>(); const Point2f* cntf = (const Point2f*)cnt; if( !is_float && !measureDist && ip.x == pt.x && ip.y == pt.y ) { // the fastest "purely integer" branch Point v0, v = cnt[total-1]; for( i = 0; i < total; i++ ) { v0 = v; v = cnt[i]; if( (v0.y <= ip.y && v.y <= ip.y) || (v0.y > ip.y && v.y > ip.y) || (v0.x < ip.x && v.x < ip.x) ) { if( ip.y == v.y && (ip.x == v.x || (ip.y == v0.y && ((v0.x <= ip.x && ip.x <= v.x) || (v.x <= ip.x && ip.x <= v0.x)))) ) return 0; continue; } int64 dist = static_cast<int64>(ip.y - v0.y)*(v.x - v0.x) - static_cast<int64>(ip.x - v0.x)*(v.y - v0.y); if( dist == 0 ) return 0; if( v.y < v0.y ) dist = -dist; counter += dist > 0; } result = counter % 2 == 0 ? -1 : 1; } else { Point2f v0, v; Point iv; if( is_float ) { v = cntf[total-1]; } else { v = cnt[total-1]; } if( !measureDist ) { for( i = 0; i < total; i++ ) { double dist; v0 = v; if( is_float ) v = cntf[i]; else v = cnt[i]; if( (v0.y <= pt.y && v.y <= pt.y) || (v0.y > pt.y && v.y > pt.y) || (v0.x < pt.x && v.x < pt.x) ) { if( pt.y == v.y && (pt.x == v.x || (pt.y == v0.y && ((v0.x <= pt.x && pt.x <= v.x) || (v.x <= pt.x && pt.x <= v0.x)))) ) return 0; continue; } dist = (double)(pt.y - v0.y)*(v.x - v0.x) - (double)(pt.x - v0.x)*(v.y - v0.y); if( dist == 0 ) return 0; if( v.y < v0.y ) dist = -dist; counter += dist > 0; } result = counter % 2 == 0 ? -1 : 1; } else { for( i = 0; i < total; i++ ) { double dx, dy, dx1, dy1, dx2, dy2, dist_num, dist_denom = 1; v0 = v; if( is_float ) v = cntf[i]; else v = cnt[i]; dx = v.x - v0.x; dy = v.y - v0.y; dx1 = pt.x - v0.x; dy1 = pt.y - v0.y; dx2 = pt.x - v.x; dy2 = pt.y - v.y; if( dx1*dx + dy1*dy <= 0 ) dist_num = dx1*dx1 + dy1*dy1; else if( dx2*dx + dy2*dy >= 0 ) dist_num = dx2*dx2 + dy2*dy2; else { dist_num = (dy1*dx - dx1*dy); dist_num *= dist_num; dist_denom = dx*dx + dy*dy; } if( dist_num*min_dist_denom < min_dist_num*dist_denom ) { min_dist_num = dist_num; min_dist_denom = dist_denom; if( min_dist_num == 0 ) break; } if( (v0.y <= pt.y && v.y <= pt.y) || (v0.y > pt.y && v.y > pt.y) || (v0.x < pt.x && v.x < pt.x) ) continue; dist_num = dy1*dx - dx1*dy; if( dy < 0 ) dist_num = -dist_num; counter += dist_num > 0; } result = std::sqrt(min_dist_num/min_dist_denom); if( counter % 2 == 0 ) result = -result; } } return result; }
// Calculates bounding rectagnle of a point set or retrieves already calculated static Rect pointSetBoundingRect( const Mat& points ) { int npoints = points.checkVector(2); int depth = points.depth(); CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_32S)); int xmin = 0, ymin = 0, xmax = -1, ymax = -1, i; bool is_float = depth == CV_32F; if( npoints == 0 ) return Rect(); const Point* pts = (const Point*)points.data; Point pt = pts[0]; #if CV_SSE4_2 if(cv::checkHardwareSupport(CV_CPU_SSE4_2)) { if( !is_float ) { __m128i minval, maxval; minval = maxval = _mm_loadl_epi64((const __m128i*)(&pt)); //min[0]=pt.x, min[1]=pt.y for( i = 1; i < npoints; i++ ) { __m128i ptXY = _mm_loadl_epi64((const __m128i*)&pts[i]); minval = _mm_min_epi32(ptXY, minval); maxval = _mm_max_epi32(ptXY, maxval); } xmin = _mm_cvtsi128_si32(minval); ymin = _mm_cvtsi128_si32(_mm_srli_si128(minval, 4)); xmax = _mm_cvtsi128_si32(maxval); ymax = _mm_cvtsi128_si32(_mm_srli_si128(maxval, 4)); } else { __m128 minvalf, maxvalf, z = _mm_setzero_ps(), ptXY = _mm_setzero_ps(); minvalf = maxvalf = _mm_loadl_pi(z, (const __m64*)(&pt)); for( i = 1; i < npoints; i++ ) { ptXY = _mm_loadl_pi(ptXY, (const __m64*)&pts[i]); minvalf = _mm_min_ps(minvalf, ptXY); maxvalf = _mm_max_ps(maxvalf, ptXY); } float xyminf[2], xymaxf[2]; _mm_storel_pi((__m64*)xyminf, minvalf); _mm_storel_pi((__m64*)xymaxf, maxvalf); xmin = cvFloor(xyminf[0]); ymin = cvFloor(xyminf[1]); xmax = cvFloor(xymaxf[0]); ymax = cvFloor(xymaxf[1]); } } else #endif { if( !is_float ) { xmin = xmax = pt.x; ymin = ymax = pt.y; for( i = 1; i < npoints; i++ ) { pt = pts[i]; if( xmin > pt.x ) xmin = pt.x; if( xmax < pt.x ) xmax = pt.x; if( ymin > pt.y ) ymin = pt.y; if( ymax < pt.y ) ymax = pt.y; } } else { Cv32suf v; // init values xmin = xmax = CV_TOGGLE_FLT(pt.x); ymin = ymax = CV_TOGGLE_FLT(pt.y); for( i = 1; i < npoints; i++ ) { pt = pts[i]; pt.x = CV_TOGGLE_FLT(pt.x); pt.y = CV_TOGGLE_FLT(pt.y); if( xmin > pt.x ) xmin = pt.x; if( xmax < pt.x ) xmax = pt.x; if( ymin > pt.y ) ymin = pt.y; if( ymax < pt.y ) ymax = pt.y; } v.i = CV_TOGGLE_FLT(xmin); xmin = cvFloor(v.f); v.i = CV_TOGGLE_FLT(ymin); ymin = cvFloor(v.f); // because right and bottom sides of the bounding rectangle are not inclusive // (note +1 in width and height calculation below), cvFloor is used here instead of cvCeil v.i = CV_TOGGLE_FLT(xmax); xmax = cvFloor(v.f); v.i = CV_TOGGLE_FLT(ymax); ymax = cvFloor(v.f); } } return Rect(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1); }
cv::RotatedRect cv::fitEllipse( InputArray _points ) { Mat points = _points.getMat(); int i, n = points.checkVector(2); int depth = points.depth(); CV_Assert( n >= 0 && (depth == CV_32F || depth == CV_32S)); RotatedRect box; if( n < 5 ) CV_Error( CV_StsBadSize, "There should be at least 5 points to fit the ellipse" ); // New fitellipse algorithm, contributed by Dr. Daniel Weiss Point2f c(0,0); double gfp[5], rp[5], t; const double min_eps = 1e-6; bool is_float = depth == CV_32F; const Point* ptsi = (const Point*)points.data; const Point2f* ptsf = (const Point2f*)points.data; AutoBuffer<double> _Ad(n*5), _bd(n); double *Ad = _Ad, *bd = _bd; // first fit for parameters A - E Mat A( n, 5, CV_64F, Ad ); Mat b( n, 1, CV_64F, bd ); Mat x( 5, 1, CV_64F, gfp ); for( i = 0; i < n; i++ ) { Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); c += p; } c.x /= n; c.y /= n; for( i = 0; i < n; i++ ) { Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); p -= c; bd[i] = 10000.0; // 1.0? Ad[i*5] = -(double)p.x * p.x; // A - C signs inverted as proposed by APP Ad[i*5 + 1] = -(double)p.y * p.y; Ad[i*5 + 2] = -(double)p.x * p.y; Ad[i*5 + 3] = p.x; Ad[i*5 + 4] = p.y; } solve(A, b, x, DECOMP_SVD); // now use general-form parameters A - E to find the ellipse center: // differentiate general form wrt x/y to get two equations for cx and cy A = Mat( 2, 2, CV_64F, Ad ); b = Mat( 2, 1, CV_64F, bd ); x = Mat( 2, 1, CV_64F, rp ); Ad[0] = 2 * gfp[0]; Ad[1] = Ad[2] = gfp[2]; Ad[3] = 2 * gfp[1]; bd[0] = gfp[3]; bd[1] = gfp[4]; solve( A, b, x, DECOMP_SVD ); // re-fit for parameters A - C with those center coordinates A = Mat( n, 3, CV_64F, Ad ); b = Mat( n, 1, CV_64F, bd ); x = Mat( 3, 1, CV_64F, gfp ); for( i = 0; i < n; i++ ) { Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); p -= c; bd[i] = 1.0; Ad[i * 3] = (p.x - rp[0]) * (p.x - rp[0]); Ad[i * 3 + 1] = (p.y - rp[1]) * (p.y - rp[1]); Ad[i * 3 + 2] = (p.x - rp[0]) * (p.y - rp[1]); } solve(A, b, x, DECOMP_SVD); // store angle and radii rp[4] = -0.5 * atan2(gfp[2], gfp[1] - gfp[0]); // convert from APP angle usage t = sin(-2.0 * rp[4]); if( fabs(t) > fabs(gfp[2])*min_eps ) t = gfp[2]/t; else t = gfp[1] - gfp[0]; rp[2] = fabs(gfp[0] + gfp[1] - t); if( rp[2] > min_eps ) rp[2] = std::sqrt(2.0 / rp[2]); rp[3] = fabs(gfp[0] + gfp[1] + t); if( rp[3] > min_eps ) rp[3] = std::sqrt(2.0 / rp[3]); box.center.x = (float)rp[0] + c.x; box.center.y = (float)rp[1] + c.y; box.size.width = (float)(rp[2]*2); box.size.height = (float)(rp[3]*2); if( box.size.width > box.size.height ) { float tmp; CV_SWAP( box.size.width, box.size.height, tmp ); box.angle = (float)(90 + rp[4]*180/CV_PI); } if( box.angle < -180 ) box.angle += 360; if( box.angle > 360 ) box.angle -= 360; return box; }
void cv::minEnclosingCircle( InputArray _points, Point2f& _center, float& _radius ) { int max_iters = 100; const float eps = FLT_EPSILON*2; bool result = false; Mat points = _points.getMat(); int i, j, k, count = points.checkVector(2); int depth = points.depth(); Point2f center; float radius = 0.f; CV_Assert(count >= 0 && (depth == CV_32F || depth == CV_32S)); _center.x = _center.y = 0.f; _radius = 0.f; if( count == 0 ) return; bool is_float = depth == CV_32F; const Point* ptsi = (const Point*)points.data; const Point2f* ptsf = (const Point2f*)points.data; Point2f pt = is_float ? ptsf[0] : Point2f((float)ptsi[0].x,(float)ptsi[0].y); Point2f pts[4] = {pt, pt, pt, pt}; for( i = 1; i < count; i++ ) { pt = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); if( pt.x < pts[0].x ) pts[0] = pt; if( pt.x > pts[1].x ) pts[1] = pt; if( pt.y < pts[2].y ) pts[2] = pt; if( pt.y > pts[3].y ) pts[3] = pt; } for( k = 0; k < max_iters; k++ ) { double min_delta = 0, delta; Point2f farAway(0,0); /*only for first iteration because the alg is repared at the loop's foot*/ if( k == 0 ) findEnslosingCicle4pts_32f( pts, center, radius ); for( i = 0; i < count; i++ ) { pt = is_float ? ptsf[i] : Point2f((float)ptsi[i].x,(float)ptsi[i].y); delta = pointInCircle( pt, center, radius ); if( delta < min_delta ) { min_delta = delta; farAway = pt; } } result = min_delta >= 0; if( result ) break; Point2f ptsCopy[4]; // find good replacement partner for the point which is at most far away, // starting with the one that lays in the actual circle (i=3) for( i = 3; i >= 0; i-- ) { for( j = 0; j < 4; j++ ) ptsCopy[j] = i != j ? pts[j] : farAway; findEnslosingCicle4pts_32f( ptsCopy, center, radius ); if( pointInCircle( pts[i], center, radius ) >= 0) { // replaced one again in the new circle? pts[i] = farAway; break; } } } if( !result ) { radius = 0.f; for( i = 0; i < count; i++ ) { pt = is_float ? ptsf[i] : Point2f((float)ptsi[i].x,(float)ptsi[i].y); float dx = center.x - pt.x, dy = center.y - pt.y; float t = dx*dx + dy*dy; radius = MAX(radius, t); } radius = (float)(std::sqrt(radius)*(1 + eps)); } _center = center; _radius = radius; }
CV_IMPL void cvBoxPoints( CvBox2D box, CvPoint2D32f pt[4] ) { if( !pt ) CV_Error( CV_StsNullPtr, "NULL vertex array pointer" ); cv::RotatedRect(box).points((cv::Point2f*)pt); } double cv::pointPolygonTest( InputArray _contour, Point2f pt, bool measureDist ) { CV_INSTRUMENT_REGION() double result = 0; Mat contour = _contour.getMat(); int i, total = contour.checkVector(2), counter = 0; int depth = contour.depth(); CV_Assert( total >= 0 && (depth == CV_32S || depth == CV_32F)); bool is_float = depth == CV_32F; double min_dist_num = FLT_MAX, min_dist_denom = 1; Point ip(cvRound(pt.x), cvRound(pt.y)); if( total == 0 ) return measureDist ? -DBL_MAX : -1; const Point* cnt = contour.ptr<Point>(); const Point2f* cntf = (const Point2f*)cnt; if( !is_float && !measureDist && ip.x == pt.x && ip.y == pt.y ) {
static Moments contourMoments( const Mat& contour ) { Moments m; int lpt = contour.checkVector(2); int is_float = contour.depth() == CV_32F; const Point* ptsi = (const Point*)contour.data; const Point2f* ptsf = (const Point2f*)contour.data; CV_Assert( contour.depth() == CV_32S || contour.depth() == CV_32F ); if( lpt == 0 ) return m; double a00 = 0, a10 = 0, a01 = 0, a20 = 0, a11 = 0, a02 = 0, a30 = 0, a21 = 0, a12 = 0, a03 = 0; double xi, yi, xi2, yi2, xi_1, yi_1, xi_12, yi_12, dxy, xii_1, yii_1; if( !is_float ) { xi_1 = ptsi[lpt-1].x; yi_1 = ptsi[lpt-1].y; } else { xi_1 = ptsf[lpt-1].x; yi_1 = ptsf[lpt-1].y; } xi_12 = xi_1 * xi_1; yi_12 = yi_1 * yi_1; for( int i = 0; i < lpt; i++ ) { if( !is_float ) { xi = ptsi[i].x; yi = ptsi[i].y; } else { xi = ptsf[i].x; yi = ptsf[i].y; } xi2 = xi * xi; yi2 = yi * yi; dxy = xi_1 * yi - xi * yi_1; xii_1 = xi_1 + xi; yii_1 = yi_1 + yi; a00 += dxy; a10 += dxy * xii_1; a01 += dxy * yii_1; a20 += dxy * (xi_1 * xii_1 + xi2); a11 += dxy * (xi_1 * (yii_1 + yi_1) + xi * (yii_1 + yi)); a02 += dxy * (yi_1 * yii_1 + yi2); a30 += dxy * xii_1 * (xi_12 + xi2); a03 += dxy * yii_1 * (yi_12 + yi2); a21 += dxy * (xi_12 * (3 * yi_1 + yi) + 2 * xi * xi_1 * yii_1 + xi2 * (yi_1 + 3 * yi)); a12 += dxy * (yi_12 * (3 * xi_1 + xi) + 2 * yi * yi_1 * xii_1 + yi2 * (xi_1 + 3 * xi)); xi_1 = xi; yi_1 = yi; xi_12 = xi2; yi_12 = yi2; } if( fabs(a00) > FLT_EPSILON ) { double db1_2, db1_6, db1_12, db1_24, db1_20, db1_60; if( a00 > 0 ) { db1_2 = 0.5; db1_6 = 0.16666666666666666666666666666667; db1_12 = 0.083333333333333333333333333333333; db1_24 = 0.041666666666666666666666666666667; db1_20 = 0.05; db1_60 = 0.016666666666666666666666666666667; } else { db1_2 = -0.5; db1_6 = -0.16666666666666666666666666666667; db1_12 = -0.083333333333333333333333333333333; db1_24 = -0.041666666666666666666666666666667; db1_20 = -0.05; db1_60 = -0.016666666666666666666666666666667; } // spatial moments m.m00 = a00 * db1_2; m.m10 = a10 * db1_6; m.m01 = a01 * db1_6; m.m20 = a20 * db1_12; m.m11 = a11 * db1_24; m.m02 = a02 * db1_12; m.m30 = a30 * db1_20; m.m21 = a21 * db1_60; m.m12 = a12 * db1_60; m.m03 = a03 * db1_20; completeMomentState( &m ); } return m; }
bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, OutputArray _inliers, int flags) { 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)) ); CV_Assert(opoints.isContinuous()); CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F); CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3); CV_Assert(ipoints.isContinuous()); CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F); 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 = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1); Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); Ptr<PointSetRegistrator::Callback> cb; // pointer to callback cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec); int model_points = 4; // minimum of number of model points if( flags == cv::SOLVEPNP_ITERATIVE ) model_points = 6; else if( flags == cv::SOLVEPNP_UPNP ) model_points = 6; else if( flags == cv::SOLVEPNP_EPNP ) model_points = 5; double param1 = reprojectionError; // reprojection error double param2 = confidence; // confidence int param3 = 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); if( result <= 0 || _local_model.rows <= 0) { _rvec.assign(rvec); // output rotation vector _tvec.assign(tvec); // output translation vector if( _inliers.needed() ) _inliers.release(); return false; } else { _rvec.assign(_local_model.col(0)); // output rotation vector _tvec.assign(_local_model.col(1)); // output translation vector } if(_inliers.needed()) { Mat _local_inliers; int count = 0; for (int i = 0; i < _mask_local_inliers.rows; ++i) { if((int)_mask_local_inliers.at<uchar>(i) == 1) // inliers mask { _local_inliers.push_back(count); // output inliers vector count++; } } _local_inliers.copyTo(_inliers); } return true; }