Dogleg狗腿法详细推导+c++代码实践

一、理论推导

通过下式求解狗腿法的步长和方向。

对应于下图情况

其中alpha的求解已经在式（3）中推导出来，还剩下beta没有求解，下面为beta的求解方案。



最后通过roi的值，来更新新的点

二、代码实践

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <iomanip>
#include <math.h>
using namespace std;
using namespace Eigen;

const double DERIV_STEP = 1e-5;
const int MAX_ITER = 100;

#define max(a,b) (((a)>(b))?(a):(b))

double func(const VectorXd& input, const VectorXd& output, const VectorXd& params, double objIndex)
{
	// obj = A * sin(Bx) + C * cos(D*x) - F
	double x1 = params(0);
	double x2 = params(1);
	double x3 = params(2);
	double x4 = params(3);

	double t = input(objIndex);
	double f = output(objIndex);

	return x1 * sin(x2 * t) + x3 * cos(x4 * t) - f;
}

//return vector make up of func() element.
VectorXd objF(const VectorXd& input, const VectorXd& output, const VectorXd& params)
{
	VectorXd obj(input.rows());
	for (int i = 0; i < input.rows(); i++)
		obj(i) = func(input, output, params, i);

	return obj;
}

//F = (f ^t * f)/2
double Func(const VectorXd& obj)
{
	//平方和，所有误差的平方和
	return obj.squaredNorm() / 2;
}

double Deriv(const VectorXd& input, const VectorXd& output, int objIndex, const VectorXd& params,
	int paraIndex)
{
	VectorXd para1 = params;
	VectorXd para2 = params;

	para1(paraIndex) -= DERIV_STEP;
	para2(paraIndex) += DERIV_STEP;

	double obj1 = func(input, output, para1, objIndex);
	double obj2 = func(input, output, para2, objIndex);

	return (obj2 - obj1) / (2 * DERIV_STEP);
}

MatrixXd Jacobin(const VectorXd& input, const VectorXd& output, const VectorXd& params)
{
	int rowNum = input.rows();
	int colNum = params.rows();

	MatrixXd Jac(rowNum, colNum);

	for (int i = 0; i < rowNum; i++)
	{
		for (int j = 0; j < colNum; j++)
		{
			Jac(i, j) = Deriv(input, output, i, params, j);
		}
	}
	return Jac;
}

void dogLeg(const VectorXd& input, const VectorXd& output, VectorXd& params)
{
	int errNum = input.rows();      //error num
	int paraNum = params.rows();    //parameter num

	VectorXd obj = objF(input, output, params);
	MatrixXd Jac = Jacobin(input, output, params);  //jacobin
	VectorXd gradient = Jac.transpose() * obj;      //gradient

	//initial parameter tao v epsilon1 epsilon2
	double eps1 = 1e-12, eps2 = 1e-12, eps3 = 1e-12;
	double radius = 1.0;

	bool found = obj.norm() <= eps3 || gradient.norm() <= eps1;
	if (found) return;

	double last_sum = 0;
	int iterCnt = 0;
	while (iterCnt < MAX_ITER)
	{
		VectorXd obj = objF(input, output, params);
		MatrixXd Jac = Jacobin(input, output, params);  //jacobin
		VectorXd gradient = Jac.transpose() * obj;      //gradient

		if (gradient.norm() <= eps1)
		{
			cout << "stop F'(x) = g(x) = 0 for a global minimizer optimizer." << endl;
			break;
		}
		if (obj.norm() <= eps3)
		{
			cout << "stop f(x) = 0 for f(x) is so small";
			break;
		}

		//compute how far go along stepest descent direction.
		double alpha = gradient.squaredNorm() / (Jac * gradient).squaredNorm();
		//compute gauss newton step and stepest descent step.
		VectorXd stepest_descent = -alpha * gradient;
		VectorXd gauss_newton = (Jac.transpose() * Jac).inverse() * Jac.transpose() * obj * (-1);

		double beta = 0;

		//compute dog-leg step.
		VectorXd dog_leg(params.rows());
		if (gauss_newton.norm() <= radius)
			dog_leg = gauss_newton;
		else if (alpha * stepest_descent.norm() >= radius)
			dog_leg = (radius / stepest_descent.norm()) * stepest_descent;
		else
		{
			VectorXd a = alpha * stepest_descent;
			VectorXd b = gauss_newton;
			double c = a.transpose() * (b - a);
			if (c <= 0) {
				beta = (sqrt(c*c + (b - a).squaredNorm()*(radius*radius - a.squaredNorm())) - c) / (b - a).squaredNorm();
			}
			else {
				beta = (radius*radius - a.squaredNorm()) / (sqrt(c*c + (b - a).squaredNorm()*(radius*radius - a.squaredNorm())) - c);
			}

			dog_leg = alpha * stepest_descent + beta * (gauss_newton - alpha * stepest_descent);

		}

		cout << "dog-leg: " << endl << dog_leg << endl;

		if (dog_leg.norm() <= eps2 * (params.norm() + eps2))
		{
			cout << "stop because change in x is small" << endl;
			break;
		}

		VectorXd new_params(params.rows());
		new_params = params + dog_leg;

		cout << "new parameter is: " << endl << new_params << endl;

		//compute f(x)
		obj = objF(input, output, params);
		//compute f(x_new)
		VectorXd obj_new = objF(input, output, new_params);

		//compute delta F = F(x) - F(x_new)
		double deltaF = Func(obj) - Func(obj_new);

		//compute delat L =L(0)-L(dog_leg)
		double deltaL = 0;
		if (gauss_newton.norm() <= radius)
			deltaL = Func(obj);
		else if (alpha * stepest_descent.norm() >= radius)
			deltaL = radius * (2 * alpha*gradient.norm() - radius) / (2.0*alpha);
		else
		{
			VectorXd a = alpha * stepest_descent;
			VectorXd b = gauss_newton;
			double c = a.transpose() * (b - a);
			if (c <= 0) {
				beta = (sqrt(c*c + (b - a).squaredNorm()*(radius*radius - a.squaredNorm())) - c) / (b - a).squaredNorm();
			}
			else {
				beta = (radius*radius - a.squaredNorm()) / (sqrt(c*c + (b - a).squaredNorm()*(radius*radius - a.squaredNorm())) - c);
			}

			deltaL = alpha * (1 - beta)*(1 - beta)*gradient.squaredNorm() / 2.0 + beta * (2.0 - beta)*Func(obj);

		}

		double roi = deltaF / deltaL;
		if (roi > 0)
		{
			params = new_params;
		}
		if (roi > 0.75)
		{
			radius = max(radius, 3.0 * dog_leg.norm());
		}
		else if (roi < 0.25)
		{
			radius = radius / 2.0;
			if (radius <= eps2 * (params.norm() + eps2))
			{
				cout << "trust region radius is too small." << endl;
				break;
			}
		}

		cout << "roi: " << roi << " dog-leg norm: " << dog_leg.norm() << endl;
		cout << "radius: " << radius << endl;

		iterCnt++;
		cout << "Iterator " << iterCnt << " times" << endl << endl;
	}
}

int main(int argc, char* argv[])
{
	// obj = A * sin(Bx) + C * cos(D*x) - F
	//there are 4 parameter: A, B, C, D.
	int num_params = 4;

	//generate random data using these parameter
	int total_data = 100;

	VectorXd input(total_data);
	VectorXd output(total_data);

	double A = 5, B = 1, C = 10, D = 2;
	//load observation data
	for (int i = 0; i < total_data; i++)
	{
		//generate a random variable [-10 10]
		double x = 20.0 * ((rand() % 1000) / 1000.0) - 10.0;
		double deltaY = 2.0 * (rand() % 1000) / 1000.0;
		double y = A * sin(B*x) + C * cos(D*x) + deltaY;

		input(i) = x;
		output(i) = y;
	}

	//gauss the parameters
	VectorXd params_gaussNewton(num_params);
	//init gauss
	params_gaussNewton << 3.6, 1.4, 6.2, 1.7;

	VectorXd params_dogLeg = params_gaussNewton;

	dogLeg(input, output, params_dogLeg);

	cout << "dog-leg parameter: " << endl << params_dogLeg << endl << endl << endl;
}

参考

1：https://blog.csdn.net/stihy/article/details/52737723
2：https://blog.csdn.net/qq_35590091/article/details/94628887
3：https://zhuanlan.zhihu.com/p/94589862