ICP_SVD
设目标点云的点集为 : X = {x1,x2......,xn}
设当前点云的点集为 : Y = {y1,y2......,yn}
所构建的目标方程为:
对后面的方程进行化简
首先减去上 X Y 的平均值 为了等式相等所以也得加上
将 R t隔离
将二范数展开
上面的式子为0
所以
令只与旋转相关的式子
令只与平移相关的式子
那么,对于任意的R,都可以找到一个t 使得上式子等于 0
所以可以先求旋转在求平移
对于求解旋转有如下:
二范数展开
由于旋转矩阵的正交性质 所以上式前两项和 R 无关 所以求取下式的最大值
展开之后就需要用到 迹 由于常数的 迹 等于本身
H为确定的,问题转换为找到和是的R使得Trace(RH)为最大值
定理: 若有正定矩阵 AAT ,则对于任意正交矩阵 R 有 Trace(AAT) >= Trace(BAAT)
意义,如果能找到R 是的 Trace(RH) 为 Trace(AAT)形式,则R就是我们要找的矩阵 (这里可以理解 后面在对这个R进行变换相当于 做成一些变换矩阵 依旧没有当前的值大)
证明 :
ai为 A的列向量 根据柯西不等式,有
因此
当前目的: 找到 R ,把Trace(RH)转换为 Trace(AAT)
方法:
对H进行SVD分解
取 R 为
R = VU^T
那么R为我们需要的值