基于C++ Eigen的列文伯格马夸尔特（LM）优化算法

#include "LM.h"
#include <opencv2/opencv.hpp>

int main()
{
	const double a = 1, b = 5, c = 2;
	double a_ = 0.9, b_ = 5.5, c_ = 2.2;

	LM::LevenbergMarquardt lm(a_, b_, c_);
	lm.SetParameters(1e-10, 1e-10, 100, true);

	const size_t N = 1000;
	cv::RNG rng(cv::getTickCount());

	for (size_t i = 0; i < N; i++)
	{
		double x = rng.uniform(0.0, 10.0);
		double y = exp(a * x * x + b * x + c) + rng.gaussian(5.0);
		lm.AddObservation(x, y);
	}

	lm.Slove();
	return 0;
}

#pragma once
#include <iostream>
#include <vector>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

namespace LM
{	
	class RunTime
	{
	public:
		void Stat();
		void Stop();
		double Duration();
	private:
		chrono::steady_clock::time_point tstat;
		chrono::steady_clock::time_point tend;
	};

	class LevenbergMarquardt
	{
	public:
		LevenbergMarquardt(double da, double db, double dc);
		void SetParameters(double sEpsilon1, double sEpsilin2, int sMaxIter, bool sIsout);
		void AddObservation(const double x, const double y);
		void CalcJ_fx();
		void CalcH_g();
		double GetCost();
		double F(double a, double b, double c);
		double L0_L(Eigen::Vector3d h);
		void Slove();
	public:
		double epsilon1, epsilon2;
		int MaxIter;
		std::vector<Eigen::Vector2d> obsPt;
		double a, b, c;
		bool isOut;
	private:			
		MatrixXd fx_;
		MatrixXd J_;
		Matrix3d H_;
		Vector3d g_;
	};
}

#include "LM.h"
#include <iomanip>

void LM::RunTime::Stat()
{
	tstat = chrono::steady_clock::now();
}

void LM::RunTime::Stop()
{
	tend = chrono::steady_clock::now();
}

double LM::RunTime::Duration()
{
	return chrono::duration_cast<chrono::duration<double>>(tend - tstat).count();
}

LM::LevenbergMarquardt::LevenbergMarquardt(double da, double db, double dc) :a(da), b(db), c(dc)
{
	epsilon1 = 1e-6;
	epsilon2 = 1e-6;
	MaxIter = 50;
	isOut = true;
}

void LM::LevenbergMarquardt::SetParameters(double sEpsilon1, double sEpsilon2, int sMaxIter, bool sIsout)
{
	epsilon1 = sEpsilon1;
	epsilon2 = sEpsilon2;
	MaxIter = sMaxIter;
	isOut = sIsout;
}

void LM::LevenbergMarquardt::AddObservation(const double x, const double y)
{
	obsPt.push_back(Eigen::Vector2d(x, y));
}

void LM::LevenbergMarquardt::CalcJ_fx()
{
	J_.resize(obsPt.size(), 3);
	fx_.resize(obsPt.size(), 1);

	for (int i = 0; i < obsPt.size(); i++)
	{
		Eigen::Vector2d obs = obsPt.at(i);
		const double x = obs(0);
		const double y = obs(1);
		double dfda = -x * x * exp(a * x * x + b * x + c);
		double dfdb = -x * exp(a * x * x + b * x + c);
		double dfdc = -exp(a * x * x + b * x + c);
		J_(i, 0) = dfda;
		J_(i, 1) = dfdb;
		J_(i, 2) = dfdc;
		fx_(i, 0) = y - exp(a * x * x + b * x + c);
	}
}

void LM::LevenbergMarquardt::CalcH_g()
{
	H_ = J_.transpose() * J_;
	g_ = - J_.transpose() * fx_;
}

double LM::LevenbergMarquardt::GetCost()
{
	MatrixXd cost = fx_.transpose() * fx_;
	return cost(0, 0);
}

double LM::LevenbergMarquardt::F(double da, double db, double dc)
{
	Eigen::VectorXd fx;
	fx.resize(obsPt.size(), 1);

	for (int i = 0; i < obsPt.size(); i++)
	{
		const Eigen::Vector2d obs = obsPt.at(i);
		const double x = obs(0);
		const double y = obs(1);
		fx(i) = y - exp(da * x * x + db * x + dc);
	}
	Eigen::MatrixXd F = 0.5 * fx.transpose() * fx;
	return F(0, 0);
}

double LM::LevenbergMarquardt::L0_L(Eigen::Vector3d h)
{
	/*Eigen::MatrixXd L = -fx_.transpose() * J_ * h - 0.5 * h.transpose() * J_ * J_.transpose() * h;*/

	Eigen::MatrixXd L = -h.transpose() * J_.transpose() * fx_ - 0.5 * h.transpose() * J_.transpose() * J_ * h;
	return L(0, 0);
}

void LM::LevenbergMarquardt::Slove()
{
	int k = 0;
	double vu = 2.0;
	CalcJ_fx();
	CalcH_g();
	bool found = (g_.lpNorm<Eigen::Infinity>() < epsilon1); //误差足够小
	std::vector<double> H_diag;
	for (int i = 0; i < 3; i++)
	{
		H_diag.push_back(H_(i, i));
	}
	auto max = std::max_element(H_diag.begin(), H_diag.end());
	double mu = *max;
	double tsum = 0;
	while (!found && k < MaxIter) // 误差足够小，增量足够小，达到迭代次数
	{
		k++;
		RunTime runtime;
		runtime.Stat();
		Eigen::Matrix3d G = H_ + mu * Eigen::Matrix3d::Identity();
		Eigen::Vector3d h = G.ldlt().solve(g_);

		if (h.norm() < epsilon2 * sqrt(a * a + b * b + c * c) + epsilon2) //增量足够小
		{
			found = true;
		}
		else
		{
			double a_new = a + h(0);
			double b_new = b + h(1);
			double c_new = c + h(2);
			double sig = (F(a, b, c) - F(a_new, b_new, c_new)) / L0_L(h);
			if (sig > 0)
			{
				a = a_new;
				b = b_new;
				c = c_new;
				CalcJ_fx();
				CalcH_g();
				found = (g_.lpNorm<Eigen::Infinity>() < epsilon1);
				mu = mu * std::max<double>(0.333, 1 - std::pow(2 * sig - 1, 3));
				vu = 2.0;
			}
			else
			{
				mu = mu * vu;
				vu *= 2;
			}
		}

		runtime.Stop();
		if (isOut)
		{
			std::cout << "Iter: " << std::left << std::setw(5) << k << std::left << std::setw(10)
				<< "Result: " << std::left << std::setw(10) << a << " " << std::left << std::setw(10) << b << " " << std::left << std::setw(10) << c << std::left << std::setw(10)
				<< "step: " << std::left << std::setw(15) << h.norm() << std::left << std::setw(10)
				<< "cost: " << std::left << std::setw(10) << GetCost() << std::left << std::setw(10)
				<< "time: " << std::left << std::setw(10) << (tsum += runtime.Duration()) << std::endl;
		}
	}

	if (found)
	{
		std::cout << "\nConverged\n\n";
	}
	else
	{
		std::cout << "\nDiverged\n\n";
	}
}