Revision history [back]

I use openCV function projectPoints to rotate/translate and project a set of 3D points and solvePnp to find this rotation/translation. This works when distortion coefficients (lens distortion) are all zero but fails otherwise. The code is below:

const int npoints = 10; // 6 min, but  CV_P3P, CV_P3P can work with 4 points

// extrinsic
const Point3f tvec(10, 20, 30);
Point3f rvec(3, 5, 7);
cout << "Finding extrinsic parameters (PnP)" << endl;
cout<<"Test transformations: ";
cout<<"Rotation: "<<rvec<<"; translation: "<<tvec<<endl;
cout<<"Intrinsic: "<<endl;
rvec*=DEG2RAD;

// intrinsic
Mat_ <double>cameraMatrix(3, 3);
cameraMatrix << 300., 0., 200., 0, 300., 100., 0., 0., 1.;
Mat_ <double>distCoeffs(1, 5); //  (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements.
distCoeffs << 1.2, 0.2, 0., 0., 0.;
//distCoeffs << 0.0, 0.0, 0.0, 0.0, 0.0;
cout<<"distrotion coeff: "<<distCoeffs<<endl;

cout<<"============= Running PnP..."<<endl;

vector<Point3f> objectPoints(npoints);
vector<Point2f> imagePoints(npoints);
Mat rvec_est, tvec_est;
vector<Point3f> objPts(npoints);
randu(Mat(objPts), 0.0f, 100.0f);

// project
projectPoints(Mat(objPts), Mat(rvec), Mat(tvec), cameraMatrix, distCoeffs, Mat(imagePoints));

// extrinsic
solvePnP(objPts, imagePoints, cameraMatrix, distCoeffs, rvec_est, tvec_est);
cout<<"Rotation: "<<rvec_est*RAD2DEG<<endl;
cout<<"Translation "<<tvec_est<<endl;

When all distortion coefficients are 0 the result is OK:

Finding extrinsic parameters (PnP) Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30] distrotion coeff: [0, 0, 0, 0, 0] ============= Running PnP... Rotation: [2.999999581709123; 4.999997813985293; 6.999999826089725] Translation [9.999999792663072; 19.99999648222693; 29.99999699621362]

However when they aren't zero the result is totally wrong:

Finding extrinsic parameters (PnP) Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30] distrotion coeff: [1.2, 0.2, 0, 0, 0] ============= Running PnP... Rotation: [-91.56479629305277; -124.3631985067845; -74.46486950666471] Translation [-69.72473511009439; -117.7463271636532; -87.27777166027946]