图像坐标到世界坐标的转换

文章目录

从图像坐标精确计算世界坐标的过程
[系数矩阵 A A A 不可逆的情况分析](#系数矩阵 A A A 不可逆的情况分析)
- [**何时会出现矩阵 A A A 不可逆的情况？**](#何时会出现矩阵 A A A 不可逆的情况？)
- - [**1. 旋转矩阵的列向量线性相关**](#1. 旋转矩阵的列向量线性相关)
  - [**2. 图像点的位置导致矩阵列线性相关**](#2. 图像点的位置导致矩阵列线性相关)
  - [**3. 相机姿态导致矩阵奇异**](#3. 相机姿态导致矩阵奇异)
  - [**4. 相机位于待测平面上方的特殊位置**](#4. 相机位于待测平面上方的特殊位置)
- [**如何判断矩阵 A A A 是否可逆？**](#如何判断矩阵 A A A 是否可逆？)
- [**如何避免矩阵 A A A 不可逆的情况？**](#如何避免矩阵 A A A 不可逆的情况？)
- - [**1. 选择适当的相机姿态**](#1. 选择适当的相机姿态)
  - [**2. 避免特殊的图像点**](#2. 避免特殊的图像点)
  - [**3. 数值稳定性处理**](#3. 数值稳定性处理)
- 实例

从图像坐标精确计算世界坐标的过程

在计算机视觉中，从图像坐标反求世界坐标是一个重要且常见的任务。为了准确地完成这一转换，需要正确地考虑相机的内参和外参，以及图像点的坐标，一般情况下从二维空间恢复三维空间的信息是不可能的，但是对于一些有先验信息的场景，比如知道物体是在地面上的，这样的情况是可以恢复三维情况。

已知条件

相机内参矩阵 K K K：

K = [ f x 0 c x 0 f y c y 0 0 1 ] K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1
相机外参（旋转矩阵 R R R 和平移向量 t t t）：
$R ∣ t \] = \[ r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 \] \[R\|t\] = \\begin{bmatrix} r_{11} \& r_{12} \& r_{13} \& t_x \\\\ r_{21} \& r_{22} \& r_{23} \& t_y \\\\ r_{31} \& r_{32} \& r_{33} \& t_z \\\\ 0 \& 0 \& 0 \& 1 \\end{bmatrix} \[R∣t\]= r11r21r310r12r22r320r13r23r330txtytz1$
假设物体位于地面，即世界坐标系中的 Z w = 0 Z_w = 0 Zw=0

计算步骤

第一步：将图像坐标转换为归一化相机坐标

使用相机内参，将图像坐标 ( u , v ) (u, v) (u,v) 转换为归一化的相机坐标 ( x c , y c ) (x_c, y_c) (xc,yc)：

x c = u − c x f x y c = v − c y f y \begin{align*} x_c &= \frac{u - c_x}{f_x} \\ y_c &= \frac{v - c_y}{f_y} \end{align*} xcyc=fxu−cx=fyv−cy

第二步：建立相机坐标与世界坐标的关系

相机坐标系下的三维点与世界坐标系下的三维点之间的关系为：

X c Y c Z c \] = R \[ X w Y w Z w \] + t \\begin{bmatrix} X_c \\\\ Y_c \\\\ Z_c \\end{bmatrix} = R \\begin{bmatrix} X_w \\\\ Y_w \\\\ Z_w \\end{bmatrix} + t XcYcZc =R XwYwZw +t 由于 Z w = 0 Z_w = 0 Zw=0，方程简化为： \[ X c Y c Z c \] = R \[ X w Y w 0 \] + t \\begin{bmatrix} X_c \\\\ Y_c \\\\ Z_c \\end{bmatrix} = R \\begin{bmatrix} X_w \\\\ Y_w \\\\ 0 \\end{bmatrix} + t XcYcZc =R XwYw0 +t 展开得到： { X c = r 11 X w + r 12 Y w + t x Y c = r 21 X w + r 22 Y w + t y Z c = r 31 X w + r 32 Y w + t z \\begin{cases} X_c = r_{11} X_w + r_{12} Y_w + t_x \\\\ Y_c = r_{21} X_w + r_{22} Y_w + t_y \\\\ Z_c = r_{31} X_w + r_{32} Y_w + t_z \\end{cases} ⎩ ⎨ ⎧Xc=r11Xw+r12Yw+txYc=r21Xw+r22Yw+tyZc=r31Xw+r32Yw+tz #### 第三步：利用透视投影关系 根据透视投影模型，归一化相机坐标与相机坐标的关系为： { x c = X c Z c y c = Y c Z c \\begin{cases} x_c = \\dfrac{X_c}{Z_c} \\\\ y_c = \\dfrac{Y_c}{Z_c} \\end{cases} ⎩ ⎨ ⎧xc=ZcXcyc=ZcYc 因此，可以表示为： { X c = x c Z c Y c = y c Z c \\begin{cases} X_c = x_c Z_c \\\\ Y_c = y_c Z_c \\end{cases} {Xc=xcZcYc=ycZc #### 第四步：将相机坐标代入方程 将 X c X_c Xc 和 Y c Y_c Yc 代入第二步的方程： { x c Z c = r 11 X w + r 12 Y w + t x y c Z c = r 21 X w + r 22 Y w + t y Z c = r 31 X w + r 32 Y w + t z \\begin{cases} x_c Z_c = r_{11} X_w + r_{12} Y_w + t_x \\\\ y_c Z_c = r_{21} X_w + r_{22} Y_w + t_y \\\\ Z_c = r_{31} X_w + r_{32} Y_w + t_z \\end{cases} ⎩ ⎨ ⎧xcZc=r11Xw+r12Yw+txycZc=r21Xw+r22Yw+tyZc=r31Xw+r32Yw+tz #### 第五步：构建线性方程组 将上述方程整理，构成关于 X w X_w Xw、 Y w Y_w Yw 和 Z c Z_c Zc 的线性方程组： \[ r 11 r 12 − x c r 21 r 22 − y c r 31 r 32 − 1 \] \[ X w Y w Z c \] = − \[ t x t y t z \] \\begin{bmatrix} r_{11} \& r_{12} \& -x_c \\\\ r_{21} \& r_{22} \& -y_c \\\\ r_{31} \& r_{32} \& -1 \\end{bmatrix} \\begin{bmatrix} X_w \\\\ Y_w \\\\ Z_c \\end{bmatrix} = -\\begin{bmatrix} t_x \\\\ t_y \\\\ t_z \\end{bmatrix} r11r21r31r12r22r32−xc−yc−1 XwYwZc =− txtytz 记作： A ⋅ X = b A \\cdot \\mathbf{X} = \\mathbf{b} A⋅X=b 其中： * A A A 为系数矩阵 * X = \[ X w Y w Z c \] \\mathbf{X} = \\begin{bmatrix} X_w \\\\ Y_w \\\\ Z_c \\end{bmatrix} X= XwYwZc * b = − \[ t x t y t z \] \\mathbf{b} = -\\begin{bmatrix} t_x \\\\ t_y \\\\ t_z \\end{bmatrix} b=− txtytz #### 第六步：求解方程组 只要矩阵 A A A 可逆（即其行列式不为零），即可求解： X = A − 1 ⋅ b \\mathbf{X} = A\^{-1} \\cdot \\mathbf{b} X=A−1⋅b 解得 X w X_w Xw、 Y w Y_w Yw 和 Z c Z_c Zc。 #### 第七步：得到世界坐标 已知 Z w = 0 Z_w = 0 Zw=0，因此世界坐标为： ( X w , Y w , Z w = 0 ) (X_w, Y_w, Z_w = 0) (Xw,Yw,Zw=0) ### 注意事项 * **矩阵可逆性** ：确保矩阵 A A A 可逆。如果不可逆，可能需要调整相机姿态或算法。 * **单位一致性**：所有参数（焦距、坐标、位置）应使用相同的单位（如米或毫米）。 * **数值稳定性**：在求解线性方程组时，注意数值计算的稳定性，必要时可使用迭代法或正则化方法。 ## 系数矩阵 A A A 不可逆的情况分析 在从图像坐标反求世界坐标的过程中，我们需要求解线性方程组 A X = b A\\mathbf{X} = \\mathbf{b} AX=b，其中： A = \[ r 11 r 12 − x c r 21 r 22 − y c r 31 r 32 − 1 \] , X = \[ X w Y w Z c \] , b = − \[ t x t y t z \] A = \\begin{bmatrix} r_{11} \& r_{12} \& -x_c \\\\ r_{21} \& r_{22} \& -y_c \\\\ r_{31} \& r_{32} \& -1 \\end{bmatrix}, \\quad \\mathbf{X} = \\begin{bmatrix} X_w \\\\ Y_w \\\\ Z_c \\end{bmatrix}, \\quad \\mathbf{b} = -\\begin{bmatrix} t_x \\\\ t_y \\\\ t_z \\end{bmatrix} A= r11r21r31r12r22r32−xc−yc−1 ,X= XwYwZc ,b=− txtytz 矩阵 A A A 的可逆性（是否存在逆矩阵）取决于其行列式是否为零，即当 det ⁡ ( A ) = 0 \\det(A) = 0 det(A)=0 时，矩阵 A A A 不可逆。 ### **何时会出现矩阵 A A A 不可逆的情况？** 矩阵 A A A 不可逆的情况主要发生在以下几种情形： #### **1. 旋转矩阵的列向量线性相关** 如果旋转矩阵 R R R 的第一列和第二列线性相关，即存在非零常数 k k k，使得： \[ r 11 r 21 r 31 \] = k \[ r 12 r 22 r 32 \] \\begin{bmatrix} r_{11} \\\\ r_{21} \\\\ r_{31} \\end{bmatrix} = k \\begin{bmatrix} r_{12} \\\\ r_{22} \\\\ r_{32} \\end{bmatrix} r11r21r31 =k r12r22r32 这意味着 R R R 的前两列向量不是线性独立的，导致矩阵 A A A 的前两列线性相关，降低了矩阵的秩，使其不可逆。在实际中，这种情况通常不会发生，因为旋转矩阵 R R R 应该是正交矩阵，列向量正交且单位化。 #### **2. 图像点的位置导致矩阵列线性相关** 当图像坐标 ( x c , y c ) (x_c, y_c) (xc,yc) 满足某种特殊条件，使得矩阵 A A A 的第三列与前两列线性相关。例如，当存在常数 λ \\lambda λ 和 μ \\mu μ，使得： \[ − x c − y c − 1 \] = λ \[ r 11 r 21 r 31 \] + μ \[ r 12 r 22 r 32 \] \\begin{bmatrix} -x_c \\\\ -y_c \\\\ -1 \\end{bmatrix} = \\lambda \\begin{bmatrix} r_{11} \\\\ r_{21} \\\\ r_{31} \\end{bmatrix} + \\mu \\begin{bmatrix} r_{12} \\\\ r_{22} \\\\ r_{32} \\end{bmatrix} −xc−yc−1 =λ r11r21r31 +μ r12r22r32 这种情况下，矩阵 A A A 的列向量线性相关，导致矩阵不可逆。这可能发生在图像点位于特定位置，例如光轴方向，或者相机姿态与世界坐标系存在特殊对称关系时。 #### **3. 相机姿态导致矩阵奇异** 当相机的姿态（由旋转矩阵 R R R 和平移向量 t t t 描述）使得矩阵 A A A 的行列式为零。例如，当相机仅绕某个轴旋转 9 0 ∘ 90\^\\circ 90∘ 或 18 0 ∘ 180\^\\circ 180∘ 时，旋转矩阵的元素会导致矩阵 A A A 的行或列出现零元素或线性相关，进而导致矩阵不可逆。 #### **4. 相机位于待测平面上方的特殊位置** 如果相机的位置 t t t 与待测平面（例如 Z w = 0 Z_w = 0 Zw=0 的地面）存在特殊关系，使得相机的投影出现退化。例如，当相机高度 t z = 0 t_z = 0 tz=0 时，可能导致无法确定深度信息，矩阵 A A A 不可逆。 ### **如何判断矩阵 A A A 是否可逆？** 要判断矩阵 A A A 是否可逆，需要计算其行列式 det ⁡ ( A ) \\det(A) det(A)： * **若 det ⁡ ( A ) ≠ 0 \\det(A) \\ne 0 det(A)=0** ，则矩阵 A A A 可逆，方程组有唯一解。 * **若 det ⁡ ( A ) = 0 \\det(A) = 0 det(A)=0** ，则矩阵 A A A 不可逆，方程组可能有无穷多解或无解。 在实际计算中，可使用以下方法： 1. **计算行列式** ：直接计算 det ⁡ ( A ) \\det(A) det(A)，若结果为零，则矩阵不可逆。 2. **检查矩阵的秩** ：计算矩阵 A A A 的秩，若小于 3（矩阵大小为 3 × 3 3 \\times 3 3×3），则矩阵不可逆。 3. **使用数值方法**：在编程实现中，尝试求解线性方程组时，如果求解算法（例如高斯消元法）出现无法进行下去的情况，可能提示矩阵不可逆。 ### **如何避免矩阵 A A A 不可逆的情况？** 为了确保矩阵 A A A 可逆，从而成功求解世界坐标，可以采取以下措施： #### **1. 选择适当的相机姿态** * **避免特殊角度** ：避免相机仅绕某个轴旋转 9 0 ∘ 90\^\\circ 90∘ 或 18 0 ∘ 180\^\\circ 180∘ 的姿态。 * **确保旋转矩阵正交** ：校准相机时，保证旋转矩阵 R R R 的正交性和列向量线性无关。 #### **2. 避免特殊的图像点** * **选择一般位置的图像点** ：避免选择可能导致矩阵 A A A 列向量线性相关的图像点，例如图像中心或特殊对称点。 * **多选取一些点**：使用多个图像点构建更大的方程组，利用最小二乘法求解，可以缓解单个点导致的退化情况。 #### **3. 数值稳定性处理** * **添加正则化项**：在求解过程中，添加微小的正则化项，防止数值计算中的奇异性。 * **检查计算结果**：在计算过程中，监控矩阵的条件数，若条件数过大，提示矩阵接近奇异，需要调整数据或算法。 ### 实例 点击图像中一点，把像素坐标转化为世界坐标在3维空间中绘制出来。从视频中可以看出还原到三维空间的点和实际情况基本吻合。 [相机位姿参考](https://blog.csdn.net/qq_40622955/article/details/143209281) camera_to_world