[基础知识点]LM算法

2024-06-18 18:52| 来源: 网络整理| 查看: 265

目录 1. 内容2. 代码2.1. 计算阻尼因子的初始值2.2. 阻尼因子的更新2.2.1 残差计算

1. 内容

在这里插入图片描述损失函数：其中 f i ( x ) f_i(x) fi(x)是残差函数，比如测量值和预测值之间的差.

定量分析，阻尼因子更新策略通过比例因子来确定的：在这里插入图片描述

其中：在这里插入图片描述这里采用的 μ 0 \mu_0 μ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 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();