LM优化算法_lm算法内参计算

LM优化算法_lm算法内参计算LM算法理论知识梯度下降高斯牛顿Levenberg–Marquardt算法框架算法的整体流程求解器update流程说明算法实现头文件cpp算法调用LM优化算法,是一种非线性优化算法,其可以看作是梯度下降和高斯牛顿法的结合。综合了梯度下降对初值不敏感和高斯牛顿在最优值附近收敛速度快的特点。本人非数学专业,且对算法理解可能不到位,详细的算法推导及各个优化算法之间的关系,非常推荐看**《METHODSFORNON-LINEARLEASTSQUARESPROBLEMS》**,其介绍更详细也更专业。

大家好,又见面了,我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新Idea2022.1教程亲测有效,一键激活。

Jetbrains全系列IDE稳定放心使用

LM优化算法,是一种非线性优化算法,其可以看作是梯度下降和高斯牛顿法的结合。综合了梯度下降对初值不敏感和高斯牛顿在最优值附近收敛速度快的特点。

本人非数学专业,且对算法理解可能不到位,详细的算法推导及各个优化算法之间的关系,非常推荐看METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS ,其介绍更详细也更专业。

以下简要介绍LM算法,然后给出opencv中有关的实现。

本文代码也是来自opencv相关代码,位置在opencv-master\modules\calib3d\src\calibration.cpp\cvFindExtrinsicCameraParams2()

当然,有关LM算法的实现,在opencv的usac模块中有更为正式的实现,在该模块中,对许多算法进行了集成和优化,后续再进行研究。

理论知识

对于一个最小二乘问题,有:

F ( x ) = 1 2 ∑ i = 1 m ( f i ( x ) ) 2 f i ( x ) 为 残 差 F(x) = \frac12\sum_{i=1}^m(f_i(x))^2\\ f_i(x) 为残差 F(x)=21i=1m(fi(x))2fi(x)
我们的目的是求解残差f(x)的一组系数,使目标函数(或代价函数)F(x)达到最小值。通常的做法是,给定一个初值x0,优化算法不断的迭代,产生x1,x2,…,最终收敛于X。

因此,在这个收敛的过程中,我们需要两样东西,即方向h和步长α
x k + 1 = x k + α h d h d 即 为 收 敛 的 方 向 α 即 为 收 敛 的 步 长 x_{k+1} = x_k + \alpha h_d \\ h_d即为收敛的方向 \\ \alpha即为收敛的步长 xk+1=xk+αhdhdα
所以,我们看到的梯度下降、高斯牛顿以及本文说的LM算法,其思想都是一致的。

梯度下降

这里给出梯度下降的公式,有:
x k + 1 = x k − α F ˙ ( x ) F ˙ ( x ) 为 F ( x ) 一 阶 导 数 x_{k+1} = x_k – \alpha \dot{F}(x) \\ \dot{F}(x)为F(x)一阶导数 xk+1=xkαF˙(x)F˙(x)F(x)

高斯牛顿

高斯牛顿是在牛顿迭代的基础上,用雅可比矩阵的平方来近似Hessian矩阵,求解起来将会更加高效(大多数情况下,Hessian矩阵太难求)。这样做的缺点就是,要求迭代初值必须在最优解附近,否则以上假设将不成立。这里给出其公式:
x k + 1 = x k + α h g n J T J h g n = − J T f x_{k+1} = x_k + \alpha h_{gn} \\ J^TJh_{gn} = -J^Tf \\ xk+1=xk+αhgnJTJhgn=JTf

Levenberg–Marquardt

给出其公式:
x k + 1 = x k + α h l m ( J T J + μ I ) h l m = − J T f x_{k+1} = x_k + \alpha h_{lm} \\ (J^TJ + \mu I)h_{lm} = -J^Tf xk+1=xk+αhlm(JTJ+μI)hlm=JTf
在有些情况下,JtJ不可逆,导致高斯牛顿无法使用。LM通过将JtJ加上一个μI(I为单位阵),保证了正规方程的左侧一定可逆。

在算法实际的应用中,通过调节μ的大小,可以使算法在梯度下降和高斯牛顿两者之间切换,如:

  • 使用较大的μ值,算法可以看作梯度下降,适合当前优化位置距离最优解较远的情况
  • 使用较小的μ值,算法可以看作高斯牛顿,适合当前优化位置接近最优解。

算法框架

LM算法通过求解正规方程,得到每次迭代的步长和方向。因此,算法的主要目的是求解正规方程左右侧的项,然后通过SVD分解即可得到参数更新的步长及方向,即:

  • JtJ – 雅可比矩阵的转置与其自己相乘
  • JErr – 雅可比矩阵的转置与残差矩阵(向量)相乘

算法框架在外部计算雅可比矩阵和残差矩阵,然后在算法内部通过求解正规方程,得到每次参数更新的步长及方向。

然后利用更新的参数,在外部计算残差,然后判断残差是否朝着收敛方向进行。

算法的整体流程

在这里插入图片描述

求解器update流程

求解器updata()内部根据不同的状态进行相应的计算,具体流程如下:

在这里插入图片描述

说明

  • J表示雅可比矩阵
  • err表示残差矩阵
  • 求解器内外同步表示求解器内部和外部相应的参数指向相同,便于在求解器外部进行雅可比矩阵和残差矩阵的计算

算法实现

算法的实现主要步骤都体现在上述流程图中。

头文件

#pragma once

#include <iostream>

#include "opencv2/core/types_c.h"
#include "opencv2/core/core_c.h"

using namespace cv;

struct Iteration
{
	int iters = 0;
	TermCriteria criteria = TermCriteria(0, 0, 0);
	int lamda_lg10 = 0;

};

class LevMarq
{
public:
	LevMarq();
	LevMarq(int nparams, int nerrs,
		TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, DBL_EPSILON), bool completeSymmFlag = false);
	~LevMarq();

	void clear();

	void initParam(Mat params);

	void init(int nparams, int nerrs,
		TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, DBL_EPSILON), bool completeSymmFlag = false);

	bool update(const CvMat*& params, CvMat*& J, CvMat*& err);

	void step();

private:
	double m_lambda_lg10;

	enum {
		DONE,
		START,
		CALC_J,
		CHECK_ERR
	};

	int m_state;
	TermCriteria m_criteria;
	int m_iters;
	bool m_complete_symm_flag;
	double m_err_norm, m_prev_err_norm;
	int m_solver_method;

	const double m_log10 = log(10.);

	Ptr<CvMat> m_mask;
	Ptr<CvMat> m_params;
	Ptr<CvMat> m_prev_params;

	Ptr<CvMat> m_JtJ;
	Ptr<CvMat> m_JtErr;
	Ptr<CvMat> m_J;
	Ptr<CvMat> m_Err;

	Ptr<CvMat> m_avl_JtJ;
	Ptr<CvMat> m_avl_JtErr;
	Ptr<CvMat> m_avl_params;
};

cpp

#include "LevMarq.h"

static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
	const std::vector<uchar>& rows) {
	int nonzeros_cols = cv::countNonZero(cols);
	cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);

	for (int i = 0, j = 0; i < (int)cols.size(); i++)
	{
		if (cols[i])
		{
			src.col(i).copyTo(tmp.col(j++));
		}
	}

	int nonzeros_rows = cv::countNonZero(rows);
	dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
	for (int i = 0, j = 0; i < (int)rows.size(); i++)
	{
		if (rows[i])
		{
			tmp.row(i).copyTo(dst.row(j++));
		}
	}
}

LevMarq::LevMarq()
{
	// only do some init
	m_lambda_lg10 = 0;
	m_state = DONE;
	m_criteria = TermCriteria(0, 0, 0);
	m_iters = 0;
	m_complete_symm_flag = false;
	m_err_norm = m_prev_err_norm = DBL_MAX;
	m_solver_method = DECOMP_SVD;
}

LevMarq::LevMarq(int nparams, int nerrs, TermCriteria criteria, bool completeSymmFlag)
{
	init(nparams, nerrs, criteria, completeSymmFlag);
}

LevMarq::~LevMarq()
{
	clear();
}

void LevMarq::clear()
{
	m_mask.release();
	m_params.release();
	m_prev_params.release();

	m_JtJ.release();
	m_JtErr.release();
	m_J.release();
	m_Err.release();

	m_avl_JtJ.release();
	m_avl_JtErr.release();
	m_avl_params.release();
}

void LevMarq::initParam(Mat params)
{
	if (params.empty())
		return;

	double* data = m_params->data.db;
	assert(params.cols == 1, "params dim must be N*1");
	for (int i = 0; i < params.rows; i++)
	{
		data[i] = params.at<double>(i, 0);
	}
}

/// <summary>
/// create some mat used for store related
/// </summary>
/// <param name="nparams">the nums of parameters to be optimized</param>
/// <param name="nerrs">the nums of residual function</param>
/// <param name="criteria">condition of convergence </param>
/// <param name="completeSymmFlag"></param>
void LevMarq::init(int nparams, int nerrs, TermCriteria criteria, bool completeSymmFlag)
{
	if (!m_params || m_params->rows != nparams || nerrs != (m_Err ? m_Err->rows : 0))
		clear();

	m_mask.reset(cvCreateMat(nparams, 1, CV_8U));
	cvSet(m_mask, cvScalarAll(1));
	m_prev_params.reset(cvCreateMat(nparams, 1, CV_64F));
	m_params.reset(cvCreateMat(nparams, 1, CV_64F));
	m_JtJ.reset(cvCreateMat(nparams, nparams, CV_64F));
	m_JtErr.reset(cvCreateMat(nparams, 1, CV_64F));

	if (nerrs > 0)
	{
		m_J.reset(cvCreateMat(nerrs, nparams, CV_64F));
		m_Err.reset(cvCreateMat(nerrs, 1, CV_64F));
	}

	m_err_norm = m_prev_err_norm = DBL_MAX;
	m_lambda_lg10 = -3;
	m_criteria = criteria;
	if (m_criteria.type & TermCriteria::COUNT)
	{
		m_criteria.maxCount = MIN(MAX(m_criteria.maxCount, 1), 100000);
	}
	else
	{
		m_criteria.maxCount = 30;
	}
	if (m_criteria.type & TermCriteria::EPS)
	{
		m_criteria.epsilon = MAX(m_criteria.epsilon, 0);
	}
	else
	{
		m_criteria.epsilon = DBL_EPSILON;
	}
	m_state = START;
	m_iters = 0;
	m_complete_symm_flag = completeSymmFlag;
	m_solver_method = DECOMP_SVD;
}

bool LevMarq::update(const CvMat*& params, CvMat*& J, CvMat*& err)
{
	J = err = 0;
	assert(!m_Err.empty());
	if (m_state == DONE)
	{
		params = m_params;
		return false;
	}
	if (m_state == START)
	{
		params = m_params;
		cvZero(m_J);
		cvZero(m_Err);
		J = m_J;
		err = m_Err;
		m_state = CALC_J;
		return true;
	}
	if (m_state == CALC_J)
	{
		cvMulTransposed(m_J, m_JtJ, 1);
		cvGEMM(m_J, m_Err, 1, 0, 0, m_JtErr, CV_GEMM_A_T);
		cvCopy(m_params, m_prev_params);
		step();

		if (m_iters == 0)
		{
			m_prev_err_norm = cvNorm(m_Err, 0, CV_L2);
		}
		params = m_params;
		cvZero(m_Err);
		err = m_Err;
		m_state = CHECK_ERR;
		return true;
	}

	assert(m_state == CHECK_ERR);
	m_err_norm = cvNorm(m_Err, 0, CV_L2);
	if (m_err_norm > m_prev_err_norm)
	{
		if (++m_lambda_lg10 <= 16)
		{
			step();

			params = m_params;
			cvZero(m_Err);
			err = m_Err;
			m_state = CHECK_ERR;
			return true;
		}
	}

	m_lambda_lg10 = MAX(m_lambda_lg10 - 1, -16);
	if (++m_iters >= m_criteria.maxCount ||
		cvNorm(m_params, m_prev_params, CV_RELATIVE_L2) <= m_criteria.epsilon)
	{
		std::cout << "criteria.epsilon: " << cvNorm(m_params, m_prev_params, CV_RELATIVE_L2) << std::endl;
		params = m_params;
		m_state = DONE;
		return true;
	}
	params = m_params;
	m_prev_err_norm = m_err_norm;
	cvZero(m_J);
	cvZero(m_Err);
	J = m_J;
	err = m_Err;
	m_state = CALC_J;
	return true;
}

void LevMarq::step()
{
	double miu = exp(m_lambda_lg10 * m_log10);
	int _nparams = m_params->rows;

	Mat _JtJ = cvarrToMat(m_JtJ);
	Mat _mask = cvarrToMat(m_mask);

	int _avl_nparams = countNonZero(_mask);
	if (!m_avl_JtJ || m_avl_JtJ->rows != _avl_nparams)
	{
		m_avl_JtJ.reset(cvCreateMat(_avl_nparams, _avl_nparams, CV_64F));
		m_avl_JtErr.reset(cvCreateMat(_avl_nparams, 1, CV_64F));
		m_avl_params.reset(cvCreateMat(_avl_nparams, 1, CV_64F));
	}

	Mat _avl_JtJ = cvarrToMat(m_avl_JtJ);
	Mat _avl_JtErr = cvarrToMat(m_avl_JtErr);
	Mat_<double> _avl_params = cvarrToMat(m_avl_params);

	subMatrix(cvarrToMat(m_JtErr), _avl_JtErr, std::vector<uchar>(1, 1), _mask);
	subMatrix(_JtJ, _avl_JtJ, _mask, _mask);

	if (!m_Err)
		completeSymm(_avl_JtErr, m_complete_symm_flag);

	_avl_JtJ.diag() *= 1.0 + miu;
	std::cout << miu << std::endl;
	solve(_avl_JtJ, _avl_JtErr, _avl_params, m_solver_method);

	int j = 0;
	for (int i = 0; i < _nparams; i++)
	{
		m_params->data.db[i] = m_prev_params->data.db[i] - (m_mask->data.ptr[i] ? _avl_params(j++) : 0);
	}
}

算法调用

double param[opt_num] = { 0,0 }; // k1 p1
CvMat _param = cvMat(opt_num, 1, CV_64F, param);
CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 10000, DBL_EPSILON);
myCvLevMarq solver(opt_num, total, criteria);
solver.initParam(distCoffes.rowRange(Range(0,2)));

int cnt = 0;
for (;;)
{
    const CvMat* __param = 0;
    CvMat* J = 0, * err = 0;
    bool proceed = solver.update(__param, J, err);
    cvCopy(__param, &_param); // 将优化的变量保存下来
    if (!proceed || !err)
        break;

    if (J)
    {
        // 计算雅可比矩阵和残差矩阵
    }
    else
    {
        //残差矩阵
    }
}
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/187323.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)
blank

相关推荐

  • Win10黑屏的时候显示时钟怎么设置「建议收藏」

    Win10黑屏的时候显示时钟怎么设置「建议收藏」Win10黑屏的时候显示时钟怎么设置?其实方法很简单,那就是让电脑进入屏幕保护程序的黑屏模式而不是睡眠状态,这样电脑就可以显示时间啦

  • hyper-v虚拟机安装xp系统网络不通_hyper-v转换vmware

    hyper-v虚拟机安装xp系统网络不通_hyper-v转换vmware下载一个ISO格式的XP系统镜像,把ISO文件设置为虚拟机的光驱,启动虚拟机,会自动从ISO镜像文件启动,创建虚拟机,创建虚拟磁盘VHD,然后加入启动项。打开本系统的磁盘管理,对虚拟磁盘进行格式化,并设置为活动分区(有一个就行)然后启动虚拟机,给虚拟机安装系统,就可以了。我是启动虚拟机后进入PE,然后选择Ghost32装的。如果启动虚拟机鼠标不能动,就点“

  • mybatis:Error parsing SQL Mapper Configuration. Cause: java.io.IOException: Could not find resource[通俗易懂]

    mybatis:Error parsing SQL Mapper Configuration. Cause: java.io.IOException: Could not find resource[通俗易懂]整天写业务逻辑代码,但偶尔整个配置搞死人(根基不牢),有些细节知识还是欠缺,遇到问题总是搞的很烦躁,通过这篇博文将自己遗忘的知识总结起来。_____________________________________________________________________________________________先贴错误:看起来很简单,按照错误排查一下,就ok,但硬生生搞了半天,还搞的烦躁,这么简单,咋找不到问题呢。分析:看错误可知,找不到mapper文件。查看myb.

  • linux中kill命令详解_linux kill函数

    linux中kill命令详解_linux kill函数linuxkill命令详解一、命令格式:kill[参数][进程号]二、命令功能:发送指定的信号到相应进程。不指定型号将发送SIGTERM(15)终止指定进程。如果任无法终止该程序可用“-KILL”参数,其发送的信号为SIGKILL(9),将强制结束进程,使用ps命令或者jobs命令可以查看进程号。root用户将影响用户的进程,非root用户只能影响自己的进程。三、命令参数:-l信号,若果不加信号的编号参数,则使用“-l”参数会列出全部的信号名称-a当处理当前进程时,不

    2022年10月29日
  • 建立排序二叉树并中序遍历

    建立排序二叉树并中序遍历分析:中序遍历也叫中根遍历,顾名思义是把根节点放在中间来遍历,其遍历顺序为左子节点–>根节点–>右子节点。方法一:#includeusingnamespacestd;structnode//二叉树结点结构{intdata;node*left;//右子树结点指针n

  • RabbitMQ(五):Exchange交换器–topic

    RabbitMQ(五):Exchange交换器–topic

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号