手写VIO代码中LM法的应用

LM 法的实现

废话不多说！！
下面Solve函数实现了LM法的主要步骤。
总结起来：
1、构建初始优化问题的H矩阵 MakeHessian() 。
2、计算阻尼因子的初始值 ComputeLambdaInitLM()。
3、开始迭代 while(退出条件)，迭代有3个退出条件 ：(1)、迭代次数 (2)、变量更新很小 (3)、总残差很小。
while(一次迭代完成)
(1)、为H矩阵添加阻尼因子，并求解方程得到deltaX。
(2)、更新状态 。
(3)、判断迭代是否成功 IsGoodStepInLM()，成功则更新阻尼系数,并重新构造H矩阵 MakeHessian() 退出while， 不成功则状态回滚继续while。
end
end

bool Problem::Solve(int iterations) {
if (edges_.size() == 0 || verticies_.size() == 0) {
   std::cerr << "\nCannot solve problem without edges or verticies" << std::endl;
   return false;
}

TicToc t_solve;
// 统计优化变量的维数，为构建 H 矩阵做准备
SetOrdering();
// 遍历edge, 构建 H = J^T * J 矩阵    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
MakeHessian();
// LM 初始化   阻尼因子初始值       ！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
ComputeLambdaInitLM();
// LM 算法迭代求解
bool stop = false;
int iter = 0;
// 进行迭代      优化退出条件2   ：迭代次数够了 
while (!stop && (iter < iterations)) {
   std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_
             << std::endl;
   bool oneStepSuccess = false;
   int false_cnt = 0;
   while (!oneStepSuccess)                     // 不断尝试 Lambda, 直到成功迭代一步
   {
       // setLambda    添加阻尼因子
       AddLambdatoHessianLM();
       // 第四步，解线性方程 H X = B    求X
       SolveLinearSystem();
       // 取出阻尼因子   因为每次阻尼因子都不一样  都需要重新赋值     
       RemoveLambdaHessianLM();

       // 优化退出条件1： delta_x_ 很小则退出
       if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
           stop = true;
           break;
       }

       // 更新状态量 X = X+ delta_x
       UpdateStates();
       // 判断当前步是否可行以及 LM 的 lambda 怎么更新
       oneStepSuccess = IsGoodStepInLM();
       // 后续处理， 如果本次迭代OK
       if (oneStepSuccess) {
           // 在新线性化点 构建 hessian
           MakeHessian();     
           // TODO:: 这个判断条件可以丢掉，条件 b_max <= 1e-12 很难达到，这里的阈值条件不应该用绝对值，而是相对值
//                double b_max = 0.0;
//                for (int i = 0; i < b_.size(); ++i) {
//                    b_max = max(fabs(b_(i)), b_max);
//                }
//                // 优化退出条件2： 如果残差 b_max 已经很小了，那就退出
//                stop = (b_max <= 1e-12);
           false_cnt = 0;
       } else {
           false_cnt++;
// 之前的状态更新不算   重新减掉
           RollbackStates();   // 误差没下降，回滚
       }
   }
   iter++;     // 本次迭代结束   迭代数++

   // 优化退出条件3： currentChi_ 跟第一次的chi2相比，下降了 1e6 倍则退出
   if (sqrt(currentChi_) <= stopThresholdLM_)
       stop = true;
}
std::cout << "problem solve cost: " << t_solve.toc() << " ms" << std::endl;
std::cout << "   makeHessian cost: " << t_hessian_cost_ << " ms" << std::endl;
return true;
}

下面对几个重要的部分进行详细的解析。

H矩阵的构建

对于一个图优化问题，损失值包含在所有的边中，我们需要寻找一个状态变化值让所有的边的损失值都下降，我们采取先对每一个边单独构建H矩阵和b矩阵，然后再将所有的边的H矩阵与b矩阵合并的方式获得最终的H和b矩阵，从而求解出状态变化量。

步骤如下：
1、先遍历全部的边，对每个边先计算出残差，以及其jacobian矩阵，注意该jacobian矩阵是通过vector保存的，Vector中每个元素保存的是边的残差关于该边连接的某一个节点的jacobian。
2、然后遍历该边的连接的所有节点，获取该节点的id，id是在构造节点的时候赋予的，它表示节点的jacobian作用在大H矩阵中的位置，在设定Id时应该要依据同类节点id连续分布的原则；获取该节点的优化变量的维度，这个维度将决定该节点的jacobian在大H矩阵中的子块的size。获取该节点的jacobian，计算JtW。
3、内部接着遍历后面的节点, 获取该节点的jacobian J’与id，计算出JtWJ’ , 再根据id号将其放置与大H矩阵合适位置，计算出b = -JtW * edge.second->Residual(),添加到大b矩阵中。
理论说明如下：

void Problem::MakeHessian() {
   TicToc t_h;       // 记时间 对象
   // 直接构造大的 H 矩阵
   ulong size = ordering_generic_;
   // H = JTJ   
   MatXX H(MatXX::Zero(size, size));
   VecX b(VecX::Zero(size));

   // TODO:: accelate, accelate, accelate
   //#ifdef USE_OPENMP
   //#pragma omp parallel for
   //#endif

   // 遍历每个残差，并计算他们的雅克比，得到最后的 H = J^T * J
   // edge是一个pair 即( id, edge )
   for (auto &edge: edges_) {
       // 先计算该边的残差   ->   计算对应jacobian矩阵 
       edge.second->ComputeResidual();
       edge.second->ComputeJacobians();
       // 取出该边的     jacobians_   容器    该容器保存改边关于其个节点的jacobian
       auto jacobians = edge.second->Jacobians();
// 取出该边连接的节点     
       auto verticies = edge.second->Verticies();
// jacobians这个vector 的元素个数 = 该边节点个数                   
       assert(jacobians.size() == verticies.size());
// 遍历该边的全部节点                                                                                                              
       for (size_t i = 0; i < verticies.size(); ++i) {
           auto v_i = verticies[i];
           if (v_i->IsFixed()) continue;                                                                                     // Hessian 里不需要添加它的信息，也就是它的雅克比为 0
           // 取出关于节点i的jacobian
           auto jacobian_i = jacobians[i];
    // 获取该节点的Id    这个id是在创建节点时设置的     这个id要保证同一类节点连续的排列在一块
           ulong index_i = v_i->OrderingId();                                                                       
           ulong dim_i = v_i->LocalDimension();                         
           
           MatXX JtW = jacobian_i.transpose() * edge.second->Information();     // 信息矩阵维度等于该边残差的维度
    // 再遍历剩余节点  将每个节点jacobian项对H矩阵的贡献添加
           for (size_t j = i; j < verticies.size(); ++j) {
               auto v_j = verticies[j];

               if (v_j->IsFixed()) continue;
               // 该节点的jacobian
               auto jacobian_j = jacobians[j];
               ulong index_j = v_j->OrderingId();             
               ulong dim_j = v_j->LocalDimension();      // 该节点的状态维数   

               assert(v_j->OrderingId() != -1);
	// Jt * W * J
               MatXX hessian = JtW * jacobian_j;
               // 所有的信息矩阵叠加起来   (i，j)
               H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
	// 如果是不同节点 直接根据对称性将 （j,i）添加
               if (j != i) {
                   // 对称的下三角
                   H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();
               }
           }
           // 每个jacobian 贡献一次b
           b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
       }

   }
   // 构建完后  赋值
   Hessian_ = H;
   b_ = b;
   t_hessian_cost_ += t_h.toc();            // 积累构建该H矩阵的时间

   delta_x_ = VecX::Zero(size);                // initial delta_x = 0_n;
}

阻尼因子初始值的设定

因为LM法x增量可以展开成下式，一般来说最好让阻尼系数和特征值处于同一个数量级。
经验的做法是，取阻尼系数使其和最大特征值的数量级相同，而特征值的最大数量级一般又与JtJ对角线最大元素相同，所以阻尼系数的初值可以设置为JtJ对角线最大元素×比例系数。

/// LM                    计算阻尼因子的初始值                           
void Problem::ComputeLambdaInitLM() {
    ni_ = 2.;
    currentLambda_ = -1.;
    currentChi_ = 0.0;
    // TODO:: robust cost chi2    求总的残差值    
    for (auto edge: edges_) {
        currentChi_ += edge.second->Chi2();
    }
     // err_prior_是什么？？？？？？？？？？？？？？？？？？？？？？？？？         
    if (err_prior_.rows() > 0)
        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");
    // 找出Hessian 对角线最大的元素
    for (ulong i = 0; i < size; ++i) {
        maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal);
    }
    // 系数
    double tau = 1e-5;    
    // 乘上系数    作为初始的阻尼系数
    currentLambda_ = tau * maxDiagonal;
}

阻尼因子的更新

//   更新阻尼因子
//  sba论文方法
bool Problem::IsGoodStepInLM() {
    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);            //  sba论文方法
    scale += 1e-3;                                                                                                                     // make sure it's non-zero :)
 //   scale = scale /  2; 
   
    // recompute residuals after update state
    // 统计所有的残差    即F(x+dx)
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }
   // 计算  ( F(X) - F（X + dX）) / ( L(X) - L（X+dX） ) 
    double rho = (currentChi_ - tempChi) / scale;
    
    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        // 1 - （2*rho - 1）^3
        double alpha = 1. - pow((2 * rho - 1), 3);
	// alpha最大是 2/3     感觉和教材上不符合？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？
        // alpha = std::min(alpha, 2. / 3.);   

        double scaleFactor = (std::max)(1. / 3., alpha);
        currentLambda_ *= scaleFactor;
        ni_ = 2;                                    
        currentChi_ = tempChi;    // 保存当前的F
        return true;
    } else {   
       // 阻尼增大   x减小 
        currentLambda_ *= ni_;
	// 增大的更多
        ni_ *= 2;
        return false;
    }
}