视觉里程计之相对位姿估计

视觉里程计之相对位姿估计

前言

这是视觉里程计的最后一个介绍，在第一个部分介绍了特征点法用于采集图像之间的特征，在匹配好特征点后，我们可以得到两个一一对应的像素点集。接下来要做的，就是根据两组匹配好的点集，计算相机的运动了。在普通的单目成像中，我们只知道这两组点的像素坐标。而在双目和RGBD配置中，我们还知道该特征点离相机的距离。因此，该问题就出现了多种形式：
2D-2D形式：通过两个图像的像素位置来估计相机的运动。

3D-2D形式：假设已知其中一组点的3D坐标，以及另一组点的2D坐标，求相机运动。

3D-3D形式:两组点的3D坐标均已知，估计相机的运动。

那么问题就来了：是否需要为这三种情况设计不同的计算方法呢？答案是：既可以单独做，也可以统一到一个大框架里去做。

单独做的时候，2D-2D使用对极几何的方法，3D-2D使用PnP求解算法，而3D-3D则称为ICP方法

统一的框架，就是指把所有未知变量均作为优化变量，而几何关系则是优化变量之间的约束。由于噪声的存在，几何约束通常无法完美满足。于是，我们把与约束不一致的地方写进误差函数。通过最小化误差函数，来求得各个变量的估计值。这种思路也称为Bundle Adjustment(BA).

这个章节主要给大家介绍3D-2D使用PnP求解算法。

PnP

PnP(Perspective-n-Point)算法是求解3D到2D点对运动的方法，它描述了当知道$n$个3D空间点及其投影位置时，如何估计相机的位姿。PnP问题有多种求解方法，例如，用3对点估计位姿的P3P、直接线性变换（DLT）、EPnP（Efficient PnP）等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，光束法平差（Bundle Adjustment, BA）。

DLT法

考虑某个空间点$P$在世界坐标系下的齐次坐标为 $P_w=(X_w,Y_w,Z_w,1{)}^T$ ，在相机坐标系下为 $P_c=(X_c,Y_c,Z_c{)}^T$ 。在图像$I_1$中，投影到特征点$x_1=(u_1,v_1,1{)}^T$(以归一化平面齐次坐标表示)。定义增广矩阵$[R|t]$为一个$3\times 4$的矩阵，包含了旋转与平移信息。世界坐标与观测的转换关系如下，像素坐标与相机坐标系的转换，深度乘左边比较好表示：

$s_1\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=K\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=K\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$

这里设：

$T=K\left[\begin{array}{cc}R& t\end{array}\right]=\left[\begin{array}{ccc}{t}_1& t_2& t_3\\ t_5& t_6& t_7\\ t_9& t_{10}& t_{11}\end{array}\begin{array}{c}{t}_4\\ t_8\\ t_{12}\end{array}\right]$

则原式子为：

$s_1\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=T\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$

如果是求相机$O_1$与相机$O_2$之间的相对关系，则这里的$P_w$就是$P_1$，而$R,t$为$O_1$在$O_2$下的姿态。

$s_1\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{ccc}{t}_1& t_2& t_3\\ t_5& t_6& t_7\\ t_9& t_{10}& t_{11}\end{array}\begin{array}{c}{t}_4\\ t_8\\ t_{12}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$

简单的矩阵乘法，最后一行可以得到$s_1$的表达式，于是$u_1,v_1$的值为：

$\begin{array}{c}{u}_1=\frac{t_1{X}_W+t_2{Y}_W+t_3{Z}_W+t_4}{t_9{X}_W+t_{10}{Y}_W+t_{11}{Z}_W+t_{12}}\end{array}$
$\begin{array}{c}{v}_1=\frac{t_5{X}_W+t_6{Y}_W+t_7{Z}_W+t_8}{t_9{X}_W+t_{10}{Y}_W+t_{11}{Z}_W+t_{12}}\end{array}$

为简化表示，设：

$t_1={\left(t_1\begin{array}{ccc}{t}_2& t_3& t_4\end{array}\right)}^T$
$t_2={\left(t_5\begin{array}{ccc}{t}_6& t_7& t_8\end{array}\right)}^T$
$t_3={\left(t_9\begin{array}{ccc}{t}_{10}& t_{11}& t_{12}\end{array}\right)}^T$

于是：

$t_1^{T}P-t_3^{T}P{u}_1=0$
$t_2^{T}P-t_3^{T}P{v}_1=0$

其中$t$为待求变量，与求单应矩阵类似，每个点对可以提供两个关于$t$的约束，假设一共有$N$个特征点，则可以得到如下线性方程组：

$\left(\begin{array}{ccc}{\mathbf{P}}_1^T& 0& -u_1{\mathbf{P}}_1^T\\ 0& {\mathbf{P}}_1^T& -v_1{\mathbf{P}}_1^T\\ \dots & \dots & \dots \\ {\mathbf{P}}_N^T& 0& -u_N{\mathbf{P}}_N^T\\ 0& {\mathbf{P}}_N^T& -v_N{\mathbf{P}}_N^T\end{array}\right)\left(\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right)=0$

$t$一共有12维，可以通过最少六对匹配点，实现对矩阵T的线性求解，这种方法称为直接线性法（DLT）。如果匹配点数大于6对，可以通过SVD方法对超定方程求最小二乘解。
求得矩阵后，需要先左乘一个内参矩阵的逆 $K^{-1}$ ，然后获得 $t$ 与 $t$ 。
需要说明的是，这种方法忽略了未知数之间的关联，比如旋转$R\in SO(3)$，使用这种方式求出的R矩阵不一定满足该关系，其为一个一般性矩阵，需要找一个矩阵对其进行近视，近似的方法由QR分解完成，也可以如下计算：

R   ← ( R R T ) { − 1 2 } R R\ \gets\left(RR^T\right)^{\left\{-\frac{1}{2}\right\}}R R ←(RRT){−21​}R

这种解法求得的解，既无尺度，也无约束，要得到尺度及约束，需要构建优化问题。

P3P

在知道世界坐标系或者其他相机坐标系下三维点与当前相机3个点的对应关系时，最少可以通过3对配对点来求当前相机相对于三维点坐标系的旋转和平移。
P3P需要利用给定的 3 个点的几何关系。它的输入数据为3对$3\text{D}-2\text{D}$匹配点。记$3\text{D}$点为$A,B，C$,$2\text{D}$点为$a,b,c$，其中小写字母代表的点为对应大写字母代表的点在相机成像平面上的投影，如下图所示：

P3P算法求得的结果，是另一个坐标系中三维点（比如世界坐标系中的$A,B,C$），在当前坐标系下的坐标（比如相机成像平面的$a,b,c$对应的相机坐标系下的3D坐标）。从而把一个3D-2D的关联，转化为3D-3D的关联，把PnP问题转换为ICP问题，通过求解ICP的方式获得旋转和平移。需要注意的是，P3P算法最后方程的解有四个，需要额外的一个点来验证，一般会选取重投影误差最小的解作为最优解。
三维点$A,B,C$，相机成像平面点$a,b,c$和相机光心$O$组成的三角形有如下对应关系：
$\Delta O_{ab}-\Delta O_{AB},\Delta O_{bc}-\Delta O_{BC},\Delta O_{ac}-\Delta O_{AC}$
根据余弦定理，对于这个大的空间三角形，有如下关系：
$\begin{array}{l}O{A}^2+O{B}^2-2OA\cdot OB\cdot \cos ⟨a,b⟩=A{B}^2\\ O{B}^2+O{C}^2-2OB\cdot OC\cdot \cos ⟨b,c⟩=B{C}^2\\ O{A}^2+O{C}^2-2OA\cdot OC\cdot \cos ⟨a,c⟩=A{C}^2.\end{array}$
对该方程进行消元，方法是除以$O{C}^2$，并设$x=OA/OC,y=OB/OC$，则方程变为：
$\begin{array}{l}{x}^2+y^2-2\cdot x\cdot y\cdot \cos ⟨a,b⟩=\frac{A{B}^2}{O{C}^2}\\ x^2+1-2\cdot x\cdot \cos ⟨a,c⟩=\frac{A{C}^2}{O{C}^2}\\ y^2+1-2\cdot y\cdot \cos ⟨b,c⟩=\frac{B{C}^2}{O{C}^2}\end{array}$
然后再进行替换，令：
$u=\frac{A{B}^2}{O{C}^2},v=\frac{B{C}^2}{A{B}^2},w=\frac{A{C}^2}{A{B}^2}$
可得：

$$\begin{array}{rl}& x^2+y^2-2\cdot x\cdot y\cdot \cos ⟨a,b⟩=u\\ & x^2+1-2\cdot x\cdot \cos ⟨a,c⟩=wu\\ & y^2+1-2\cdot y\cdot \cos ⟨b,c⟩=uv\end{array}$$

将第一个式子，带入第二第三个式子，可得：
$$\begin{array}{rl}& (1-w)x^2-w\cdot y^2-2\cdot x\cdot \cos ⟨a,c⟩+2\cdot w\cdot x\cdot y\cdot \cos ⟨a,b⟩+1=0\\ & (1-v)y^2-v\cdot x^2-2\cdot y\cdot \cos ⟨b,c⟩+2\cdot v\cdot x\cdot y\cdot \cos ⟨a,b⟩+1=0\end{array}$$
因为$ABC$之间的相对位置关系是不变的，所以其中$v,w$值为已知值，而三个角度余弦值可以通过像素观测坐标求得，对于上述方程，只有$x,y$为未知数，为一个二元二次方程组，求解这个方程组，需要用到吴消元法。
它可以将原方程等效成一组特征列（Characteristic Serial, CS），凡是原方程组的解都会是CS的解，但是CS的解不一定是原方程的解，所以需要验证，这里的等效方程为：
$\{\begin{array}{l}{a}_4{x}^4+a_3{x}^3+a_2{x}^2+a_1{x}^1+a_0=0\\ b_{1}y-b_0=0\end{array}$
求得了$x,y$的值，就可以求取$OA,OB,OC$的值，根据下面的公式，$AB$已知，可以先求$OC$，然后分别求解$OB,OA$：
$x^2+y^2-2\cdot x\cdot y\cdot cos<a,b>=\frac{A{B}^2}{O{C}^2}y=\frac{OB}{OC},x=\frac{OA}{OC}$
求得三个点在相机坐标系下的深度之后，就可以通过像素坐标与三维点的投影关系，求出相机坐标系下点的坐标。
$\mathbf{A}=\vec{a}\cdot \Vert \mathbf{PA}\Vert$
附：吴消元法系数
$p=2\cdot cos<a,c>q=2\cdot cos<b,c>r=2\cdot cos<a,b>$
$\{\begin{array}{rl}{a}_4=& a^2+b^2-2a-2b+2\left(1-r^2\right)ba+1\\ a_3=& -2q{a}^2-rp{b}^2+4qa+(2q+pr)b+\left(r^{2}q-2q+rp\right)ab-2q\\ a_2=& \left(2+q^2\right)a^2+\left(p^2+r^2-2\right)b^2-\left(4+2q^2\right)a-\left(pqr+p^2\right)b-\left(pqr+r^2\right)ab+q^2+2\\ a_1=& -2q{a}^2-rp{b}^2+4qa+\left(pr+q{p}^2-2q\right)b+(rp+2q)ab-2q\\ a_0=& a^2+b^2-2a+\left(2-p^2\right)b-2ab+1\\ b_1=& b{\left(\left(p^2-pqr+r^2\right)a+\left(p^2-r^2\right)b-p^2+pqr-r^2\right)}^2\\ b_0=& \left((1-a-b)x^2+(a-1)qx-a+b+1\right))\\ & ((r^3\left(a^2+b^2-2a-2b+\left(2-r^2\right)ab+1\right)x^3+\\ & r^2\left(p+p{a}^2-2rqab+2rqb-2rq-2pa-2pb+p{r}^{2}b+4rqa+q{r}^{3}ab-2rq{a}^2+2pab+p{b}^2-r^{2}p{b}^2\right)x^2+\\ & (r^5\left(b^2-ab\right)-r^{4}pqb+r^3\left(q^2-4a-2q^{2}a+q^2{a}^2+2a^2-2b^2+2\right)+r^2\left(4pqa-2pqab+2pqb-2pq-2pq{a}^2\right)+\\ & r\left(p^2{b}^2-2p^{2}b+2p^{2}ab-2p^{2}a+p^2+p^2{a}^2\right))x+\\ & \left(2p{r}^2-2r^{3}q+p^3-2p^{2}qr+p{q}^2{r}^2\right)a^2+\left(p^3-2p{r}^2\right)b^2+\left(4q{r}^3-4p{r}^2-2p^3+4p^{2}qr-2p{q}^2{r}^2\right)a+\\ & \left(-2q{r}^3+p{r}^4+2p^{2}qr-2p^3\right)b+\left(2p^3+2q{r}^3-2p^{2}qr\right)ab+\\ & p{q}^2{r}^2-2p^{2}qr+2p{r}^2+p^3-2r^{3}q)\end{array}$

EPnP

EPnP算法最后求解在相机坐标系下，$3D$点在控制点下坐标时，复杂度为$O(n)$，其中$n$为配对点对数，比较高效，其名称也是如此得来的。其求解的思路为，先通过世界坐标系或者上一坐标系下3D点的坐标，计算出四个控制点坐标。基于控制点坐标，计算出3D点在控制点坐标系下坐标，而可以证明，在世界坐标系下控制点坐标，与在此时相机坐标系下控制点坐标是相同的。于是问题转为求此时相机坐标系下控制点坐标，此时先假设四个局部坐标系下控制点坐标，通过控制点坐标与观测坐标对应关系，即针孔相机投影，每一对匹配点，可以得到两个方程。把所有配对点都联立方程，得到一个12个未知数，$2n$个方程的齐次方程组，求解这个方程，方法为求矩阵零特征值的特征向量。由于方程组解系中非共线解向量数量和矩阵的秩与12的差值有关，这里只考虑有1到4个非共线解向量的情况。求得解后，再通过世界坐标系下控制点坐标与局部坐标系下控制点坐标具有相同的相对位置关系，得到解系中系数，从而得到相机坐标系下控制点坐标。这样就把3D-2D的关联，转为3D-3D的关联，最后使用带匹配关系的ICP算法，得到两个坐标系下的R和t。这时候会有四组解，取其中重投影误差最小的一组解为最后的解。
其中求解解系中系数时，可以用高斯牛顿法建立世界坐标系下控制点相对坐标与局部坐标系下相对坐标之差，对系数进一步优化。
所以这些算法，都是线性代数，非线性优化。
下面对该算法进行介绍：
与其他方法相比，EPnP方法的复杂度为O(n)，对于点对数量较多的PnP问题，非常高效。
核心思想是将3D点表示为4个控制点的组合，优化也只针对4个控制点，所以速度很快，在求解 Mx=0时，最多考虑了4个奇异向量，因此精度也很高。
第一步，选择世界坐标系下控制点坐标：
选择3D点的质心，作为第一个控制点，

${\mathbf{c}}_1^w=\frac{1}{n}{\sum }_{i=1}^n{\mathbf{P}}_i^w$

对n个3D点进行去重心操作：
$\mathbf{A}=\left[\begin{array}{c}({\mathbf{P}}_1^w{)}^T-({\mathbf{c}}_1^w{)}^t\\ \cdots \\ ({\mathbf{P}}_n^w{)}^T-({\mathbf{c}}_1^w{)}^t\end{array}\right]$
计算A的协方差矩阵，并求其特征值与对应的特征向量，剩余的三个控制点，由这三个特征值与特征向量构成：

$$\begin{array}{rl}& {\mathbf{c}}_2^w={\mathbf{c}}_1^w+\sqrt{{\lambda }_1}{\mathbf{V}}_1\\ & {\mathbf{c}}_3^w={\mathbf{c}}_1^w+\sqrt{{\lambda }_2}{\mathbf{V}}_2\\ & {\mathbf{c}}_4^w={\mathbf{c}}_1^w+\sqrt{{\lambda }_3}{\mathbf{V}}_3\end{array}$$

第二步，求解齐次重心坐标，也就是在上面的控制点坐标系下的坐标：

${\mathbf{P}}_i^w={\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w,with{\sum }_{j=1}^4{\alpha }_{ij}=1$

建立3D点坐标与控制点坐标关系：

$\left[\begin{array}{c}{\mathbf{P}}_i^w\\ 1\end{array}\right]=\mathbf{C}\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]=\left[\begin{array}{cccc}{\mathbf{c}}_1^w& {\mathbf{c}}_2^w& {\mathbf{c}}_3^w& {\mathbf{c}}_4^w\\ 1& 1& 1& 1\end{array}\right]\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]$
及：
${\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]}_{4\times 1}={\left[\begin{array}{cccc}{\mathbf{c}}_1^w& {\mathbf{c}}_2^w& {\mathbf{c}}_3^w& {\mathbf{c}}_4^w\\ 1& 1& 1& 1\end{array}\right]}_{4\times 4}^{-1}{\left[\begin{array}{c}{\mathbf{P}}_i^w\\ 1\end{array}\right]}_{4\times 1}={\mathbf{C}}^{-1}\left[\begin{array}{c}{\mathbf{P}}_i^w\\ 1\end{array}\right]$

对于世界坐标系与当前相机坐标系下观测点之间转换关系，有：
$${\mathbf{c}}_j^c=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\mathbf{c}}_j^w\\ 1\end{array}\right]$$

$${\mathbf{p}}_i^c=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_i^w\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w\\ 1\end{array}\right]$$

$${\mathbf{p}}_i^c=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w\\ {\sum }_{j=1}^4{\alpha }_{ij}\end{array}\right]=\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\mathbf{c}}_j^w\\ 1\end{array}\right]=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c$$
即，相机坐标系下控制点也是由世界坐标系下控制点刚体转换而来的，而控制点的系数不变。

第三步，在相机坐标系下，使用控制点坐标系数，建立观测方程：
$w_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_u& 0& u_c\\ 0& f_v& v_c\\ 0& 0& 1\end{array}\right]{\sum }_{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right]$
从而可以得到两个线性方程：
$$$\begin{array}{rl}& \sum _{j=1}^4{\alpha }_{ij}{f}_u{x}_j^c+{\alpha }_{ij}(u_c-u_i)z_j^c=0\\ & \sum _{j=1}^4{\alpha }_{ij}{f}_v{y}_j^c+{\alpha }_{ij}(v_c-v_j)z_j^c=0\end{array}$

把所有n 个点串联起来，我们可以得到一个线性方程组：

上式中，齐次重心坐标，相机内参数和2D像点坐标都是已知量，未知量是4个控制点在相机坐标系下的坐标。共12个位置参数，一个像点可以列2个方程，n 个像点可以列出2n 个方程。

其中$\text{x}=[c_1^{c^T},c_2^{c^T},c_3^{c^T},c_4^{c^T}]$,$\text{x}$就是控制点在摄像头坐标系下的坐标，显然这是一个$12\times 1$的向量，且$\text{x}$在$M$的右零空间中，或者$\text{x}\in ker(M)$:

$\text{x}={\sum }_{i=1}^N{\beta }_i{\text{v}}_i$

上式中，${\text{v}}_i$是$M$的$N$个零特征值对应的$N$特征向量。对于第$i$个控制点：

${\mathbf{c}}_i^c={\sum }_{k=1}^N{\beta }_k{\text{v}}_k^{[i]}$

求得特征向量后，还需要求解系数β，该步通过世界坐标系下控制点相对坐标与局部坐标系下控制点相对坐标一样来解决：
根据仿真发现$M^{T}M$为0的特征值的个数喝摄像头的焦距有关（注意：是和焦距有关，而不是和参考点的位置有关！），EPnP算法建议只考虑$N=1,2,3,4$的情况。摄像头的外参描述的只是坐标变换，不会改变控制点之间的距离
，${\Vert {\mathbf{c}}_i^c-{\mathbf{c}}_j^c\Vert }^2={\Vert {\mathbf{c}}_i^w-{\mathbf{c}}_j^w\Vert }^2$,
${\Vert {\sum }_{k=1}^N{\beta }_k{\text{v}}_k^{[i]}-{\sum }_{k=1}^N{\beta }_k{\text{v}}_k^{[j]}\Vert }^2={\Vert {\mathbf{c}}_i^w-{\mathbf{c}}_j^w\Vert }^2$

这是一个关于 { β k } k = 1 , ⋯   , N \{\beta_k\}_{k=1,\cdots,N} {βk​}k=1,⋯,N​的二次方程，这个方程的特点是没有关于 { β } k = 1 , ⋯   , N \{\beta\}_{k=1,\cdots,N} {β}k=1,⋯,N​的一次项。如果将这些二次项${\beta }_i{\beta }_j$变量替换为${\beta }_{ij}$,那么该方程是 { β i j } i , j = 1 , ⋯   , N \{\beta_{ij}\}_{i,j=1,\cdots,N} {βij​}i,j=1,⋯,N​的线性方程了。对于4个控制点，可以得到$C_4^2=6$个这样的方程。
如果不挖掘任何附加条件，$N$取不同值的时候，新的线性未知数的个数分别为：
○ $N=1$，线性未知数的个数为1；
○ $N=2$，线性未知数的个数为3；
○ $N=3$，线性未知数的个数为6；
○ $N=4$，线性未知数的个数为10；
$N=4$的时候，未知数的个数多于方程的个数了。注意到
${\beta }_{ab}{\beta }_{cd}={\beta }_a{\beta }_b{\beta }_c{\beta }_d={\beta }_{a^{\prime }{b}^{\prime }}{\beta }_{c^{\prime }{d}^{\prime }}$,其中  ⁣ { a ′ , b ′ , c ′ , d ′ }  ⁣ \!\{a',b',c',d'\}\! {a′,b′,c′,d′}是 { a , b , c , d } \{a,b,c,d \} {a,b,c,d}的一个排列，我们可以减少未知数的个数。举例，如果我们求出了${\beta }_{11},{\beta }_{12},{\beta }_{13}$,那么我们可以得到${\beta }_{23}=\frac{{\beta }_{12}{\beta }_{13}}{{\beta }_{11}}$。这样，即使对于$N=4$，我们也可以求解出 { β i j } i , j = 1 , ⋯   , N \{\beta_{ij}\}_{i,j=1,\cdots,N} {βij​}i,j=1,⋯,N​了。
高斯-牛顿最优化
优化的目标函数为：
$\text{Error}(\beta )={\sum }_{(i,j)s.t.i<j}{\left({\Vert {\mathbf{c}}_i^c-{\mathbf{c}}_j^c\Vert }^2-{\Vert {\mathbf{c}}_i^w-{\mathbf{c}}_j^w\Vert }^2\right)}^2$
这是一个无约束的非线性最优化问题，简单。
求得系数后，便可以得到世界坐标系下3D点坐标在相机坐标系下坐标，接下来使用icp算法进行求解。
icp算法求解的步骤为：
Step1：计算点云质心坐标

$\begin{array}{rl}& {\text{P}}_c^c=\frac{1}{n}\sum _{i=1}^n{\mathbf{P}}_i^c\end{array}$
$\begin{array}{rl}& {\text{P}}_c^w=\frac{1}{n}\sum _{i=1}^n{\mathbf{P}}_i^w\end{array}$

Step2：点云去质心

${\bar{\text{P}}}^c=\left[\begin{array}{c}({\text{p}}_1^c{)}^T-({\text{p}}_c^c{)}^T\\ \cdots \\ ({\text{p}}_n^c{)}^T-({\text{p}}_c^c{)}^T\end{array}\right]$
${\bar{\text{P}}}^c=\left[\begin{array}{c}({\text{p}}_1^w{)}^T-({\text{p}}_c^w{)}^T\\ \cdots \\ ({\text{p}}_n^w{)}^T-({\text{p}}_c^w{)}^T\end{array}\right]$

Step3：计算旋转矩阵 $\text{R}$

$\text{W}={\left({\bar{\text{P}}}^c\right)}^T{\bar{\text{P}}}^w$
$[\text{U}\mathbf{\Sigma }\text{V}]=\text{SVD}(\text{W})$
$\text{R}={\text{UV}}^{\mathbf{T}}$

如果$det(\text{R})<0,\text{R}(2,:)=-\text{R}(2,:)$

Step4：计算平移向量t

$\text{t}={\text{p}}_c^c-{\text{Rp}}_c^w$

注意上面提到R和t有四种情况的解，最后通过计算反投影误差。采用反投影误差最小的求解结果。

ICP

ICP方法是用于求解$3\text{D}-3\text{D}$的位姿估计问题，ICP算法为迭代最近点算法，是求两个点云之间精配准的算法，其流程主要如下：
(一) 寻找最近点
(二) 计算loss,得到当前使loss最小的旋转和平移
(三) 变换原点云，再次寻找最近点
(四) 判断此时距离是否小于某个阈值，即收敛，没有则再次回到1进行迭代。
计算的loss函数如下，其中pt,ps为target点云下对应点及source点云下对应点。

$R^{\ast },t^{\ast }=\arg \underset{R,t}{\min }\frac{1}{|P_s|}{\sum }_{i=1}^{|P_s|}{\left|p_t^i-\left(R\cdot p_s^i+t\right)\right|}^2$

第一步，寻找最近点，有如下两种方法：
1.设置距离阈值，当点与点距离小于一定阈值就认为找到了对应点，不用遍历完整个点集；
2.使用 ANN(Approximate Nearest Neighbor) 加速查找，常用的有 KD-tree；KD-tree 建树的计算复杂度为 O(N log(N))，查找通常复杂度为 O(log(N))（最坏情况下 O(N)）。
第二步，寻找最优变换
对于 point-to-point ICP 问题，求最优变换是有闭形式解（closed-form solution）的，可以借助 SVD 分解来计算。
计算两个点云的质心μt, μs

${\mu }_p=\frac{1}{n}{\sum }_{i=1}^n{p}_i{\mu }_q=\frac{1}{n}{\sum }_{i=1}^n{q}_i$

对匹配点距离误差进行变换：

$\begin{array}{rl}& \frac{1}{2}\sum _{i=1}^n{\Vert {\text{q}}_i-{\text{Rp}}_i-\text{t}\Vert }^2\\ =& \frac{1}{2}\sum _{i=1}^n{\Vert {\text{q}}_i-{\text{Rp}}_i-\text{t}-({\mu }_q-{\text{R}\mathbf{\mu }}_p)+({\mathbf{\mu }}_q-{\text{R}\mathbf{\mu }}_p)\Vert }^2\\ =& \frac{1}{2}\sum _{i=1}^n{\Vert {(\text{q}}_i-{\mathbf{\mu }}_q-\text{R}({\text{p}}_i-{\mathbf{\mu }}_{\mathbf{p}}))+({\mathbf{\mu }}_q-{\text{R}\mathbf{\mu }}_p-\text{t})\Vert }^2\\ =& \frac{1}{2}\sum _{i=1}^n\left({\Vert {\text{q}}_i-{\mathbf{\mu }}_q-\text{R}({\text{p}}_i-{\mathbf{\mu }}_{\mathbf{p}})\Vert }^2+{\Vert {\mathbf{\mu }}_q-{\text{R}\mathbf{\mu }}_p-\text{t}\Vert }^2+2({\text{q}}_i-{\mathbf{\mu }}_{\mathbf{q}}-\text{R}({\text{p}}_i-{\mathbf{\mu }}_{\mathbf{p}}){)}^T({\mathbf{\mu }}_{\mathbf{q}}-\text{R}{\mathbf{\mu }}_{\mathbf{p}}-\mathbf{t})\right)\end{array}$
对于最后一项，有

$\begin{array}{rl}& \sum _{i=1}^n({\mathbf{q}}_i-{\mathbf{\mu }}_q-\mathbf{R}({\mathbf{p}}_i-{\mathbf{\mu }}_p){)}^T({\mathbf{\mu }}_q-\mathbf{R}{\mathbf{\mu }}_p-\mathbf{t})\\ & =({\mathbf{\mu }}_q-\mathbf{R}{\mathbf{\mu }}_p-\mathbf{t}{)}^T\sum _{i=1}^n({\mathbf{q}}_i-{\mathbf{\mu }}_q-\mathbf{R}({\mathbf{p}}_i-{\mathbf{\mu }}_p))\\ & =({\mathbf{\mu }}_q-\mathbf{R}{\mathbf{\mu }}_p-\mathbf{t}{)}^T(n{\mathbf{\mu }}_q-n{\mathbf{\mu }}_q-\mathbf{R}(n{\mathbf{\mu }}_p-n{\mathbf{\mu }}_p))\\ & =0\end{array}$

再进一步变换：

令 ${\text{p}}_i^{\prime }={\text{p}}_i-{\mathbf{\mu }}_p,{\text{q}}_i^{\prime }={\text{q}}_i-{\mathbf{\mu }}_q$，目标函数简化为：

$\frac{1}{2}{\sum }_{i=1}^n\left({\Vert {\text{q}}_i^{\prime }-\text{R}{\text{p}}_i^{\prime }\Vert }^2+{\Vert {\mathbf{\mu }}_q-\text{R}{\mathbf{\mu }}_p-\mathbf{t}\Vert }^2\right)$

设${\text{R}}^{\ast },{\text{t}}^{\ast }$为最优解，可以将优化变为如下问题：

(一) ${\text{R}}^{\ast }={\mathrm{argmin}}_{\text{R}}\frac{1}{2}{\sum }_{i=1}^n{\Vert {\text{q}}_i^{\prime }-\text{R}{\text{p}}_i^{\prime }\Vert }^2$
(二) ${\text{t}}^{\ast }={\mathbf{\mu }}_q-\text{R}{\mathbf{\mu }}_p$
对于第一步，有：
$\begin{array}{rl}{\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\min }\frac{1}{2}\sum _{i=1}^n\left(({\mathbf{q}}_i^{\prime }{)}^T{\mathbf{q}}_i+({\mathbf{p}}_i^{\prime }{)}^T{\mathbf{R}}^T\mathbf{R}{\mathbf{p}}_i^{\prime }-2({\mathbf{q}}_i^{\prime }{)}^T\mathbf{R}{\mathbf{p}}_i^{\prime }\right)\\ & =\arg \underset{\mathbf{R}}{\min }\frac{1}{2}\sum _{i=1}^n\left(({\mathbf{q}}_i^{\prime }{)}^T{\mathbf{q}}_i+\mathbf{I}-2({\mathbf{q}}_i^{\prime }{)}^T\mathbf{R}{\mathbf{p}}_i^{\prime }\right)\\ & =\arg \underset{\mathbf{R}}{\min }\sum _{i=1}^n-({\mathbf{q}}_i^{\prime }{)}^T\mathbf{R}{\mathbf{p}}_i^{\prime }\end{array}$

对这个优化，有如下最优解：

令$\text{W}={\sum }_{i=1}^n{\text{q}}_i^{\prime }{{\text{p}}_i^{\prime }}^T$，通过SVD分解有

$\text{W}=\text{U}\Sigma {\text{V}}^T$

求得旋转和平移为：

当$\text{W}$满秩事有：

$\Sigma =\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right]$

且对应唯一的$\text{U},\text{V}$组合，对应地

$\begin{array}{rl}& {\text{R}}^{\ast }=\text{U}{\text{V}}^T\\ & {\text{t}}^{\ast }={\mathbf{\mu }}_q-\text{R}{\mathbf{\mu }}_p\end{array}$

一般需要对求得的旋转矩阵进行检查，即判断其是否为一个旋转矩阵，判断方式如下：
最后还需要进行Orientation rectification，即验证${\mathbf{R}}^{\ast }=\mathbf{V}{\mathbf{U}}^T$是不是一个旋转矩阵（检查是否有$|R|=1$, 因为存在$|R|=-1$的可能，此时$R$表示的不是旋转而是一个reflection，所以我们还要给上述优化解加上一个$|R|=1$的约束）。
根据矩阵行列式的性质，以及$\text{U,V}$都是正交阵：

$\begin{array}{rl}|\mathbf{M}|& =|{\mathbf{V}}^T||\mathbf{U}||\mathbf{R}|\\ & =|{\mathbf{V}}^T||\mathbf{U}|=\pm 1\end{array}$

如果$|VUT|=1$，则$|M|=1$,${\mathbf{R}}^{\ast }=V{U}^T$已经给出最有旋转：如果$|V{U}^T|=-1$，则$|M|=-1$，我们需要求解此时的$R$，也就是分析$M$应该具有何种形式。经分析可得出结论：当$|M|=-1$时，使得$trace(\Sigma M)$最大的$M$为：

$M=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]$

综合考虑$|M|=1$和$|M|=-1$两种情况，我们可以得到：

$R^{\ast }=V\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& |V{U}^T|\end{array}\right]U^T$

第三步，迭代
每一次迭代我们都会得到当前的最优变换参数$R_k,t_k$，然后将该变换作用于当前源点云；"找最近对应点"和"求解最优变换"这两步不停迭代进行，直到满足迭代终止条件，常用的终止条件有：
1.$R_k,t_k$的变化量小于一定值
2.loss 变化量小于一定值
3.达到最大迭代次数
迭代结束后，把所有的平移和旋转求和，就是我们要求的平移和旋转了。

参考：https://www.guyuehome.com/46511