LM优化算法

快乐得小萝卜

于 2023-07-08 16:10:21 发布

阅读量246

点赞数

分类专栏：杂七杂八的支线任务文章标签： c++ slam 算法

本文链接：https://blog.csdn.net/weixin_41738773/article/details/131613190

版权

杂七杂八的支线任务专栏收录该内容

4 篇文章 0 订阅

订阅专栏

LM优化算法

LM优化算法更新策略
问题描述
代码

求解 $(J^T J + \lambda I) \delta x = b$

LM优化算法更新策略

Nielsen

问题描述

求解 $=\begin{bmatrix} 2 \\ 4 \end{bmatrix} - \begin{bmatrix} x_1^2 \\ x_2^2 \end{bmatrix}$ 这样的一个问题

代码

#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;
}