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

三角测量

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

设 $x _{1}, x _{2}$x1​,x2​ 为两个特征点的归一化坐标，那么它们满足:
$s_{1} x _{1}=s_{2} R x _{2}+ t$s1​x1​=s2​Rx2​+t

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

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

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

直接线性变

考虑某个空间点 $P,$P, 它的齐次坐标为 $P =(X, Y, Z, 1)^{T}$P=(X,Y,Z,1)T 。在图像 $I_{1}$I1​ 中，投影到特征 点 $x _{1}=\left(u_{1}, v_{1}, 1\right)^{T}$x1​=(u1​,v1​,1)T （以归一化平面齐次坐标表示）。此时相机的位姿 $R , t$R,t 是未知的。与 单应矩阵的求解类似，我们定义增广矩阵 $[R|t]$[R∣t] 为一个 $3 \times 4$3×4 的矩阵，包含了旋转与平移信息。我们把它的展开形式列写如下：
$\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right) =\left(\begin{array}{c} \boldsymbol{t_1}^T \\ \boldsymbol{t_2}^T \\ \boldsymbol{t_3}^T \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \\ 1 \end{array}\right)$⎝⎛​u1​v1​1​⎠⎞​=⎝⎛​t1​Tt2​Tt3​T​⎠⎞​⎝⎜⎜⎛​XYZ1​⎠⎟⎟⎞​
$\boldsymbol{t_1}, \boldsymbol{t_2}, \boldsymbol{t_3}$t1​,t2​,t3​为3个4维向量，则我们得到两个约束:
$\begin{aligned} &t _{1}^{T} P - t _{3}^{T} P u_{1}=0\\ &t _{2}^{T} P - t _{3}^{T} P v_{1}=0 \end{aligned}$​t1T​P−t3T​Pu1​=0t2T​P−t3T​Pv1​=0​
请注意 $t$t 是待求的变量，可以看到每个特征点提供了两个关于 $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} \\ \vdots & \vdots & \vdots \\ 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$⎝⎜⎜⎜⎜⎜⎛​P1T​0⋮PNT​0​0P1T​⋮0PNT​​−u1​P1T​−v1​P1T​⋮−uN​PNT​−vN​PNT​​⎠⎟⎟⎟⎟⎟⎞​⎝⎛​t1​t2​t3​​⎠⎞​=0

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

最小化重投影误差

除了使用线性方法之外，我们可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。
假设某空间点坐标为 $P _{i}=\left[X_{i}, Y_{i}, Z_{i}\right]^{T},$Pi​=[Xi​,Yi​,Zi​]T, 其投影的像素坐标为 $u _{i}=\left[u_{i}, v_{i}\right]^{T}$ui​=[ui​,vi​]T。我们定义增广矩阵 $T=[R|t]$T=[R∣t] 为一个 $3 \times 4$3×4 的矩阵，它的李代数表示为$\xi$ξ，包含了旋转与平移。
$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)$si​ui​=K(TPi​)[1:3]​,T=[R0T​t1​]=exp(ξ∧)
隐含着的齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的。$T P_{i}$TPi​ 结果是 $4 \times 1$4×1 的，而它左侧的 $K$K 是 $3 \times 3$3×3 的，所以必责把 $T P _{i}$TPi​ 的前三维取出来，变成三维的非齐次坐标。或者：
$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]$si​⎣⎡​ui​vi​1​⎦⎤​=K[R∣t]3×4​⎣⎢⎢⎡​Xi​Yi​Zi​1​⎦⎥⎥⎤​
该问题的误差项，是将像素坐标（观测到的投影位置）与 3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称之为重投影误差。构建最小二乘问题，然后寻找最好的相机位姿，使它最小化。
$T ^{*}=\arg \min _{ \xi } \frac{1}{2} \sum_{i=1}^{n}\left\| u _{i}-\frac{1}{s_{i}} K (T P _{i})_{[1:3]}\right\|_{2}^{2}$T∗=argξmin​21​i=1∑n​∥∥∥∥​ui​−si​1​K(TPi​)[1:3]​∥∥∥∥​22​

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

假设我们有一组配对好的3D点:
$P =\left\{ p _{1}, \ldots, p _{n}\right\}, \quad P ^{\prime}=\left\{ p _{1}^{\prime}, \ldots, p _{n}^{\prime}\right\}$P={p1​,…,pn​},P′={p1′​,…,pn′​}
现在，想要找一个欧氏变换 $R , t ,$R,t, 使得:
$\forall i, p _{i}= R p _{i}^{\prime}+ t$∀i,pi​=Rpi′​+t

SVD 方法

根据前面描述的ICP问题，我们先定义第 $i$i 对点的误差项：
$e _{i}= p _{i}-\left( R p _{i}^{\prime}+ t \right)$ei​=pi​−(Rpi′​+t)
然后，构建最小二乘问题，求使误差平方和达到极小的 $R, t:$R,t:
$\min _{ R , t } J=\frac{1}{2} \sum_{i=1}^{n}\left\|\left( p _{i}-\left( R p _{i}^{\prime}+ t \right)\right)\right\|_{2}^{2}$R,tmin​J=21​i=1∑n​∥(pi​−(Rpi′​+t))∥22​

非线性优化方法

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