视觉SLAM14讲笔记05：ch7三角测量，PnP，ICP

三角测量

考虑图像 $I_1$ 和 $I_2,$ 以左图为参考，右图的变换矩阵为 $T$ 。相机光心为 $O_1$ 和 $O_{2} $。在$I_{1}$ 中有特征点 $p_1,$ 对应 $I_2$ 中有特征点 $p_2$ 。理论上直线 $O_1{p}_1$ 与 $O_2{p}_2$ 在 场景中会相交于一点 P，该点即是两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。因此，可以通过最二小乘去求解。

设 $x_1,x_2$ 为两个特征点的归一化坐标，那么它们满足:
$$s_1{x}_1=s_{2}R{x}_2+t$$

比方说先来看 $s_{2} $。如果我要算 $s_2,$ 那么先对上式两侧左乘一个 $x_1^{\wedge },$ 得:
$$0=s_2{x}_1^{\wedge }R{x}_2+x_1^{\wedge }t$$
可看成 $s_2$ 的一个方程，可以根据它直接求得 $s_2$。

3D-2D: PnP(Perspective-n-Point)

PnP是求解 3D 到 2D 点对运动的方法。它描述了当我们知道$n$个3D空间点以及它们的投影位置时，如何估计相机所在的位姿。前面已经说了，2D-2D的对极几何方法需要8个或8个以上的点对(以八点法为例)，且存在着初始化、纯旋转和尺度的问题。然而，如果两张图像中，其中一张特征点的3D位置已知，那么最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。

直接线性变

考虑某个空间点 $P,$ 它的齐次坐标为 $P=(X,Y,Z,1{)}^T$ 。在图像 $I_1$ 中，投影到特征 点 $x_1={\left(u_1,v_1,1\right)}^T$ （以归一化平面齐次坐标表示）。此时相机的位姿 $R,t$ 是未知的。与 单应矩阵的求解类似，我们定义增广矩阵 $[R|t]$ 为一个 $3\times 4$ 的矩阵，包含了旋转与平移信息。我们把它的展开形式列写如下：
$$\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{c}{{\mathbf{t}}_1}^T\\ {{\mathbf{t}}_2}^T\\ {{\mathbf{t}}_3}^T\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$
${\mathbf{t}}_1,{\mathbf{t}}_2,{\mathbf{t}}_3$为3个4维向量，则我们得到两个约束:
$$\begin{array}{rl}& t_1^{T}P-t_3^{T}P{u}_1=0\\ & \\ & t_2^{T}P-t_3^{T}P{v}_1=0\end{array}$$
请注意 $t$ 是待求的变量，可以看到每个特征点提供了两个关于 $t$ 的线性约束。假设 共有 N 个特征点，可以列出线性方程组:
$$\left(\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ ⋮& ⋮& ⋮\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right)\left(\begin{array}{l}{t}_1\\ t_2\\ t_3\end{array}\right)=0$$

由于 $t$ 一共有12维，因此最少通过六对匹配点，即可实现矩阵 $T$ 的线性求解，这种方法称为直接线性变换（Direct Linear Transform, DLT）。

最小化重投影误差

除了使用线性方法之外，我们可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。
假设某空间点坐标为 $P_i={\left[X_i,Y_i,Z_i\right]}^T,$ 其投影的像素坐标为 $u_i={\left[u_i,v_i\right]}^T$。我们定义增广矩阵 $T=[R|t]$ 为一个 $3\times 4$ 的矩阵，它的李代数表示为$\xi$，包含了旋转与平移。
$$\begin{gathered} s_i{u}_i=K(T{P}_i{)}_{[1:3]}, \\ T=\left[\begin{array}{ll}R& t\\ 0^T& 1\end{array}\right]=\exp \left({\xi }^{\wedge }\right) \end{gathered}$$
隐含着的齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的。$T{P}_i$ 结果是 $4\times 1$ 的，而它左侧的 $K$ 是 $3\times 3$ 的，所以必责把 $T{P}_i$ 的前三维取出来，变成三维的非齐次坐标。或者：
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=K[R|t{]}_{3\times 4}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$
该问题的误差项，是将像素坐标（观测到的投影位置）与 3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称之为重投影误差。构建最小二乘问题，然后寻找最好的相机位姿，使它最小化。
$$T^{\ast }=\arg \underset{\xi }{\min }\frac{1}{2}\sum _{i=1}^n{\Vert u_i-\frac{1}{s_i}K(T{P}_i{)}_{[1:3]}\Vert }_2^2$$

3D-3D: ICP(Iterative Closest Point)

假设我们有一组配对好的3D点:
$$P=\left\{p_1,\dots ,p_n\right\},P^{\prime }=\left\{p_1^{\prime },\dots ,p_n^{\prime }\right\}$$
现在，想要找一个欧氏变换 $R,t,$ 使得:
$$\forall i,p_i=R{p}_i^{\prime }+t$$

SVD 方法

根据前面描述的ICP问题，我们先定义第 $i$ 对点的误差项：
$$e_i=p_i-\left(R{p}_i^{\prime }+t\right)$$
然后，构建最小二乘问题，求使误差平方和达到极小的 $R,t:$
$$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n{\Vert \left(p_i-\left(R{p}_i^{\prime }+t\right)\right)\Vert }_2^2$$

非线性优化方法

求解ICP的另一种方式是使用非线性优化，以迭代的方式去找最优值。该方法和我们前面讲述的PnP非常相似。以李代数表达位姿时，目标函数可以写成：
$$\underset{\xi }{\min }=\frac{1}{2}\sum _{i=1}^n{\Vert \left(p_i-\exp \left({\xi }^{\wedge }\right)p_i^{\prime }\right)\Vert }_2^2$$
单个误差项关于位姿导数已经在前面推导过了，使用李代数扰动模型即可：
$$\frac{\partial e}{\partial \delta \xi }=-{\left(\exp \left({\xi }^{\wedge }\right)p_i^{\prime }\right)}^{\odot }$$