LM优化算法
求解 ( J T J + λ I ) δ x = b (J^T J + \lambda I) \delta x = b (JTJ+λI)δx=b
LM优化算法更新策略
问题描述
求解 e r r = [ 2 4 ] − [ x 1 2 x 2 2 ] err =\begin{bmatrix} 2 \\ 4 \end{bmatrix} - \begin{bmatrix} x_1^2 \\ x_2^2 \end{bmatrix} err=[24]−[x12x22]这样的一个问题
代码
#include<Eigen/Core>
#include<Eigen/Dense>
#include<iostream>
#include<vector>
#include<math.h>
// LM 算法 Nielsen更新策略
class LM
{
public:
LM() {}
~LM() {}
// 实现优化的部分
bool opt() {
Eigen::Vector2d trueX(2., 4.);
Eigen::Vector2d now(0.1, 0.2);// 从0. 0 优化
// step1 计算初始参数
Eigen::Matrix2d J;
J << -2. * now[0], 0., 0., -2. * now[1];
for (int i = 0; i < J.cols(); i++) {
Eigen::Matrix<double, 2, 1> coli = J.block(0, i, J.rows(), 1);
double _mu = coli.transpose() * coli;
mu = std::max(mu, _mu);
}
mu = tau * mu;
std::cout << "mu set value = " << mu << std::endl;// 计算tau = max J^T J 的对角线
// step2 迭代优化计算
for (int iter = 0; iter < mMaxIter; ++iter) {
// 求当前雅可比矩阵
J << -2. * now[0], 0., 0., -2. * now[1];
Eigen::Matrix2d JTJ = J.transpose() * J;
// 计算当前误差
Eigen::Vector2d target;
target << trueX[0] * trueX[0], trueX[1] * trueX[1];
Eigen::Vector2d NowValue(now[0] * now[0], now[1] * now[1]);
Eigen::Vector2d err = target - NowValue;
Eigen::Vector2d b = -J.transpose() * err;
Eigen::Vector2d deltaX;
Eigen::Matrix2d H = JTJ + mu * Eigen::Matrix2d::Identity();// 设置H矩阵
deltaX = H.ldlt().solve(b);// 求解 H deltaX = b
// 计算增量后误差
Eigen::Vector2d temp = now + deltaX;
temp << temp[0] * temp[0], temp[1] * temp[1];
Eigen::Vector2d errDeltaX = target - temp;
// mu更新策略
double temp_a = err.transpose() * err;
double temp_b = errDeltaX.transpose() * errDeltaX;
double fenzi = 0.5 * (temp_a - temp_b);
double fenmu = 0.5 * deltaX.transpose() * (mu * deltaX + b);
double rou = fenzi / fenmu;
if (rou > 0) {// 当前更新是好的接受更新
mu = mu * std::max(1. / 3., 1. - (2. * rou - 1.) * (2. * rou - 1.) * (2. * rou - 1.));
v = 2.;
now = now + deltaX;
}
else {// 增大阻尼拒绝更新
mu = mu * v;
v = 2. * v;
}
if (deltaX.norm() < 1e-6) break;// 如果增量太小跳出循环
std::cout << "iter = " << iter << " delta = " << deltaX.transpose() << " now = " << now.transpose() << std::endl;// 打印迭代中的数据
}
// 打印求解完毕的
std::cout << "opt finish" << std::endl;
std::cout << "opt result = " << now.transpose() << std::endl;
return true;
}
private:
int mMaxIter = 100;// 默认的最大迭代次数
double v = 2.;// 初始设置为2.
double mu = 1;// TODO: 先随便设置一个
double tau = 1* 1e6;// 1e-8 ~ 1 之间
};
int main(int argc, char** argv) {
LM lmer;
lmer.opt();
return 0;
}