Lego_loam非线性迭代中的退化处理

        Lego_loam：分割地面点，对非地面点进行聚类，对于有效聚类点再进行特征提取。手写的优化方法和雅克比推导，并在代码层面实现了加权思想（floam论文里面提及，但是实际在代码层面未实现）；          

        Lego_loam论文说是采用LM，实际采用的是GN算法去进行最小二乘优化的。那么问题来了，在很久以前我手写非线性优化的一篇文章中，提及GN算法在迭代时，对系数矩阵是有要求的，它要求矩阵可逆且正定，否则求解的增量将会具有不稳定性。

        lego-loam我认为是比较优秀的算法（虽然开源代码有些编写错误，有空了发篇文章说一说），那么在lego-loam中自然也是考虑到了这个矩阵退化问题，那么他的解决思路：

        退化计算：(根据经验，在每轮特征迭代优化的首次计算中，判断一次即可)            
对于线性化系统AX=b，退化因子D是A的函数，与b无关。                

1）计算A的特征值matE和特征向量矩阵matV；
2）根据计算出的特征值matE与阈值（阈值设定>0）比较，特征值小于阈值，则认为A的该特征值退化；
3）在计算出的特征向量的矩阵matV中抹去其分量（不考虑退化方向），得到matV2；
4）matP = matV.inverse() * matV2；        
5）matX = matP * matX；得到最终的增量matX；      

     注意：以上流程中matV是以列存储。上述2）点，在lego-loam中，因为其采用的是高斯牛顿去迭代，高斯牛顿的解要稳定，A矩阵必须满足正定，但是A矩阵可能出现非正定，这里设定阈值，我猜测是保证A矩阵具有强正定性质，保证求解出的增量的稳定性！

        至于上述做法的详细原理，可以去参考非线性优化关于解退化相关方面的论文，比如：

《On Degeneracy of Optimization-based State Estimation Problems》