title: OpenCV unproject 2D points to 3D with known depth `Z`
body: <h1>Problem statement</h1>
  
  <p>I am trying to reproject 2D points to their original 3D coordinates, assuming I know the distance at which each point is. Following the <a href="https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#projectpoints" rel="noreferrer">OpenCV documentation</a>, I managed to get it to work with zero-distortions. However, when there are distortions, the result is not correct.</p>
  
  <h1>Current approach</h1>
  
  <p>So, the idea is to reverse the following:</p>
  
  <p><a href="https://docs.opencv.org/2.4/_images/math/331ebcd980b851f25de1979ebb67a2fed1c8477e.png" rel="noreferrer"><img src="https://docs.opencv.org/2.4/_images/math/331ebcd980b851f25de1979ebb67a2fed1c8477e.png" alt="Distorted projection"></a></p>
  
  <p>into the following:</p>
  
  <p><a href="https://i.stack.imgur.com/CcH2O.png" rel="noreferrer"><img src="https://i.stack.imgur.com/CcH2O.png" alt="enter image description here"></a></p>
  
  <p>By:</p>
  
  <ol>
  <li>Geting rid of any distortions using <code>cv::undistortPoints</code></li>
  <li>Use intrinsics to get back to the normalized camera coordinates by reversing the second equation above</li>
  <li>Multiplying by <code>z</code> to reverse the normalization.</li>
  </ol>
  
  <h1>Questions</h1>
  
  <ol>
  <li><s>Why do I need to subtract <code>f_x</code> and <code>f_y</code> to get back to the normalized camera coordinates (found empirically when testing)? In the code below, in step 2, if I don't subtract -- even the non-distorted result is off</s> This was my mistake -- I messed up the indexes.</li>
  <li>If I include the distortion, the result is wrong -- what am I doing wrong?</li>
  </ol>
  
  <h1>Sample code (C++)</h1>
  
  <pre><code>#include &lt;iostream&gt;
  #include &lt;opencv2/calib3d/calib3d.hpp&gt;
  #include &lt;opencv2/core/core.hpp&gt;
  #include &lt;opencv2/imgproc/imgproc.hpp&gt;
  #include &lt;vector&gt;
  
  std::vector&lt;cv::Point2d&gt; Project(const std::vector&lt;cv::Point3d&gt;&amp; points,
                                   const cv::Mat&amp; intrinsic,
                                   const cv::Mat&amp; distortion) {
    std::vector&lt;cv::Point2d&gt; result;
    if (!points.empty()) {
      cv::projectPoints(points, cv::Mat(3, 1, CV_64F, cvScalar(0.)),
                        cv::Mat(3, 1, CV_64F, cvScalar(0.)), intrinsic,
                        distortion, result);
    }
    return result;
  }
  
  std::vector&lt;cv::Point3d&gt; Unproject(const std::vector&lt;cv::Point2d&gt;&amp; points,
                                     const std::vector&lt;double&gt;&amp; Z,
                                     const cv::Mat&amp; intrinsic,
                                     const cv::Mat&amp; distortion) {
    double f_x = intrinsic.at&lt;double&gt;(0, 0);
    double f_y = intrinsic.at&lt;double&gt;(1, 1);
    double c_x = intrinsic.at&lt;double&gt;(0, 2);
    double c_y = intrinsic.at&lt;double&gt;(1, 2);
    // This was an error before:
    // double c_x = intrinsic.at&lt;double&gt;(0, 3);
    // double c_y = intrinsic.at&lt;double&gt;(1, 3);
  
    // Step 1. Undistort
    std::vector&lt;cv::Point2d&gt; points_undistorted;
    assert(Z.size() == 1 || Z.size() == points.size());
    if (!points.empty()) {
      cv::undistortPoints(points, points_undistorted, intrinsic,
                          distortion, cv::noArray(), intrinsic);
    }
  
    // Step 2. Reproject
    std::vector&lt;cv::Point3d&gt; result;
    result.reserve(points.size());
    for (size_t idx = 0; idx &lt; points_undistorted.size(); ++idx) {
      const double z = Z.size() == 1 ? Z[0] : Z[idx];
      result.push_back(
          cv::Point3d((points_undistorted[idx].x - c_x) / f_x * z,
                      (points_undistorted[idx].y - c_y) / f_y * z, z));
    }
    return result;
  }
  
  int main() {
    const double f_x = 1000.0;
    const double f_y = 1000.0;
    const double c_x = 1000.0;
    const double c_y = 1000.0;
    const cv::Mat intrinsic =
        (cv::Mat_&lt;double&gt;(3, 3) &lt;&lt; f_x, 0.0, c_x, 0.0, f_y, c_y, 0.0, 0.0, 1.0);
    const cv::Mat distortion =
        // (cv::Mat_&lt;double&gt;(5, 1) &lt;&lt; 0.0, 0.0, 0.0, 0.0);  // This works!
        (cv::Mat_&lt;double&gt;(5, 1) &lt;&lt; -0.32, 1.24, 0.0013, 0.0013);  // This doesn't!
  
    // Single point test.
    const cv::Point3d point_single(-10.0, 2.0, 12.0);
    const cv::Point2d point_single_projected = Project({point_single}, intrinsic,
                                                       distortion)[0];
    const cv::Point3d point_single_unprojected = Unproject({point_single_projected},
                                      {point_single.z}, intrinsic, distortion)[0];
  
    std::cout &lt;&lt; "Expected Point: " &lt;&lt; point_single.x;
    std::cout &lt;&lt; " " &lt;&lt; point_single.y;
    std::cout &lt;&lt; " " &lt;&lt; point_single.z &lt;&lt; std::endl;
    std::cout &lt;&lt; "Computed Point: " &lt;&lt; point_single_unprojected.x;
    std::cout &lt;&lt; " " &lt;&lt; point_single_unprojected.y;
    std::cout &lt;&lt; " " &lt;&lt; point_single_unprojected.z &lt;&lt; std::endl;
  }
  </code></pre>
  
  <h1>Same Code (Python)</h1>
  
  <pre><code>import cv2
  import numpy as np
  
  def Project(points, intrinsic, distortion):
    result = []
    rvec = tvec = np.array([0.0, 0.0, 0.0])
    if len(points) &gt; 0:
      result, _ = cv2.projectPoints(points, rvec, tvec,
                                    intrinsic, distortion)
    return np.squeeze(result, axis=1)
  
  def Unproject(points, Z, intrinsic, distortion):
    f_x = intrinsic[0, 0]
    f_y = intrinsic[1, 1]
    c_x = intrinsic[0, 2]
    c_y = intrinsic[1, 2]
    # This was an error before
    # c_x = intrinsic[0, 3]
    # c_y = intrinsic[1, 3]
  
    # Step 1. Undistort.
    points_undistorted = np.array([])
    if len(points) &gt; 0:
      points_undistorted = cv2.undistortPoints(np.expand_dims(points, axis=1), intrinsic, distortion, P=intrinsic)
    points_undistorted = np.squeeze(points_undistorted, axis=1)
  
    # Step 2. Reproject.
    result = []
    for idx in range(points_undistorted.shape[0]):
      z = Z[0] if len(Z) == 1 else Z[idx]
      x = (points_undistorted[idx, 0] - c_x) / f_x * z
      y = (points_undistorted[idx, 1] - c_y) / f_y * z
      result.append([x, y, z])
    return result
  
  f_x = 1000.
  f_y = 1000.
  c_x = 1000.
  c_y = 1000.
  
  intrinsic = np.array([
    [f_x, 0.0, c_x],
    [0.0, f_y, c_y],
    [0.0, 0.0, 1.0]
  ])
  
  distortion = np.array([0.0, 0.0, 0.0, 0.0])  # This works!
  distortion = np.array([-0.32, 1.24, 0.0013, 0.0013])  # This doesn't!
  
  point_single = np.array([[-10.0, 2.0, 12.0],])
  point_single_projected = Project(point_single, intrinsic, distortion)
  Z = np.array([point[2] for point in point_single])
  point_single_unprojected = Unproject(point_single_projected,
                                       Z,
                                       intrinsic, distortion)
  print "Expected point:", point_single[0]
  print "Computed point:", point_single_unprojected[0]
  </code></pre>
  
  <p>The results for zero-distortion (as mentioned) are correct:</p>
  
  <pre><code>Expected Point: -10 2 12
  Computed Point: -10 2 12
  </code></pre>
  
  <p>But when the distortions are included, the result is off:</p>
  
  <pre><code>Expected Point: -10 2 12
  Computed Point: -4.26634 0.848872 12
  </code></pre>
  
  <h2>Update 1. Clarification</h2>
  
  <p>This is a camera to image projection -- I am assuming the 3D points are in the camera-frame coordinates.</p>
  
  <h2>Update 2. Figured out the first question</h2>
  
  <p>OK, I figure out the subtraction of the <code>f_x</code> and <code>f_y</code> -- I was stupid enough to mess up the indexes. Updated the code to correct. The other question still holds.</p>
  
  <h2>Update 3. Added Python equivalent code</h2>
  
  <p>To increase visibility, adding the Python codes, because it has the same error.</p>