How to perform 2d to 3d reconstruction considering camera calibration

Question

I am trying to reconstruct 3d positions of objects captured by a camera, having its position on a 2d plane and all camera calibration parameters. I am working with Python and OpenCV.

I already searched and tried multiple solutions, but I can't achieve the transformation that I want to. My main issue is that I have no enough background in graphics to understand and perform the exact set of needed steps.

<?xml version="1.0"?>
<opencv_storage>
<intrinsic type_id="opencv-matrix">
  <rows>3</rows>
  <cols>3</cols>
  <dt>f</dt>
  <data>
    4.04310596e+003 0. 9.15485046e+002
    0. 4.03170264e+003 4.26480865e+002
    0. 0. 1.</data></intrinsic>
<rotation_vector type_id="opencv-matrix">
  <rows>1</rows>
  <cols>3</cols>
  <dt>f</dt>
  <data>
    -4.56216574e-001 1.76409543e+000 2.05966163e+000</data></rotation_vector>
<rotation_matrix type_id="opencv-matrix">
  <rows>3</rows>
  <cols>3</cols>
  <dt>f</dt>
  <data>
    -8.71332586e-001 -4.90659207e-001 5.74691826e-003 8.10814202e-002
    -1.32417098e-001 9.87872243e-001 -4.83947605e-001 8.61231267e-001
    1.55162677e-001</data></rotation_matrix>
<translation type_id="opencv-matrix">
  <rows>1</rows>
  <cols>3</cols>
  <dt>f</dt>
  <data>
    3.16912168e+004 -1.31297791e+003 8.73433125e+004</data></translation>
<distortion type_id="opencv-matrix">
  <rows>1</rows>
  <cols>4</cols>
  <dt>f</dt>
  <data>
    4.86164242e-001 -3.57553625e+000 -1.77373271e-002 -3.11793620e-003</data></distortion>
<points_2d type_id="opencv-matrix">
  <rows>10</rows>
  <cols>1</cols>
  <dt>"2f"</dt>
  <data>
    1454. 223. 463. 375. 742. 461. 1163. 588. 1704. 755. 646. 550. 129.
    497. 567. 690. 196. 738. 546. 935.</data></points_2d>
<points_3d type_id="opencv-matrix">
  <rows>10</rows>
  <cols>3</cols>
  <dt>f</dt>
  <data>
    0. 34000. 0. 36000. 20160. 0. 36000. 7.31248877e+003 0. 36000.
    -7.31248877e+003 0. 36000. -20160. 0. 41500. 0. 0. 47000. 9160. 0.
    47000. -9160. 0. 52500. -9160. 0. 52500. -20160. 0.</data></points_3d>
<reprojection_errors type_id="opencv-matrix">
  <rows>1</rows>
  <cols>20</cols>
  <dt>f</dt>
  <data>
    19. -2. -9. -2. 0. 1. -1. -1. 3. 1. 0. 1. -19. 0. -8. 0. -4. 2. 9.
    1.</data></reprojection_errors>
</opencv_storage>

This is what I have, 2d and 3d points as example and all camera calibration parameters: intrinsic, distortion, and so on.

What sequence of operation should I need to perform the 2d to 3d transformation? Looking at the data, I want to convert (1454.0, 223.0) to (0.0, 34000.0, 0.0) and so on.

Answer 1

In the second part of this question you can find some math to solve your problem, and a c++ implementation of the solution.

Anyway I have implemented a similar solution in Python that is like this:

matrices = [
    "intrinsic",
    "rotation_vector",
    "rotation_matrix",
    "translation",
    "distortion",
    "points_2d",
    "points_3d",
    "reprojection_errors"
]

# Load data from persistent storage
dic = {}
data = cv2.FileStorage(storage_file, cv2.FILE_STORAGE_READ)
for m in matrices:
    dic[m] = data.getNode(m).mat()

# Prepare matrices
rotation_matrix = np.mat(dic["rotation_matrix"])
translation_vector = np.mat(dic["translation"])
intrinsic_matrix = np.mat(dic["intrinsic"])

# Extrinsic Parameters Matrix
translation_vector_transposed = np.transpose(translation_vector)
extrinsic_matrix = np.concatenate((rotation_matrix, translation_vector_transposed), axis=1)

# Projection Matrix
projection_matrix = intrinsic_matrix * extrinsic_matrix

# Homography Matrix
p11 = projection_matrix[0,0]
p12 = projection_matrix[0,1]
p14 = projection_matrix[0,3]
p21 = projection_matrix[1,0]
p22 = projection_matrix[1,1]
p24 = projection_matrix[1,3]
p31 = projection_matrix[2,0]
p32 = projection_matrix[2,1]
p34 = projection_matrix[2,3]
homography_matrix = np.array([[p11,p12,p14], [p21,p22,p24], [p31,p32,p34]], dtype=np.float)
homography_matrix_inverse = inv(homography_matrix)

for i in range(0,10):

    # Prepare points
    np.set_printoptions(suppress=True)
    point_2D = np.append(np.array(dic["points_2d"][i]), np.array([[1]]), axis=1)
    print("\nPoint2D:", end=" ")
    print_point(point_2D)
    point_3d_expected = dic["points_3d"][i]
    print("\nPoint3D Exptected:", end=" ")
    print_point_simple(point_3d_expected)

    # Projection
    point_3D_w = np.mat(homography_matrix_inverse) * np.mat(np.transpose(point_2D))

    # Normalization
    point_3D = np.divide(point_3D_w,point_3D_w[2])
    point_3D[2] = 0

    # Show Result
    print("\nPoint3D:", end=" ")
    print_point(point_3D)
    print('')

Hope this will help you.