SLAM-对极几何

SLAM-对极几何

对极几何是计算机视觉和SLAM中的一个基本概念，它描述了同一场景的两个摄像机视图之间的几何关系。这种关系有助于建立立体图像中点之间的对应关系并恢复摄像机运动。

1. 设置对极约束

1.1 几何设置

考虑同一3D点\(P\)的两个摄像机视图。设：

• \(O_L\)和\(O_R\)分别为左右摄像机的光心
• \(P\)为空间中的3D点，在两个摄像机坐标系中都有坐标
• \(p_L\)和\(p_R\)分别为点\(P\)在左右摄像机图像平面上的投影

1.2 约束推导

对极约束源于三个点\(O_L\)、\(O_R\)和\(P\)的共面性。

由于这三个点共面，我们可以将约束表示为：

\[\vec{O_L P} \cdot (\vec{O_R O_L} \times \vec{O_R P}) = 0 \]

在齐次坐标中，对于投影矩阵，如果点\(P\)被两个摄像机观察，其投影矩阵分别为\(P_L = K_L[I | 0]\)和\(P_R = K_R[R | t]\)，那么对应图像点\(x_L\)和\(x_R\)满足对极约束：

\[x_R^T E x_L = 0 \]

其中：

• \(E\)是本质矩阵
• \(x_L\)和\(x_R\)是左右图像中对应点的齐次坐标

1.3 本质矩阵

本质矩阵\(E\)定义为：

\[E = [t]_\times R \]

其中：

• \(R\)是从左摄像机坐标系到右摄像机坐标系的旋转矩阵
• \(t\)是两个摄像机坐标系之间的平移向量
• \([t]_\times\)是平移向量的反对称矩阵：

\[[t]_\times = \begin{bmatrix} 0 & -t_z & t_y \\ t_z & 0 & -t_x \\ -t_y & t_x & 0 \end{bmatrix} \]

1.4 基础矩阵

当考虑摄像机内参时，我们得到基础矩阵：

\[F = K_R^{-T} E K_L^{-1} \]

对极约束变为：

\[x_R^T F x_L = 0 \]

2. 求解本质矩阵 - 八点法

2.1 基本公式

八点算法使用两幅图像中对应点集来估计本质矩阵。给定\(n\)对对应点\((x_i, x'_i)\)，我们可以形成如下线性方程组：

对于每对对应点，我们有：

\[x'^T E x = 0 \]

展开这个方程：

\[\begin{bmatrix} x' & y' & 1 \end{bmatrix} \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23} \\ e_{31} & e_{32} & e_{33} \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = 0 \]

这导致：

\[x'x e_{11} + x'y e_{12} + x' e_{13} + y'x e_{21} + y'y e_{22} + y' e_{23} + x e_{31} + y e_{32} + e_{33} = 0 \]

2.2 线性系统构建

我们可以将其重写为线性系统：

\[\begin{bmatrix} x'_1x_1 & x'_1y_1 & x'_1 & y'_1x_1 & y'_1y_1 & y'_1 & x_1 & y_1 & 1 \\ x'_2x_2 & x'_2y_2 & x'_2 & y'_2x_2 & y'_2y_2 & y'_2 & x_2 & y_2 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x'_nx_n & x'_ny_n & x'_n & y'_nx_n & y'_ny_n & y'_n & x_n & y_n & 1 \end{bmatrix} \begin{bmatrix} e_{11} \\ e_{12} \\ e_{13} \\ e_{21} \\ e_{22} \\ e_{23} \\ e_{31} \\ e_{32} \\ e_{33} \end{bmatrix} = \mathbf{0} \]

或紧凑形式：

\[A e = \mathbf{0} \]

其中\(e = [e_{11}, e_{12}, \ldots, e_{33}]^T\)是向量化本质矩阵。

2.3 求解过程

1. 坐标归一化：为了提高数值稳定性，对坐标进行归一化，使质心在原点且到原点的平均距离为\(\sqrt{2}\)。

2. 构建矩阵A：使用归一化坐标构建系数矩阵A。

3. 求解齐次系统：通过SVD（奇异值分解）得到对应于A最小奇异值的奇异向量即为解。

4. 强制奇异性约束：本质矩阵必须有两个相等的奇异值和一个零奇异值。这通过将最小奇异值设为零来实现。

5. 去归一化：将本质矩阵转换回原始坐标系。

3. 恢复运动 - SVD分解

3.1 本质矩阵的SVD分解

为了从本质矩阵E恢复旋转R和平移t，我们使用SVD：

\[E = U \Sigma V^T \]

其中：

• \(U\)和\(V\)是正交矩阵
• \(\Sigma = \mathrm{diag}(\sigma, \sigma, 0)\)，其中\(\sigma\)是重复的奇异值

3.2 可能的解

本质矩阵分解产生四种可能的旋转和平移解：

定义：

\[W = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \quad Z = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \]

那么四种可能的解为：

1. \(R = U W V^T, t = U_{:,3}\)
2. \(R = U W V^T, t = -U_{:,3}\)
3. \(R = U W^T V^T, t = U_{:,3}\)
4. \(R = U W^T V^T, t = -U_{:,3}\)

其中\(U_{:,3}\)是U的第三列。

3.3 确定正确解

为了在四种可能性中确定正确解：

1. 视差约束：3D点必须在两个摄像机前方。这消除了点会在一个或两个摄像机后方的两种解。

2. 三角化：对于每种剩余解，对几个对应点进行三角化并检查是否满足朝前约束。

3. 尺度模糊性：平移向量t只能确定到比例因子。在单目SLAM中，只有通过额外信息（如已知场景尺寸或IMU数据）才能解决这种尺度问题。

3.4 实现步骤

1. 计算本质矩阵的SVD：\(E = U \Sigma V^T\)
2. 确保\(U\)和\(V\)是旋转矩阵（行列式=1）
3. 生成四种可能解
4. 对于每种解，对一组对应点进行三角化
5. 计算在两个摄像机前方的点数
6. 选择具有最多朝前点的解