303 lines
6.9 KiB
C++
303 lines
6.9 KiB
C++
#include "optimization.h"
|
|
#include "math.h"
|
|
#include <iostream>
|
|
#include <algorithm> //max
|
|
using namespace std; //max
|
|
|
|
|
|
void Optimization::Init(fx_module *module) {
|
|
module_=module;
|
|
}
|
|
|
|
|
|
void Optimization::ComputeDoglegDirection(double radius,
|
|
Vector &gradient,
|
|
Matrix &hessian,
|
|
Vector *p,
|
|
double *delta_m) {
|
|
//check positive definiteness of the hessian
|
|
Matrix inverse_hessian;
|
|
if( !PASSED(la::InverseInit(hessian, &inverse_hessian)) ) {
|
|
|
|
cout<<"Hessian matrix is not invertible"<<endl;
|
|
cout<<"Find Cauchy Point..."<<endl;
|
|
|
|
//if hessian is indefinite->use cauchy point
|
|
double gHg;
|
|
|
|
Matrix transpose_hessian;
|
|
la::TransposeInit(hessian, &transpose_hessian);
|
|
|
|
Vector temp2; //H*g
|
|
la::MulInit(hessian, gradient, &temp2);
|
|
gHg=la::Dot(temp2, gradient);
|
|
|
|
double gradient_norm=sqrt(la::Dot(gradient, gradient));
|
|
|
|
|
|
if(gHg<=0){
|
|
//double gradient_norm=sqrt(la::Dot(gradient, gradient));
|
|
//p=-(radius/gradient_norm)*gradient;
|
|
la::ScaleInit(-1*radius/gradient_norm, gradient, p);
|
|
}
|
|
else{
|
|
double zeta=0;
|
|
zeta=std::min(pow(gradient_norm, 3)/(radius*gHg), 1.0);
|
|
la::ScaleInit(-1*zeta*radius/gradient_norm, gradient, p);
|
|
|
|
}
|
|
|
|
|
|
} //if
|
|
else { //if hessian matrix is positive definite
|
|
//la::InverseInit(hessian, &inverse_hessian);
|
|
Vector p_b;
|
|
//p_b= - (hessian)^-1 * g
|
|
la::MulInit(inverse_hessian, gradient, &p_b);
|
|
la::Scale(-1.0, &p_b);
|
|
|
|
double p_b_norm;
|
|
//p_b_norm=la::Dot(p_b, p_b);
|
|
p_b_norm=sqrt(la::Dot(p_b, p_b));
|
|
|
|
if(radius>=p_b_norm){
|
|
p->Copy(p_b);
|
|
}
|
|
else{
|
|
//g'*H*g = (H*g)'g
|
|
double gHg;
|
|
|
|
Matrix transpose_hessian;
|
|
la::TransposeInit(hessian, &transpose_hessian);
|
|
|
|
/*
|
|
//check whether hessian is symmetric
|
|
double cnt=0;
|
|
for(index_t i=0; i<hessian.n_rows(); i++) {
|
|
for(index_t j=0; j<hessian.n_cols(); j++) {
|
|
if(hessian.get(i,j) != transpose_hessian.get(i,j)){
|
|
cnt+=1;
|
|
} //if
|
|
} //j
|
|
} //i
|
|
if( cnt !=0 ) {
|
|
NOTIFY("Hessian matrix is NOT symmetric.");
|
|
}
|
|
*/
|
|
|
|
Vector temp1; //H*g
|
|
la::MulInit(hessian, gradient, &temp1);
|
|
gHg=la::Dot(temp1, gradient);
|
|
|
|
Vector p_u;
|
|
//p_u= -(g'g/g'Hg)*g
|
|
double p_u_norm;
|
|
la::ScaleInit(-1*la::Dot(gradient, gradient)/gHg, gradient, &p_u);
|
|
p_u_norm=sqrt(la::Dot(p_u, p_u));
|
|
|
|
if( p_u_norm>=radius ) { //p=radius/p_u_norm * p_u)
|
|
la::ScaleInit(radius/p_u_norm, p_u, p);
|
|
} //if
|
|
else{ //combination of p_u and p_b
|
|
//solve the quadratic equation ||p_u-zeta(p_b-p_u)||^2=radius^2
|
|
Vector diff; //p_b-p_u
|
|
la::SubInit(p_u, p_b, &diff);
|
|
double a=la::Dot(diff, diff);
|
|
Vector diff2; //2p_u-p_b
|
|
la::ScaleInit(2, p_u, &diff2);
|
|
la::SubFrom(p_b, &diff2);
|
|
double b=la::Dot(diff, diff2);
|
|
double c=la::Dot(diff2, diff2)-math::Sqr(radius);
|
|
|
|
DEBUG_ASSERT_MSG(b*b*-4*a*c>0,
|
|
"(Dogleg)Discriminant is negative. Fail to get the solution zeta.");
|
|
|
|
double sqrt_discriminant=sqrt(b*b-4*a*c);
|
|
double zeta1=(-b+sqrt_discriminant)/(2*a);
|
|
double zeta2=(-b-sqrt_discriminant)/(2*a);
|
|
double zeta=-1;
|
|
|
|
if( (zeta1<2)&&(zeta1>0)&&(zeta2<2)&&(zeta2>0)){
|
|
zeta=max(zeta1, zeta2);
|
|
}
|
|
else if( (zeta1<2)&&(zeta1>0) ){
|
|
zeta=zeta1;
|
|
}
|
|
else if((zeta2<2)&&(zeta2>0)){
|
|
zeta=zeta2;
|
|
}
|
|
else{
|
|
DEBUG_ASSERT_MSG((zeta>0), "Fail to get zeta");
|
|
}
|
|
|
|
if(zeta<=1){
|
|
la::ScaleInit(zeta, p_u, p);
|
|
}
|
|
else{
|
|
Vector temp; //(zeta-1)*(p_b-p_u)
|
|
la::ScaleInit((zeta-1), diff, &temp);
|
|
la::AddInit(p_u, temp, p);
|
|
} //else
|
|
|
|
} //else
|
|
}
|
|
}
|
|
|
|
//delta_m calculation -g'p-0.5*p'Hp=-g'p-0.5*(Hp)'p
|
|
Vector temp3; //Hp
|
|
la::MulInit(hessian, *p, &temp3);
|
|
double pHp=0;;
|
|
pHp=la::Dot(temp3, *p);
|
|
|
|
*delta_m=-1*(la::Dot(gradient, *p))-0.5*(pHp);
|
|
|
|
}
|
|
|
|
|
|
|
|
void Optimization::ComputeSteihaugDirection(double radius,
|
|
Vector &gradient,
|
|
Matrix &hessian,
|
|
Vector *p,
|
|
double *delta_m) {
|
|
//"Numerical optimization" p.171 CG-Steihaug
|
|
//Truncated conjugated gradient algorithm implementation
|
|
//"Trust_Region Methods", pp. 202-207
|
|
//
|
|
Vector z;
|
|
z.Init(gradient.length());
|
|
z.SetZero(); //z_0=0;
|
|
|
|
Vector r;
|
|
r.Alias(gradient); //r_0=gradient
|
|
|
|
Vector old_r;
|
|
old_r.Init(gradient.length());
|
|
old_r.SetZero();
|
|
|
|
Vector d;
|
|
la::ScaleInit(-1.0, r, &d);
|
|
|
|
//double ZERO_EPS=1e-9;
|
|
double r0_norm;
|
|
r0_norm=sqrt(la::Dot(r,r));
|
|
//Define epsilon
|
|
double e=sqrt(r0_norm);
|
|
if(e>0.1){
|
|
e=0.1;
|
|
}
|
|
|
|
Vector temp1; //Hd
|
|
temp1.Init(gradient.length());
|
|
|
|
Vector temp2; //alpha*d
|
|
temp2.Init(gradient.length());
|
|
|
|
Vector temp3; //Hd
|
|
temp3.Init(gradient.length());
|
|
|
|
Vector temp4; //beta*d
|
|
temp4.Init(gradient.length());
|
|
|
|
double cnt=0;
|
|
while(1){
|
|
cnt+=1;
|
|
if(cnt>150){
|
|
NOTIFY("Exceeded the maximum iteration for SteihaugDirection");
|
|
break;
|
|
}
|
|
//Calculate d'Hd=(Hd)'d
|
|
//Vector temp1; //Hd
|
|
//temp1.Init(gradient.length());
|
|
la::MulOverwrite(hessian, d, &temp1);
|
|
double dHd;
|
|
dHd=la::Dot(temp1, d);
|
|
|
|
if(dHd<=0) {
|
|
//find zeta ||p_k||=radius
|
|
double a=la::Dot(d,d);
|
|
double b=2*la::Dot(z,d);
|
|
double c=la::Dot(z,z)-pow(radius,2);
|
|
DEBUG_ASSERT_MSG(b*b*-4*a*c>0,
|
|
"Discriminant is negative. Fail to get the solution zeta.");
|
|
double sqrt_discriminant=sqrt(b*b-4*a*c);
|
|
double zeta=(-b+sqrt_discriminant)/(2*a);
|
|
|
|
//p=z+zeta*d
|
|
la::ScaleInit(zeta, d, p);
|
|
la::AddTo(z, p);
|
|
break;
|
|
}
|
|
|
|
Vector z_next;
|
|
z_next.Init(z.length());
|
|
|
|
double alpha=la::Dot(r,r)/dHd;
|
|
//Vector temp2; //alpha*d
|
|
//temp2.Init(gradient.length());
|
|
la::ScaleOverwrite(alpha, d, &temp2);
|
|
la::AddOverwrite(z, temp2, &z_next); //z_(j+1)=z_j+alpha*d_j
|
|
|
|
if(la::Dot(z_next, z_next)>=(radius*radius)) {
|
|
double a=la::Dot(d,d);
|
|
double b=2*la::Dot(z,d);
|
|
double c=la::Dot(z,z)-pow(radius,2);
|
|
DEBUG_ASSERT_MSG(b*b*-4*a*c>0,
|
|
"(Second)Discriminant is negative. Fail to get the solution zeta.");
|
|
double sqrt_discriminant=sqrt(b*b-4*a*c);
|
|
double zeta=(-b+sqrt_discriminant)/(2*a);
|
|
|
|
//p=z+zeta*d
|
|
la::ScaleInit(zeta, d, p);
|
|
la::AddTo(z, p);
|
|
break;
|
|
}
|
|
z.CopyValues(z_next);
|
|
old_r.CopyValues(r);
|
|
|
|
//Vector temp3; //Hd
|
|
//temp3.Init(gradient.length());
|
|
la::MulOverwrite(hessian, d, &temp3);
|
|
la::Scale(alpha, &temp3);
|
|
la::AddOverwrite(temp3, old_r, &r);
|
|
|
|
if(sqrt(la::Dot(r,r))>r0_norm*e){
|
|
p->Copy(z);
|
|
break;
|
|
}
|
|
|
|
double beta=la::Dot(r,r)/la::Dot(old_r, old_r);
|
|
|
|
//Vector temp4; //beta*d
|
|
//temp4.Init(gradient.length());
|
|
la::ScaleOverwrite(beta, d, &temp4);
|
|
la::SubOverwrite(r, temp4, &d);
|
|
|
|
|
|
|
|
} //while
|
|
|
|
cout<<"cnt="<<cnt<<endl;
|
|
//Calculate delta_m
|
|
Vector temp5; //Hp
|
|
la::MulInit(hessian, *p, &temp5);
|
|
double pHp=0;
|
|
pHp=la::Dot(temp5, *p);
|
|
*delta_m=-1*(la::Dot(gradient, *p))-0.5*(pHp);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|