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

Question

我正在尝试编写一个函数，当给定两个摄像机时，它们的旋转，平移矩阵，焦点以及每个摄像机的点坐标将能够将点三角测量为3D空间。基本上，给定所需的所有外在/内在值

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

Answer 1

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

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

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

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

起始公式

一旦您在现实世界中选择了两个相机相同的原点，您将有两个具有相同 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

Answer 2

假设您有两个摄像头 - 相机 1 和相机 2。

对于每个相机 j = 1， 2 你得到：

1. 中心Oj之间的距离hj("焦点"是正确的术语吗？基本上是相机看屏幕的点Oj)和相机的屏幕。相机的坐标系以Oj为中心，Oj--->x轴和Oj--->y轴平行于屏幕，而Oj--->z轴垂直于屏幕。

3 x 3

2. 旋转矩阵Uj和 3 x 1 平移向量Tj将相对于相机 j 系统的笛卡尔 3D 坐标(参见第 1 点)转换为世界坐标，即相对于描述 3D 世界中所有点的第三个坐标系的坐标。

3. 在相机j的屏幕上，这是平行于平面Oj-x-y的平面，距离原点Ojhj，你有点pj的2D坐标(假设只有x，y坐标)，其中p1和p2的两个点实际上是同一点的投影图像P， 在 3D 中的某个地方，分别在相机 1 和 2 的屏幕上。投影是通过在点Oj和点P之间绘制3D线并将点pj定义为该线与相机j的屏幕的唯一交点而获得的。相机j的3D坐标系中屏幕的方程是z = hj，所以点pj相对于相机j的3D坐标系的坐标看起来像pj = (xj, yj, hj)，所以2D屏幕坐标是简单的pj = (xj, yj)。

输入：给定 2D 点p1 = (x1, y1), p2 = (x2, y2)、twp 相机的焦距h1, h2、两个 3 x 3 旋转矩阵U1和U2、两个平移 3 x 1 矢量列T1和T2。

输出：世界坐标系中点 P 的坐标P = (x0, y0, z0)。

一种更简单的方法是以下算法，避免齐次坐标和投影矩阵(这也很好，或多或少是等效的)：

1. 形式Q1 = [x1; y1; h1]和Q2 = [x2; y2; h2]，其中它们被解释为 3 x 1 向量列;

2. 变换P1 = U1*Q1 + T1和P2 = U1*Q2 + T1，其中*是矩阵乘法，这里是 3 x 3 矩阵乘以 3 x 1 列，得到 3 x 1 列;

3. 形成X = T1 + t1*(P1 - T1)和X = T2 + t2*(P2 - T2)线;

前面步骤 3 中的两条线要么在一个公共点相交，

4. 即P点，要么它们是斜线，即它们不相交但不平行(不是共面)。

5. 如果线是斜线，则在第一行上找到唯一点X1，在第二行上找到X2的 uniqe 点，以使向量X2 - X1垂直于两条线，即X2 - X1垂直于向量P1 - T1和P2 - T2。这两个点 X1 和 X2 是两条线上最近的点。然后可以将第P = (X1 + X2)/2点作为段X1 X2的中点。

一般来说，两条线应该彼此非常接近，所以两点 X1 和 X2 应该彼此非常接近。