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

代码实践"/>

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

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) - Fdouble 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 numint paraNum = params.rows();    //parameter numVectorXd obj = objF(input, output, params);MatrixXd Jac = Jacobin(input, output, params);  //jacobinVectorXd gradient = Jac.transpose() * obj;      //gradient//initial parameter tao v epsilon1 epsilon2double 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);  //jacobinVectorXd gradient = Jac.transpose() * obj;      //gradientif (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 parameterint total_data = 100;VectorXd input(total_data);VectorXd output(total_data);double A = 5, B = 1, C = 10, D = 2;//load observation datafor (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 parametersVectorXd params_gaussNewton(num_params);//init gaussparams_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：
2：
3：