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

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 基本用法

  1. 标记并行区域 :使用#pragma omp parallel指令标记需要并行执行的代码区域。

    cpp 复制代码
    #pragma omp parallel
    {
        // 并行执行的代码
    }
  2. 控制线程数量 :使用num_threads子句控制线程数量。

    cpp 复制代码
    #pragma omp parallel num_threads(4)
    {
        // 使用4个线程并行执行
    }
  3. 并行化循环 :使用#pragma omp for指令并行化循环。

    cpp 复制代码
    #pragma omp parallel for
    for (int i = 0; i < n; i++) {
        // 循环体
    }
  4. 共享和私有变量 :使用sharedprivate子句来指定共享和私有变量。

    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其实就是之前代码中的写法,取海塞矩阵对角线元素最大值,乘以一个常数后作为初始λ ![在这里插入图片描述](https://file.jishuzhan.net/article/1779381872837529601/ffe45924e49f150d186dec420193d68d.webp) > 之前方法 ```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的减少,则接受该更新量并对参数进行更新;否则,增加阻尼参数λ,并进入下一次迭代。 ​ ![在这里插入图片描述](https://file.jishuzhan.net/article/1779381872837529601/f48dcbaa02d0d1cccac7bd967c9f7e85.webp) > 对于公式中参数L_up 和L_down,论文作者给出一个参考值(针对方法1) ![在这里插入图片描述](https://file.jishuzhan.net/article/1779381872837529601/5e670da4b3a6cb10d4d4b183e053572c.webp) > 这里设置 ϵ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; } } ```