SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现

参考论文 On degeneracy of optimization-based state estimation problems发表在2016 ICRA上

原理非常直观，用线性代数的基础知识。用Guassian-Newton， 增量方程的Hessian matrix包含了求解该优化问题的退化信息。

求解增量方程时，可以将Hessian matrix的eigen value， eigen vector先解出来。较小的eigen vector对应的eigen value就代表类退化方向。

论文的算法流程为：

下面给出我用eigen的算法实现，具体实施如下：

一般我们解为 x_delta = -(JT*J).inv() *JT*b, 将x_delta， JT*J放到modifysolution即可。

//input: 
//    eigThreshlod: for eigen value
//    matAtA: Hessian matrix for the normal equation
//    matX: original solution
//output:
//    matX: modified solution
bool modifySolution(double eigThreshlod , Eigen::Matrix<double, 6, 6> &matAtA, 
Eigen::Matrix<double, 6, 1> &matX)
{
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> es(matAtA);
    Eigen::Matrix<double, 3, 1> matE;// = es.eigenvalues();
    Eigen::Matrix<double, 3, 3> matV;// = es.eigenvectors();
    Eigen::Matrix<double, 3, 3> matV2;// = es.eigenvectors();

    for (int i = 2; i >= 0; i--)
    {
        matE.row(i) = es.eigenvalues().row(2-i);
        matV.row(i) = es.eigenvectors().row(2-i);
        matV2.row(i) = es.eigenvectors().row(2-i); 
    }
		
    isDegenerate = false;
    double eignThre[3] = {eigThreshlod, eigThreshlod, eigThreshlod};
    for (int i = 2; i >= 0; i--)
    {
        if (matE(i, 0) < eignThre[i]) 
        {
            for (int j = 0; j < 3; j++)
	    {
                 matV2(i, j) = 0;
            }
            m_isDegenerate = true;
        } 
        else 
            break;
    }
    matP = matV.inverse() * matV2;

    if (isDegenerate) 
    {
        Eigen::Matrix<double, 3, 1> matX2 = matX;
        matX = matP * matX2;
    }

    return !isDegenerate;
}