[基础知识点]LM算法——阻尼因子更新策略(Nielsen)——内容与代码解析

1. 内容

定量分析，阻尼因子更新策略通过比例因子来确定的：

其中：


这里采用的${\mu }_0$初始值的策略为：

2. 代码

此代码节选自BA

2.1. 计算阻尼因子的初始值

/// LM
void Problem::ComputeLambdaInitLM() {    
    ni_ = 2.;    
    currentLambda_ = -1.;    
    currentChi_ = 0.0;    
    // TODO:: robust cost chi2    
    for (auto edge: edges_) {        
         currentChi_ += edge.second->Chi2();
    }    
    if (err_prior_.rows() > 0)      // marg prior residual        
        currentChi_ += err_prior_.norm();
    
    stopThresholdLM_ = 1e-6 * currentChi_;          // 迭代条件为 误差下降 1e-6 倍
    double maxDiagonal = 0;    
    ulong size = Hessian_.cols();    
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");    
    for (ulong i = 0; i < size; ++i) {        
        maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal); // fabs取绝对值
    }    
    double tau = 1e-5;    
    currentLambda_ = tau * maxDiagonal;
}

其中$\tau$是一个系数，按工程经验取值(程序里取$10^{-5}$)double tau = 1e-5;，最后阻尼因子的计算为currentLambda_ = tau * maxDiagonal;maxdiagonal是H矩阵对角线上最大的块。

2.2. 阻尼因子的更新

bool Problem::IsGoodStepInLM() {    
    double scale = 0;    
    // 公式(11)的实现
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_); 
    scale += 1e-3;    // make sure it's non-zero :)
    
    // recompute residuals after update state    
    // TODO:: get robustChi2() instead of Chi2()    
    double tempChi = 0.0;    
    for (auto edge: edges_) {        
        edge.second->ComputeResidual(); // 残差计算        
        tempChi += edge.second->Chi2();    
    }    
    if (err_prior_.size() > 0)        
    tempChi += err_prior_.norm(); //  加先验信息的归一化 
    // 公式(10)的实现
    double rho = (currentChi_ - tempChi) / scale;
    // 公式(13)的实现    
    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降    
    {        
        double alpha = 1. - pow((2 * rho - 1), 3);        
        alpha = std::min(alpha, 2. / 3.);        

        double scaleFactor = (std::max)(1. / 3., alpha);        
        currentLambda_ *= scaleFactor;        

        ni_ = 2; // v        
        currentChi_ = tempChi;        
        return true;    
    } else {        
        currentLambda_ *= ni_;        
        ni_ *= 2;        
        return false;    
    }
}

2.2.1 残差计算

edge.second->ComputeResidual();

相关代码如下：

void EdgeReprojectionPoseOnly::ComputeResidual() 
{    
    VecX pose_params = verticies_[0]->Parameters();    
    Sophus::SE3d pose(        
        Qd(pose_params[6], pose_params[3], pose_params[4], pose_params[5]), // 旋转        
        pose_params.head<3>() // 平移    
    );
    Vec3 pc = pose * landmark_world_; // 世界坐标系下的路标点坐标转换成相机坐标系    
    pc = pc / pc[2]; // 归一化坐标    
    Vec2 pixel = (K_ * pc).head<2>() - observation_; // 残差   
    residual_ = pixel;
}