BOOL PnP445(char *target, void* conn,EXINFO exinfo)
{
for(int i=0;i<3;i++){
if(PnP(target,conn,exinfo,i)) return TRUE;
else return FALSE;
}
return FALSE;
}
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 testEPNP() {
ublas::matrix<double> boardPoints(3,12);
int p=0;
for(int w=0; w<2/*7*/; w++) {
for(int h=0; h<6; h++) {
boardPoints(0,p) = -20.0+w*5.0;
boardPoints(1,p) = -20.0 + h*5.0;
boardPoints(2,p) = 0.0;
p++;
}
}
ublas::matrix<double> cameraMatrix(3,3);
cameraMatrix(0,0) = 654.8611202347574;
cameraMatrix(0,1) = 0;
cameraMatrix(0,2) = 319.5;
cameraMatrix(1,0) = 0;
cameraMatrix(1,1) = 654.8611202347574;
cameraMatrix(1,2) = 239.5;
cameraMatrix(2,0) = 0;
cameraMatrix(2,1) = 0;
cameraMatrix(2,2) = 1;
ublas::vector<double> distCoeffVec(5);
distCoeffVec(0) = -0.3669624087278113;
distCoeffVec(1) = -0.07638290902180184;
distCoeffVec(2) = 0;
distCoeffVec(3) = 0;
distCoeffVec(4) = 0.5764668364450144;
ublas::matrix<double> foundPoints(2,12);
foundPoints(0,0) = 122.56181;
foundPoints(1,0) = 64.894775;
foundPoints(0,1) = 163.85579;
foundPoints(1,1) = 63.682297;
foundPoints(0,2) = 204.5;
foundPoints(1,2) = 62;
foundPoints(0,2) = 245.62991;
foundPoints(1,2) = 61.093784;
foundPoints(0,3) = 283;
foundPoints(1,3) = 61;
foundPoints(0,4) = 319.81903;
foundPoints(1,4) = 61.967384;
foundPoints(0,5) = 354.08017;
foundPoints(1,5) = 62.274704;
foundPoints(0,6) = 123;
foundPoints(1,6) = 104.5;
foundPoints(0,7) = 164.5;
foundPoints(1,7) = 102.5;
foundPoints(0,8) = 204.5;
foundPoints(1,8) = 100.5;
foundPoints(0,9) = 244;
foundPoints(1,9) = 99.5;
foundPoints(0,10) = 281.5;
foundPoints(1,10) = 99;
foundPoints(0,11) = 318.5;
foundPoints(1,11) = 99;
/* foundPoints(353.72653, 96.017204));
foundPoints(124.62646,144.43448));
foundPoints(165.5,142.5));
foundPoints(206.03426,140.04895));
foundPoints(245,138.5));
foundPoints(283,137.5));
foundPoints(319,136));
foundPoints(354.5,134.5));
foundPoints(127.50209,184.51553));
foundPoints(167.5,181.5));
foundPoints(207,179));
foundPoints(245.5,177));
foundPoints(283,175));
foundPoints(318.88574,174.8522));
foundPoints(353.5,170.5));
foundPoints(128.28163,224.11998));
foundPoints(169,220.5));
foundPoints(210.13632,217.0213));
foundPoints(247,214.5));
foundPoints(282.64105,212.52209));
foundPoints(319.19608,209.22551));
foundPoints(343,206));
foundPoints(134,260.5));
foundPoints(172.92181,256.8718));
foundPoints(211,255));
foundPoints(248.5,250.5));
foundPoints(285,248));
foundPoints(319.5,243));
foundPoints(353.30963,240.85687)); */
epnp PnP(cameraMatrix, boardPoints, foundPoints/*undistortedPoints*/);
ublas::matrix<double> R (3,3);
ublas::vector<double> tvec(3);
PnP.compute_pose(R,tvec);
print("R", R);
print("tvec", tvec);
}
