VIO第8讲：更优的LM策略与H矩阵加速

文章目录

VIO第8讲：更优的LM策略与H矩阵加速
[1 编译报错](#1 编译报错)
[2 OpenMP并行化加速测试](#2 OpenMP并行化加速测试)
- [2.1 基本用法](#2.1 基本用法)
- [2.2 一个简单的并行化测试程序](#2.2 一个简单的并行化测试程序)
- [2.3 海塞矩阵并行化处理](#2.3 海塞矩阵并行化处理)
[3 更优的 LM 策略](#3 更优的 LM 策略)
- [3.1 论文阅读](#3.1 论文阅读)
- - [3.1.1 引言](#3.1.1 引言)
  - [3.1.2 The Gradient Descent Method](#3.1.2 The Gradient Descent Method)
  - [3.1.3 The Gauss-Newton Method](#3.1.3 The Gauss-Newton Method)
  - [3.1.4 The Levenberg-Marquardt Method](#3.1.4 The Levenberg-Marquardt Method)
- [3.2 策略](#3.2 策略)
- - [3.2.1 阻尼因子初始化](#3.2.1 阻尼因子初始化)
  - [3.2.2 比列因子ρ](#3.2.2 比列因子ρ)
  - [3.2.3 阻尼因子更新策略](#3.2.3 阻尼因子更新策略)

1 编译报错

这种错误之前经常出现，就是c++版本不一样导致的！

Ubuntu20下编译会报错

error: 'slots_reference' was not declared in this scope1180 | cow_copy_type<list_type, Lockable> ref = slots_reference();| ~~~~~~~~~~~~~~~^~

error: 'm_slots' was not declared in this scope
1538 | auto &groups = detail::cow_write(m_slots);

改为c++14，因为这里是在ubuntu20上运行

c 复制代码

set(CMAKE_BUILD_TYPE "Debug")  # 设置构建类型为Debug
set(CMAKE_CXX_STANDARD 14)     # 设置C++标准版本为C++14
set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g -Wall")  # Debug模式下的编译器选项

2 OpenMP并行化加速测试

2.1 基本用法

标记并行区域 ：使用#pragma omp parallel指令标记需要并行执行的代码区域。
cpp 复制代码
```
#pragma omp parallel
{
    // 并行执行的代码
}
```
控制线程数量 ：使用num_threads子句控制线程数量。
cpp 复制代码
```
#pragma omp parallel num_threads(4)
{
    // 使用4个线程并行执行
}
```
并行化循环 ：使用#pragma omp for指令并行化循环。
cpp 复制代码
```
#pragma omp parallel for
for (int i = 0; i < n; i++) {
    // 循环体
}
```

共享和私有变量 ：使用shared和private子句来指定共享和私有变量。

cpp 复制代码

int sum = 0;
#pragma omp parallel for shared(sum)
for (int i = 0; i < n; i++) {
    sum += array[i];
}

2.2 一个简单的并行化测试程序

main.cpp，引入头文件#include <omp.h>

cpp 复制代码

#include <iostream>
#include <vector>
#include <omp.h>

int main() {
    const int n = 100;
    std::vector<int> array(n);

    // 初始化数组 1-100
    for (int i = 0; i < n; ++i) {
        array[i] = i + 1;
    }

    int sum = 0;

    // 使用OpenMP并行化计算数组元素和
    #pragma omp parallel for reduction(+:sum)
    for (int i = 0; i < n; ++i) {
        sum += array[i];
        // 
        std::cout << "Sum: " << sum << std::endl;
    }

    std::cout << "o Sum: " << sum << std::endl;
    return 0;
}

CMakeLists.txt

c 复制代码

cmake_minimum_required(VERSION 3.10)
project(OpenMPExample)

# Find OpenMP
find_package(OpenMP REQUIRED)

# Set C++ version
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)

# Add executable
add_executable(main main.cpp)

# Add OpenMP flags
target_link_libraries(main PRIVATE OpenMP::OpenMP_CXX)

输出显示---并行化

yaml 复制代码

...
Sum: 903
Sum: 946
Sum: 990
Sum: 1035
Sum: 1081
Sum: 1128
Sum: 1176
Sum: 1225
Sum: 1275
o Sum: 5050

2.3 海塞矩阵并行化处理

cpp 复制代码

// 并行化处理，在下面代码前加上这一行即可
#pragma omp parallel for
for (size_t i = 0; i < verticies.size(); ++i) {
    auto v_i = verticies[i];
    if (v_i->IsFixed()) continue;    // Hessian 里不需要添加它的信息，也就是它的雅克比为 0

    auto jacobian_i = jacobians[i];
    ulong index_i = v_i->OrderingId();
    ulong dim_i = v_i->LocalDimension();

在处理小规模问题时，使用OpenMP并不一定会加速，甚至可能会引入额外的开销。这是因为并行化本身会引入一些开销，如线程创建和同步等。当问题规模较小时，这些额外的开销可能会超过并行化所带来的性能提升。

比如用仿真的几帧数据去测试，发现速度反而增加！之前平均在25 ms，加上后反而需要70 ms。导致整个problem的求解也变得更慢了。

但是在自己写的VSLAM_demo上，速度得到了明显提升。

3 更优的 LM 策略

3.1 论文阅读

参考论文The Levenberg-Marquardt algorithm for nonlinear least squares curve-fitting problems

3.1.1 引言

引出最小二乘问题，p是参数向量，W是权值矩阵，一般是协方差矩阵之逆。

χ 2 ( p ) = ∑ i = 1 m [ y ( t i ) − y ^ ( t i ; p ) σ y i ] 2 = ( y − y ^ ( p ) ) T W ( y − y ^ ( p ) ) = y T W y − 2 y T W y ^ + y T ^ W y ^ \begin{aligned} \chi^{2}(\boldsymbol{p})& =\sum_{i=1}^{m}\left[\frac{y(t_{i})-\hat{y}(t_{i};\boldsymbol{p})}{\sigma_{y_{i}}}\right]^{2} \\ &=\quad(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p}))^\mathsf{T}\boldsymbol{W}(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p})) \\ &\begin{array}{rcl}=&\boldsymbol{y^\mathrm{T}}\boldsymbol{W}\boldsymbol{y}-2\boldsymbol{y^\mathrm{T}}\boldsymbol{W}\hat{\boldsymbol{y}}+\hat{\boldsymbol{y^\mathrm{T}}}\boldsymbol{W}\hat{\boldsymbol{y}}\end{array} \end{aligned} χ2(p)=i=1∑m[σyiy(ti)−y^(ti;p)]2=(y−y^(p))TW(y−y^(p))=yTWy−2yTWy^+yT^Wy^

3.1.2 The Gradient Descent Method

梯度下降，就是沿着梯度的反方向。

∂ ∂ p χ 2 = 2 ( y − y ^ ( p ) ) T W ∂ ∂ p ( y − y ^ ( p ) ) = − 2 ( y − y ^ ( p ) ) T W [ ∂ y ^ ( p ) ∂ p ] = − 2 ( y − y ^ ) T W J \begin{aligned} \frac{\partial}{\partial\boldsymbol{p}^{\chi^{2}}}& =2(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p}))^{\mathsf{T}}\boldsymbol{W}\frac{\partial}{\partial\boldsymbol{p}}\left(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p})\right) \\ &=\quad-2(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p}))^{\mathsf{T}}\boldsymbol{W}\left[\frac{\partial\hat{\boldsymbol{y}}(\boldsymbol{p})}{\partial\boldsymbol{p}}\right] \\ &=\quad-2(\boldsymbol{y}-\hat{\boldsymbol{y}})^{\mathsf{T}}\boldsymbol{WJ} \end{aligned} ∂pχ2∂=2(y−y^(p))TW∂p∂(y−y^(p))=−2(y−y^(p))TW[∂p∂y^(p)]=−2(y−y^)TWJ

3.1.3 The Gauss-Newton Method

假设目标函数在参数的最优解附近近似二次，通过一阶泰勒级数展开来近似函数，并找到这个二次函数的最小值。

这部分推导在vio第3讲仔细推导过了

一阶泰勒近似

χ 2 ( p + h ) ≈ y T W y + y ^ T W y ^ − 2 y T W y ^ − 2 ( y − y ^ ) T W J h + h T J T W J h \chi^2(\boldsymbol{p}+\boldsymbol{h})\approx\boldsymbol{y}^\mathsf{T}\boldsymbol{W}\boldsymbol{y}+\hat{\boldsymbol{y}}^\mathsf{T}\boldsymbol{W}\hat{\boldsymbol{y}}-2\boldsymbol{y}^\mathsf{T}\boldsymbol{W}\hat{\boldsymbol{y}}-2(\boldsymbol{y}-\hat{\boldsymbol{y}})^\mathsf{T}\boldsymbol{W}\boldsymbol{J}\boldsymbol{h}+\boldsymbol{h}^\mathsf{T}\boldsymbol{J}^\mathsf{T}\boldsymbol{W}\boldsymbol{J}\boldsymbol{h} χ2(p+h)≈yTWy+y^TWy^−2yTWy^−2(y−y^)TWJh+hTJTWJh

求导，导数为0处即最值

∂ ∂ h χ 2 ( p + h ) ≈ − 2 ( y − y ^ ) T W J + 2 h T J T W J \frac\partial{\partial\boldsymbol{h}}\chi^2(\boldsymbol{p}+\boldsymbol{h})\approx-2(\boldsymbol{y}-\hat{\boldsymbol{y}})^\mathsf{T}\boldsymbol{W}\boldsymbol{J}+2h^\mathsf{T}\boldsymbol{J}^\mathsf{T}\boldsymbol{W}\boldsymbol{J} ∂h∂χ2(p+h)≈−2(y−y^)TWJ+2hTJTWJ

更新公式

[ J T W J ] h g n = J T W ( y − y ^ ) \left[J^{\mathsf{T}}WJ\right]h_\mathrm{gn}=J^{\mathsf{T}}W(y-\hat{y}) [JTWJ]hgn=JTW(y−y^)

3.1.4 The Levenberg-Marquardt Method

LM算法自适应地在GD更新和GN更新之间变化参数更新。

前面提到，LM算法通过引入阻尼因子，解决了GN算法中H矩阵的不正定，但是也带来了如零空间变换等问题。

[ J T W J + λ I ] h I m = J T W ( y − y ^ ) , \left[J^{\mathsf{T}}WJ+\lambda I\right]h_{\mathsf{Im}}=J^{\mathsf{T}}W(y-\hat{y}), [JTWJ+λI]hIm=JTW(y−y^),

small values of the damping parameter λ result in a Gauss-Newton update and large values of λ result in a gradient descent update

当阻尼参数 λ 的值较小时，LM算法会采用高斯-牛顿更新，而当 λ 的值较大时，会采用梯度下降更新。

初始：设置较大的阻尼参数 λ，这样首次更新将是沿最陡下降方向的小步长。如果迭代使得残差结果增大，增大阻尼λ；否则，减小λ，更接近与GN算法。（理论上，就是让一开始快速下降，然后快接近最优值，使用GN缓慢优化，避免GD反复振荡）

列文伯格提出优化算法一般会令阻尼因子λ乘单位阵，如上面公式。马夸尔特则引入海塞矩阵H的对角矩阵，一般用下式表示
[ J T W J + λ d i a g ( J T W J ) ] h I m = J T W ( y − y ^ ) \left[J^{\mathsf{T}}WJ+\lambda\mathrm{~diag}(J^{\mathsf{T}}WJ)\right]h_{\mathsf{Im}}=J^{\mathsf{T}}W(y-\hat{y}) [JTWJ+λ diag(JTWJ)]hIm=JTW(y−y^)

3.2 策略

3.2.1 阻尼因子初始化

我们这里使用方法1（设置初始值为0.5倍初始误差和），整体的更新使用马夸尔特那种。方法2和3其实就是之前代码中的写法，取海塞矩阵对角线元素最大值，乘以一个常数后作为初始λ

之前方法

cpp 复制代码

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);
    }
    double tau = 1e-5;
    currentLambda_ = tau * maxDiagonal;
}

更改

cpp 复制代码

for (auto edge: edges_) {
        currentChi_ += edge.second->Chi2();
    }
if (err_prior_.rows() > 0)      // marg prior residual
    currentChi_ += err_prior_.norm();
currentLambda_ = 0.5 * currentChi_;

3.2.2 比列因子ρ

我们采用的是第三行公式（由上面LM算法中阻尼因子乘以单位矩阵I还是diagonal（H）决定），ρ的含义就是实际下降/预测下降

ρ i ( h l m ) = χ 2 ( p ) − λ 2 ( p + h l m ) ∣ ( y − y ^ ) T W ( y − y ^ ) − ( y − y ^ − J h l m ) T W ( y − y ^ − J h l m ) ∣ ( 14 ) = χ 2 ( p ) − χ 2 ( p + h l m ) ∣ h l m T ( λ i h m + J T W ( y − y ^ ( p ) ) ) ∣ if using eq'n (12) for h l m ( 15 ) = χ 2 ( p ) − χ 2 ( p + h l m ) ∣ h l m T ( λ i d i a g ( J T W J ) h l m + J T W ( y − y ^ ( p ) ) ∣ if using eq'n (13) for h ( p 16 ) \begin{array}{rcl}\rho_i(\boldsymbol{h}\mathrm{lm})&=&\frac{\chi^2(\boldsymbol{p})-\lambda^2(\boldsymbol{p}+\boldsymbol{h}\mathrm{lm})}{|(\boldsymbol{y}-\hat{\boldsymbol{y}})^\mathrm{T}\boldsymbol{W}(\boldsymbol{y}-\hat{\boldsymbol{y}})-(\boldsymbol{y}-\hat{\boldsymbol{y}}-\boldsymbol{J}\boldsymbol{h}\mathrm{lm})^\mathsf{T}\boldsymbol{W}(\boldsymbol{y}-\hat{\boldsymbol{y}}-\boldsymbol{J}\boldsymbol{h}\mathrm{lm})|}&(14)\\&=&\frac{\chi^2(\boldsymbol{p})-\boldsymbol{\chi}^2(\boldsymbol{p}+\boldsymbol{h}\mathrm{lm})}{|\boldsymbol{h}\mathrm{lm}^\mathsf{T}\left(\lambda_i\boldsymbol{h}\mathrm{m}+\boldsymbol{J}^\mathsf{T}\boldsymbol{W}\left(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p})\right)\right)|}&\text{if using eq'n (12) for }\boldsymbol{h}\mathrm{lm}(15)\\&=&\frac{\chi^2(\boldsymbol{p})-\chi^2(\boldsymbol{p}+\boldsymbol{h}\mathrm{lm})}{|\boldsymbol{h}\mathrm{lm}^\mathsf{T}\left(\lambda_i\mathrm{diag}(\boldsymbol{J}^\mathsf{T}\boldsymbol{W}J)\boldsymbol{h}_\mathrm{lm}+\boldsymbol{J}^\mathsf{T}\boldsymbol{W}\left(\boldsymbol{y}-\hat{\boldsymbol{y}}(\boldsymbol{p})\right)\right|}&\text{if using eq'n (13) for }\boldsymbol{h}(\mathfrak{p}16)\end{array} ρi(hlm)===∣(y−y^)TW(y−y^)−(y−y^−Jhlm)TW(y−y^−Jhlm)∣χ2(p)−λ2(p+hlm)∣hlmT(λihm+JTW(y−y^(p)))∣χ2(p)−χ2(p+hlm)∣hlmT(λidiag(JTWJ)hlm+JTW(y−y^(p))∣χ2(p)−χ2(p+hlm)(14)if using eq'n (12) for hlm(15)if using eq'n (13) for h(p16)

3.2.3 阻尼因子更新策略

之前的代码实现：使用了g2o、ceres中的更新策略，如下

if ρ > 0 . λ : = λ ∗ max ⁡ { 1 3 , 1 − ( 2 ρ − 1 ) 3 } ; ν : = 2 else λ : = λ ∗ ν ; ν : = 2 ∗ ν \begin{aligned} &\text{if }\rho>0& \text{.} \\ && λ:= λ*\operatorname*{max}\left\{\frac13,1-(2\rho-1)^{3}\right\};\quad\nu:=2 \\ &\text{else} \\ && λ:= λ*\nu;\quad\nu:=2*\nu &&& \end{aligned} if ρ>0else.λ:=λ∗max{31,1−(2ρ−1)3};ν:=2λ:=λ∗ν;ν:=2∗ν

论文中提到的思路，这里采用方法1

在迭代过程的每次步骤中，算法需要评估参数的更新量。更新量的接受与否取决于一个用户定义的阈值ϵ4。如果ρ > ϵ4，参数更新导致目标函数χ2的减少，则接受该更新量并对参数进行更新；否则，增加阻尼参数λ，并进入下一次迭代。

对于公式中参数L_up 和L_down，论文作者给出一个参考值（针对方法1）

这里设置 ϵ4 = 0

cpp 复制代码

bool Problem::IsGoodStepInLM() {
    double tempChi = 0.0;
    for (auto& edge : edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->RobustChi2();
    }
    if (err_prior_.size() > 0) {
        tempChi += err_prior_.norm();
    }
    tempChi *= 0.5;

    // 计算 rho
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    ulong size = Hessian_.cols();
    MatXX diag_hessian = MatXX::Zero(size, size);
    for (ulong i = 0; i < size; ++i) {
        diag_hessian(i, i) = Hessian_(i, i);
    }
    double scale = delta_x_.transpose() * (currentLambda_ * diag_hessian * delta_x_ + b_);
    scale += 1e-6;
    double rho = (currentChi_ - tempChi) / scale;

    // 更新 currentLambda----核心
    double epsilon = 0;
    double L_down = 9.0， L_up = 11.0;
    if (rho > epsilon && std::isfinite(tempChi)) {
        currentLambda_ = std::max(currentLambda_ / L_down, 1e-7);
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ = std::min(currentLambda_ * L_up, 1e7);
        return false;
    }
}