【Triangulation】三角测量

1. 三角测量定义

在三维重建中，三角测量（Triangulation）是一种基本的算法，用于利用两个摄像头从两个不同视角拍摄得到的图片中的匹配点来还原一个三维点的位置。具体方法是通过解线性方程组，找到最佳的三维点位置，使得该点在两个摄像机视角下的投影和实际观察到的二维点尽可能匹配。

2. 公式推导

这个 TriangulatePoint 函数接受两个矩阵（cam1_from_world 和 cam2_from_world，它们代表两个摄像机相对于世界坐标系的变换矩阵），以及两个二维向量（point1 和 point2，它们表示在每个摄像机视角下观察到的点的坐标）。函数的目标是计算出全局坐标系中的三维点，该点在数学上应该映射到这两个二维点上。

为了进行三角化，我们利用摄像机投影矩阵的性质。一个点 $P$ 在世界坐标系中的坐标可以通过摄像机的投影矩阵 $C$ 转换为图像坐标系中的坐标 $p$ ，这一过程可以用下面的等式表示：

$$p=CP$$

为了解这个方程，我们可以把它拆分为两部分，一部分对应 $x$ 坐标，另一部分对应 $y$ 坐标。对于二维点与三维点之间的对应关系，我们有：

$$\begin{array}{rl}x& =(c_{11}X+c_{12}Y+c_{13}Z+c_{14})/(c_{31}X+c_{32}Y+c_{33}Z+c_{34})\\ y& =(c_{21}X+c_{22}Y+c_{23}Z+c_{24})/(c_{31}X+c_{32}Y+c_{33}Z+c_{34})\end{array}$$

为了消除分母，我们可以将每个式子乘以分母的项，得到两个线性方程：

$$\begin{array}{rl}x\cdot (c_{31}X+c_{32}Y+c_{33}Z+c_{34})& =c_{11}X+c_{12}Y+c_{13}Z+c_{14}\\ y\cdot (c_{31}X+c_{32}Y+c_{33}Z+c_{34})& =c_{21}X+c_{22}Y+c_{23}Z+c_{24}\end{array}$$

当有两个摄像机和两个对应点时，我们可以将上面的方程组写为矩阵 $A$ 的形式：

$$
Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& cam1_from_world,
const Eigen::Matrix3x4d& cam2_from_world,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2) {
Eigen::Matrix4d A;

A.row(0) = point1(0) * cam1_from_world.row(2) - cam1_from_world.row(0);
A.row(1) = point1(1) * cam1_from_world.row(2) - cam1_from_world.row(1);
A.row(2) = point2(0) * cam2_from_world.row(2) - cam2_from_world.row(0);
A.row(3) = point2(1) * cam2_from_world.row(2) - cam2_from_world.row(1);

Eigen::JacobiSVDEigen::Matrix4d svd(A, Eigen::ComputeFullV);

return svd.matrixV().col(3).hnormalized();
}
$$

3. 代码实现中：

A.row(0) = point1(0) * cam1_from_world.row(2) - cam1_from_world.row(0);
A.row(1) = point1(1) * cam1_from_world.row(2) - cam1_from_world.row(1);
A.row(2) = point2(0) * cam2_from_world.row(2) - cam2_from_world.row(0);
A.row(3) = point2(1) * cam2_from_world.row(2) - cam2_from_world.row(1);

这里的 point1(0) 和 point1(1) 对应于上面方程中的 $x$ 和 $y$，cam1_from_world.row(2) 是变换矩阵的第三行（我们称其为 $c_{3i}^{(1)}$），cam1_from_world.row(0) 和 cam1_from_world.row(1) 分别对应变换矩阵的第一行和第二行（我们称其为 $c_{1i}^{(1)}$ 和 $c_{2i}^{(1)}$）。对于第二个摄像机 cam2_from_world 也是同样的方法。

最后，通过计算 $A$ 的奇异值分解（SVD）并且取 $V$ 矩阵的最后一列，我们能够得到解空间的最小范数解。hnormalized() 函数是对坐标进行齐次归一化，从而得到三维空间中的点坐标。