关于ICP线性方法的学习

​ 关于ICP线性方法的学习

​ 当我们已知一些匹配好的3D点， P={p1,p2,p3……pn},p’’={p’1,p’2,p’3,……p’n},我们现在需要知道他们之间的变换R,t 使得
$P=RP'+t \quad (1)$P=RP′+t(1)
之后我们定义误差项
$e_i=P-RP'-t \quad (2)$ei​=P−RP′−t(2)
然后我们可以构建最小二乘问题，计算该误差项的平方和的最小值
$minJ=1/2\sum_{i=1}^{n}{||P-RP'-t||^2_2} \quad\quad (3)$minJ=1/2i=1∑n​∣∣P−RP′−t∣∣22​(3)
之后我们可以来解决该问题，可以先定义质心来求解
$p=1/n\sum_{i=1}^{n}{p_i} \quad\quad(4)$p=1/ni=1∑n​pi​(4)

$p'=1/n\sum_{i=1}^{n}p'_i \quad\quad (5)$p′=1/ni=1∑n​pi′​(5)

接下来我们对原式进行分解

通过该项化简，我们可以得知最后我们所需要化简的最终目标为
$min1/2\sum_{i=1}^{n}{||p_i-p-R(p'_i-p')||^2+||{p-Rp-t'}||} \quad \quad (6)$min1/2i=1∑n​∣∣pi​−p−R(pi′​−p′)∣∣2+∣∣p−Rp−t′∣∣(6)
在这里我们明确一下我们的目标，我们化简到这一步，最终我们需要求出的是向量R，t

那接下来我们可以先将式子（6）的后面部分视为0（这里我个人理解只是为了方便计算，而暂时把它看做0,)这样我们可以很快的算出前面的R，在代入式子
$$
p-Rp-t=0 \quad\quad (7)

$$
就可以计算t了。

那么我们现在对之前进行总结第一步计算质心p , p’然后计算去质心坐标
$q=p_i-p \quad\quad(8)$q=pi​−p(8)

$q'=p_i'-p‘\quad\quad(9)$q′=pi′​−p‘(9)

然后代入6式我们可以得到
$min1/2\sum_{i=1}^n{||q_i-Rq'_i||^2}\quad\quad(10)$min1/2i=1∑n​∣∣qi​−Rqi′​∣∣2(10)
这样我们只需对这个最小二乘化问题进行求解即可接下来介绍如何求解
$原式=\sum_{i=1}^n1/2||q_i^Tq_i+q_i'^TR^TRq_i'-2q_i^TRq_i'||\quad\quad(11)$原式=i=1∑n​1/2∣∣qiT​qi​+qi′T​RTRqi′​−2qiT​Rqi′​∣∣(11)
我们注意到只有最后一项与R有关，即我们需要优化的只有最后一项（第二项中优于R为对称矩阵，RRT=I)所以接下来讨论对于最后一项的简化
$\sum_{i=1}^n-q_i^TRq_i'=\sum_{i=1}^n-tr(Rq_i'q_i^T)=-tr(R\sum_{i=1}^nq_i'q_i^T) \quad\quad(12)$i=1∑n​−qiT​Rqi′​=i=1∑n​−tr(Rqi′​qiT​)=−tr(Ri=1∑n​qi′​qiT​)(12)
然后我们可以利用SVD方法对该式子进行进一步的求解，
$令W=\sum_{i=1}^nq_iq_i'^T\quad\quad(13)$令W=i=1∑n​qi​qi′T​(13)
对W进行SVD分解可得
$W=U\sum V^T\quad\quad(14)$W=U∑VT(14)
当W满秩时
$R=UV^T\quad\quad(15)$R=UVT(15)
解出R之后，我们便可以利用式子（7）计算t ICP问题便得以解决，