title: undistortPoints() cannot handle lens distortions
body: <p>I use openCV function projectPoints() to rotate, translate and project a set of 3D points and solvePnp() to find this rotation and translation. This works well when the lens distortion coefficients are all zero but fails otherwise. It takes as little distortion as this to fail completely: </p>
  
  <pre><code> distCoeffs &lt;&lt; 0.0, 0.01, 0.0, 0.0, 0.0;  
  </code></pre>
  
  <p>The code is below:</p>
  
  <pre><code>#include &lt;iostream&gt;
  #include "opencv.hpp"
  using namespace std;
  using namespace cv;
  #define DEG2RAD (3.1415293/180.0)
  #define RAD2DEG (1.0/DEG2RAD)
  
  int main() {
      const int npoints = 10; // number of points
  
      // extrinsic
      const Point3f tvec(10, 20, 30);
      Point3f rvec(3, 5, 7);
      cout &lt;&lt; "Finding extrinsic parameters (PnP)" &lt;&lt; endl;
      cout&lt;&lt;"Test transformations: ";
      cout&lt;&lt;"Rotation: "&lt;&lt;rvec&lt;&lt;"; translation: "&lt;&lt;tvec&lt;&lt;endl;
      rvec*=DEG2RAD;
  
      // intrinsic
      Mat_ &lt;double&gt;cameraMatrix(3, 3);
      cameraMatrix &lt;&lt; 300., 0., 200., 0, 300., 100., 0., 0., 1.;
      Mat_ &lt;double&gt;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 &lt;&lt; 1.2, 0.2, 0., 0., 0.;  // non-zero distortion
      distCoeffs &lt;&lt; 0.0, 0.0, 0.0, 0.0, 0.0; // zero distortion
      cout&lt;&lt;"distrotion coeff: "&lt;&lt;distCoeffs&lt;&lt;endl;
  
      cout&lt;&lt;"============= Running PnP..."&lt;&lt;endl;
      vector&lt;Point3f&gt; objPts(npoints);
      vector&lt;Point2f&gt; imagePoints(npoints);
      Mat rvec_est, tvec_est;
      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&lt;&lt;"Rotation: "&lt;&lt;rvec_est*RAD2DEG&lt;&lt;endl;
      cout&lt;&lt;"Translation "&lt;&lt;tvec_est&lt;&lt;endl;
  
      return 0;
  }
  </code></pre>
  
  <p><strong>When all distortion coefficients are 0 the result is OK:</strong>  </p>
  
  <p>Finding extrinsic parameters (PnP)<br>
  Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30]<br>
  distrotion coeff: [0, 0, 0, 0, 0]<br>
  ============= Running PnP...<br>
  Rotation: [2.999999581709123; 4.999997813985293; 6.999999826089725]<br>
  Translation [9.999999792663072; 19.99999648222693; 29.99999699621362]  </p>
  
  <p><strong>However when they aren't zero the result is totally wrong:</strong></p>
  
  <p>Finding extrinsic parameters (PnP)<br>
  Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30]<br>
  distrotion coeff: [1.2, 0.2, 0, 0, 0]<br>
  ============= Running PnP...<br>
  Rotation: [-91.56479629305277; -124.3631985067845; -74.46486950666471]<br>
  Translation [-69.72473511009439; -117.7463271636532; -87.27777166027946]  </p>
  
  <p>Since people asked, I am adding <strong>intermediate input</strong> - some 3D points and their projections for non-zero distortion coefficients. My camera matrix was 
  cameraMatrix &lt;&lt; 300., 0., 200., 0, 300., 100., 0., 0., 1.;  </p>
  
  <p>3d points [53.0283, 19.9259, 40.1059]; 2D projection [1060.34, 700.59]<br>
  3d points [81.4385, 43.7133, 24.879]; 2D projection [6553.88, 5344.22]<br>
  3d points [77.3105, 76.2094, 30.7794]; 2D projection [5143.32, 6497.12]<br>
  3d points [70.2432, 47.8447, 79.219]; 2D projection [771.497, 611.726]</p>
  
  <p>Another interesting observation: applying undistort when distCoeff are non zero doesn’t really works (but it does produce identical 2D points when distortion coefficients are all 0):</p>
  
  <pre><code>cout&lt;&lt;"applying undistort..."&lt;&lt;endl;
  vector&lt;Point2f&gt; imagePointsUndistort(npoints);
  undistortPoints(Mat(imagePoints), Mat(imagePointsUndistort), cameraMatrix, distCoeffs);
  for (int i=0; i&lt;4; i++)
      cout&lt;&lt;"2d original "&lt;&lt;imagePoints[i]&lt;&lt;"; 2d undistort "&lt;&lt;imagePointsUndistort[i]&lt;&lt;endl;
  </code></pre>
  
  <p>applying undistort...<br>
  2d original [1060.34, 700.59]; 2d undistort [0, 0]<br>
  2d original [6553.88, 5344.22]; 2d undistort [0, 0]<br>
  2d original [5143.32, 6497.12]; 2d undistort [0, 0]<br>
  2d original [771.497, 611.726]; 2d undistort [0, 0]  </p>
  
  <p>The reason why I tried undistort() is because if one undoes the effect of known intrinsic parameters PnP becomes just a minimum direction problem of the form Ax=0. It needs min. 6 points for an approximate linear solution which is probably further improved with LMA (flags=CV_ITERATIVE). Technically there are only 6DOF and thus 3 points required so other methods (flags=CV_P3P, CV_EPNP) take less points. Anyways, regardless of a method or number of points the result is still invalid with non-zero distortion coefficients. The last thing I will try is to put all points on a 3D plane. It still fails:</p>
  
  <pre><code> for (int i=0; i&lt;npoints; i++)
          objPts[i].z=0.0f;
  </code></pre>
  
  <p>Finding extrinsic parameters (PnP)<br>
  Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30]<br>
  distrotion coeff: [1.2, 0.2, 0, 0, 0]<br>
  ============= Running PnP...<br>
  Rotation: [-1830.321574903016; 2542.206083947917; 2532.255948350521]<br>
  Translation [1407.918216894239; 1391.373407846455; 556.7108606094299]  </p>