原文：Yin的笔记本，非常推荐
图片部分来自知乎，大黑
本文大部分内容来自上方链接，在此基础上加入了一些自己的思考。非原创，仅作为个人笔记。

0. 主要内容

任务	场景数据	相机数据	输入	输出
Pose Estimation	已知	测得	相机视野内各特征点在空间中的位置	相机位姿
Triangulation	测得	已知	双目相机位姿和相机视野内特征点在图片上的位置	各特征点在空间中的位置
Epipolar Geometry	部分已知	测得	双目相机视野内特征点在图片上的位置	相机位姿和各特征点在空间中的位置
Reconstruction	测得	测得	相机视野内 2 D 图片	相机视野内各点在空间中的位置
首先是 3 D 视觉四个大任务简单的介绍，

姿态估计（Pose Estimation）：场景数据可以简单理解为特征点的三维坐标，例如标定物自身建立的 3 维坐标系下每个标定点的位置。相机数据可以理解为这些特征点在左右相机中的像素坐标。输入则需要我们进行特征点的对齐，即每个特征点的 3 D 坐标和左右视图像素坐标的匹配。输出则是相机的位姿，方法可以使用解 PnP 方法，例如 Opencv 中的 cv::solvePnP, cv::solvePnPRansac 等。应用场景有例如增强现实（AR）：根据现实世界的 3 D 点与相机图像匹配，估计相机位置；机器人导航：通过环境中的特征点估计相机（机器人）的运动轨迹。
三角定位（Triangulation）：根据双目相机中相同特征点在左右视图中的位置（像素坐标），结合相机的位姿，计算这些特征点的三维位置。已知信息有同一个 3 D 特征点投影到左右视图的像素坐标（假设已经确定了对应关系），和相机的内外参，求特征点的 3 D 坐标。方法就是一系列三角测量的方法，例如 cv::triangulatePoints。应用例如特征点定位等。
对极几何（Epipolar Geometry）：这个范围比较广，它描述了一类几何关系，后面会有详细讲解。借用维基百科的描述:

对极几何 是立体视觉中的一种几何关系。当两个摄像机从两个不同的位置观察 3 D 场景时，3 D 点及其在 2 D 图像上的投影之间存在许多几何关系，从而导致图像点之间的约束。这些关系是基于针孔相机模型的假设推导出来的。

三维重建（Reconstruction）：通过单目或多目相机拍摄的图像重建场景中点云或表面的三维结构。例如通过 SfM（Structure from Motion）恢复相机的运动轨迹和三维结构；Nerf， 3 D Gaussian 等三维重建方法，是现在研究的热门方向。

1. 三角定位 Triangulation

Triangulation 解决的是从两张图片中计算对应点的三维坐标的问题。

这个问题的假设是：

两张图片对应的相机内参和外参都是已知的
相机中对应点关系是已知的并且正确的

这样我们就可以从这两张图片中计算出指向该点方向的两条光线，进而求其交点就得这个点对应的三维坐标

理想情况的三角定位

我们首先研究简化的情况，这个情况的假设就是两个相机的内参矩阵是 完全一样 的，而且两个相机 在 x 方向是完全对齐的。

这样我们就可以有如下的模型。

其中 b 被称为 baseline，代表的是两个相机的光心对应的距离。图中分别以 $C_lP$ 和 $C_rP$ 为斜边可见左右两个直角三角形，其直角边可以写出如下的关系式：

误差

所以我们可以得出一下几个结论：

对于同一个 3 D 点，disparity 越大，误差越小
对于固定位置关系的两张图片，3 D 点离得越近误差越小

我们同时还可以总结一下 baseline 对他们的影响：

B 越大，triangulate 的误差越小
B 越大，最近可测量距离变大

那么我们就知道怎样来提高双目相机系统的精度了：

增大 baseline
增大焦距
尽量测量近距离的点

带误差的三角定位

理想情况的三角定位很好，但是在现实生活中，由于生产和装配误差，再好的双目相机，两个相机之间也不可能是完全对齐的，求出来的光线也不一定能精准相交。于是，我们需要在相没对齐光线也不能相交的情况下求解点的坐标：

直接上截图了：
Yin 的另一篇相机参数的讲解也非常推荐：相机参数

Tip

后续更新自己实现的 triangulation 方法，论文参考的是 Triangulation: Why Optimize？
先贴个图，容易忘

实现如下，逻辑有时间开帖单独聊

bool triangulateIDWMidpoint(const Eigen::Vector3d &x0,  
                            const Eigen::Vector3d &x1,  
                            const Eigen::Matrix3d &rotation,  
                            const Eigen::Vector3d &translation,  
                            Eigen::Ref<Eigen::Vector3d> result_point,  
                            double &error) {  
  Eigen::Vector3d rx0;  
  
  rx0 = rotation * x0;  
  
  const double p_norm = rx0.cross(x1).norm();  
  const double q_norm = rx0.cross(translation).norm();  
  const double r_norm = x1.cross(translation).norm();  
  
  const Eigen::Vector3d xprime1 =  
          (q_norm / (q_norm + r_norm)) *  
          (translation + (r_norm / p_norm) * (rx0 + x1));  
  
  result_point = rotation.transpose() * (xprime1 - translation);  
  
  const Eigen::Vector3d lambda0_rx0 = (r_norm / p_norm) * rx0;  
  const Eigen::Vector3d lambda1_x1  = (q_norm / p_norm) * x1;  
  
  error = (translation + lambda0_rx0 - lambda1_x1).norm();  
  
  /*  
  ** Eq. (9) - test adequacy  
  */  return (error * error) <  
         (std::min)((std::min)((translation + lambda0_rx0 + lambda1_x1)  
                                       .squaredNorm(),  
                               (translation - lambda0_rx0 - lambda1_x1)  
                                       .squaredNorm()),  
                    (translation - lambda0_rx0 + lambda1_x1).squaredNorm());  
}

2. 对极几何 Epipolar Geometry

刚才说的是知道了内参外参和点的对应关系之后，怎样进行三角定位。那往回推一步，怎样找点之间的对应关系呢？

如果直接在两张图里面找对应提取关键点进行匹配，只有在左右两边都找到的关键点才有可能匹配成功。如果对任意一点进行匹配的话，马上就会变成一个对左右两张图片所有像素的穷举搜索，复杂度为 $O(hwh'w')$ , 其中 h, w 和 h′, w′分别为左右两图片的长宽。

在已知相对位姿的双目相机中，能不能简化一些？能！现在有了相对的位姿约束，我们可以将这 个二维的搜索问题，降低到一维。

如图所示，两个相机之间的相对位姿，为我们提供了一个很好的先验知识：已知相机位姿的情况下，从左边相机中发出的指向某个特征点的射线在右边相机中的投影可以计算出来，所以直接在这条投影线上搜索特征点即可。

这个约束叫做对极约束（epipolar constraint）
$c_l, c_r, p$ 组成的平面成为极平面 (epipolar plane)
极平面与两个图像的交线成为极线 (epipolar line)
所有的极线的交点我们成为极点（epipole），也是 baseline 和像平面的交点

通过这个方法，我们就可以将二维搜索问题降低到一维。

对极约束

去掉相机内参，还原出对应点在左右两侧相机坐标系下的坐标 $x1,x2$ ：（ $x_1, x_2$ 是归一化归一化图像平面坐标， $x,x'$ 是像素坐标）

$$ \begin{aligned} \boldsymbol{x}_1 &= \boldsymbol{K}_1^{-1} \boldsymbol{x} = \boldsymbol{X} \\ \boldsymbol{x}_2 &= \boldsymbol{K}_2^{-1} \boldsymbol{x}' = \boldsymbol{R}\boldsymbol{X} + \boldsymbol{t} \end{aligned} $$

那么右侧相机的 $x_2$ 就可从左侧相机的 $x_1$ 经过变换得到：

$$ {{x}_2} = {R} {{x}_1} + {t} $$

注意 a, b 是两个向量，他们的点乘如下：

本质矩阵 essential matrix

基础矩阵 fundamental matrix

求基础矩阵：八点算法 8-point algorithm

关于本质矩阵和基础矩阵秩为 2，可以从以下几个角度来思考：
首先是反对陈矩阵 $[\mathbf{t}]_\times$ ，是一个 $3 \times 3$ 的矩阵，用于描述三维空间中的叉积操作。关于反对称矩阵，直接解特征值：

所以反对称矩阵的秩为 2，旋转矩阵 R 是 $3 \times 3$ 的满秩矩阵，所以 $[\mathbf{t}]_\times R$ 的秩仍然为 2（一个矩阵乘上满秩矩阵，秩不变），所以 E 的秩为 2
由于内参 K 可逆满秩，所以 F 的秩也为 2。也可以从奇异值分解和几何意义的角度去解释，详细的推理可以看经典文献

Hartley, R., & Zisserman, A. (2004). Multiple View Geometry in Computer Vision. Cambridge University Press.

这里作者没有打完，我们继续补全已知本质矩阵，求解相机外参 t 和 R 的过程：

代码实现可以研究 Opencv 的：

int cv::recoverPose(
    InputArray E,         // 本质矩阵
    InputArray points1,   // 第一幅图像中的点
    InputArray points2,   // 第二幅图像中的点
    InputArray cameraMatrix,  // 相机内参矩阵
    OutputArray R,        // 输出旋转矩阵
    OutputArray t,        // 输出平移向量
    OutputArray mask = noArray() // 有效点的掩码
);

实例：已知基础矩阵和左图特征点位置求右图特征点搜索空间

就是求右图极线

实例：已知基础矩阵求极点位置

设左相机的极点坐标为 e，而极点物理含义是左右相机中心连线 OO'与相机成像平面的交点，其显然在极线上，且其相当于右侧相机（即坐标原点）在左侧成像平面上的位置：

import numpy as np

def find_epipole(F):
    """
    使用 SVD 求解基础矩阵 F 的极点 e
    :param F: 基础矩阵 (3x3)
    :return: 极点 e 的齐次坐标
    """
    # 对 F 进行 SVD 分解
    U, S, Vt = np.linalg.svd(F)
    
    # 极点 e 是 V 的最后一列（V 是 Vt 的转置）
    e = Vt[-1]  # 最后一行的转置就是最后一列
    
    # 齐次归一化
    e = e / e[-1]
    
    return e

单应矩阵 Homography

除了基本矩阵和本质矩阵，我们还有一种称为单应矩阵 H 的东西，它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙，地面等），则可以通过单应性来进行运动估计。这种情况在无人机携带的俯视相机，或扫地机携带的顶视相机中比较常见。

其中 $n$ 是该平面的法向量 $n = [n_1, n_2, n_3]^{T}$ , $d$ 是偏移量。如果点 $X$ 在该平面上，则满足上述平面约束。

单应矩阵 H 在双目视觉中应用非常广泛，例如 cv2.warpPerspective 透视变换等。

3. 双目矫正 stereo rectification

这部分内容在工作中未用到，感兴趣可以看原作者blog

【转载】双目视觉模型和三角定位