Line segment matching

FMII2方法：FMII方法的轻微的修改。有限线段和无限线段（直线）的匹配。

求解方法：

SVD分解

Unit Quaternion

协方差矩阵：

通过对C进行SVD分解得到R，根据R求得T。

算法流程：

计算均值；

计算C；

SVD分解，求得R；求得T。

更新sn；

判断sn和前一次相比是否显著不同，如果不再显著不同，跳出循环。