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; }
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; }