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

文章目录

从图像坐标精确计算世界坐标的过程
[系数矩阵 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
图像点坐标 ( u , v ) (u, v) (u,v)
假设物体位于地面，即世界坐标系中的 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 不可逆，方程组可能有无穷多解或无解。

在实际计算中，可使用以下方法：

计算行列式 ：直接计算 det ⁡ ( A ) \det(A) det(A)，若结果为零，则矩阵不可逆。
检查矩阵的秩 ：计算矩阵 A A A 的秩，若小于 3（矩阵大小为 3 × 3 3 \times 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维空间中绘制出来。从视频中可以看出还原到三维空间的点和实际情况基本吻合。

相机位姿参考

camera_to_world