GICP中 plane-to-plane协方差矩阵的计算

这一块当初看paper时简单看了一下,对原理没有做太多分析,最近要写一个类似的算法,花了几个小时的时间对这一块进行了仔细理解,发出来和大家分享。

GICP原文中,协方差矩阵的设计:

principle:

假设点服从平面分布。设计一个协方差矩阵,使得点在法向量方向具有较小的自由度,在平面方向具有较大的自由度。

 

特征值分解:

A=W\Sigma W^T

其中,W是A的特征向量构成的矩阵,W是正交矩阵,Σ是由A的特征值构成的对角矩阵。

因此,要设计一个3x3的协方差矩阵,我们只需要知道它的3个特征值和3个特征向量,然后按上述公式合成就可以了。

现在已知三个特征值,即Σ,同时知道一个特征向量v(即法向量e1),我们只需要求出另外两个特征向量就可以了。另外我们知道,一个三维空间点的3个特征向量两两正交。

因此,另外两个特征向量的构建就非常简单了,我们任意给定一个不和v平行的向量v1,然后对v和v1求叉乘并单位化,就可以得到另一个特征向量u,然后对v和u求叉乘并单位化,就可以得到第三个特征向量w。

这一个链接https://blog.csdn.net/zhazhiqiang/article/details/52383328描述了构建旋转矩阵的代码:

第一个特征向量是n,任意给定第二个向量y,则第二个特征向量为:

y=y-(n\cdot y)n

由于y=(0, 1, 0),则n\cdot y=n_y

第三个特征向量u=v\times y

 

 

 

 

 

 


更多精彩内容