python - 如何在 3D 空间中对一个点进行三角测量，给定 2 个图像中的坐标点和相机的外部值

Question

我正在尝试编写一个函数，当给定两个相机、它们的旋转、平移矩阵、焦点和每个相机的一个点的坐标时，将能够将该点三角化到 3D 空间中。基本上，给定所有需要的外在/内在值(value)

我熟悉一般的想法:以某种方式创建两条射线并找到满足最小二乘问题的最近点，但是，我不知道如何将给定的信息转换为一系列方程式3D坐标点。

Answer 1

我的旅程晚了几年。我遇到了完全相同的问题，发现有几个人在问同样的问题，但从来没有找到一个足以让我理解的简化答案，所以我花了几天时间学习这些东西，这样我就可以将它简化为基本要素并发布我发现的内容这里是为了 future 的人。

最后我还会给你一些代码示例，让你在 python 中做你想做的事，所以坚持下去。

我手写笔记的一些屏幕截图，解释了整个过程。 Page 1. Page 2. Page 3.

这是我开始的方程式，可以在 https://docs.opencv.org/master/d9/d0c/group__calib3d.html 中找到

Starting formula

一旦您在现实世界中为两个相机选择了相同的原点，您将得到其中两个具有相同 X、Y、Z 值的等式。

抱歉，您已经有了下一部分，但其他人可能还没有做到这一点:

首先，您需要校准相机，这将为您提供每个相机的相机矩阵和畸变(固有属性)。 https://docs.opencv.org/master/dc/dbb/tutorial_py_calibration.html

你只需要这两个并且可以转储 rvecs 和 tvecs 因为这会在你设置相机时改变。

一旦选择了真实世界的坐标系，就可以使用 cv2.solvePnP 获取旋转和平移向量。为此，您需要一组真实世界点及其在每个相机的相机中的对应坐标。我的诀窍是我编写了一些代码来显示该字段的图像，然后我将传递点数。然后我会点击图像上的位置并创建一个映射。这部分的代码有点长，除非有人要求，否则我不会在这里分享。

cv2.solvePnP 将为您提供旋转矩阵的向量，因此您需要使用以下行将其转换为 3x3 矩阵:

`R, jac = cv2.Rodrigues(rvec)`

现在回到最初的问题:每个摄像头都有 3x3 摄像头矩阵。每个相机都有 3x3 旋转矩阵。每个相机都有 3x1 的平移向量。您有一些 (u, v) 坐标表示感兴趣的对象在每个相机中的位置。数学在笔记的图像中得到了更多解释。

import numpy as np

def get_xyz(camera1_coords, camera1_M, camera1_R, camera1_T, camera2_coords, camera2_M, camera2_R, camera2_T):
    # Get the two key equations from camera1
    camera1_u, camera1_v = camera1_coords
    # Put the rotation and translation side by side and then multiply with camera matrix
    camera1_P = camera1_M.dot(np.column_stack((camera1_R,camera1_T)))
    # Get the two linearly independent equation referenced in the notes
    camera1_vect1 = camera1_v*camera1_P[2,:]-camera1_P[1,:]
    camera1_vect2 = camera1_P[0,:] - camera1_u*camera1_P[2,:]
    
    # Get the two key equations from camera2
    camera2_u, camera2_v = camera2_coords
    # Put the rotation and translation side by side and then multiply with camera matrix
    camera2_P = camera2_M.dot(np.column_stack((camera2_R,camera2_T)))
    # Get the two linearly independent equation referenced in the notes
    camera2_vect1 = camera2_v*camera2_P[2,:]-camera2_P[1,:]
    camera2_vect2 = camera2_P[0,:] - camera2_u*camera2_P[2,:]
    
    # Stack the 4 rows to create one 4x3 matrix
    full_matrix = np.row_stack((camera1_vect1, camera1_vect2, camera2_vect1, camera2_vect2))
    # The first three columns make up A and the last column is b
    A = full_matrix[:, :3]
    b = full_matrix[:, 3].reshape((4, 1))
    # Solve overdetermined system. Note b in the wikipedia article is -b here.
    # https://en.wikipedia.org/wiki/Overdetermined_system
    soln = np.linalg.inv(A.T.dot(A)).dot(A.T).dot(-b)
    return soln