Open darewolf007 opened 1 year ago
#ifndef ALM_HPP
#define ALM_HPP
#include "lbfgs.hpp"
#include <Eigen/Eigen>
#include <cmath>
#include <cfloat>
#include <iostream>
#include <vector>
using namespace std;
using namespace Eigen;
namespace alm
{
class Alm
{
public:
Alm() = default;
Alm(const Eigen::MatrixXd A, const Eigen::VectorXd b,const Eigen::VectorXd c,const Eigen::VectorXd f)
:A(A), b(b), c(c), f(f)
{
mu.resize(b.size());
mu.fill(0);
d.resize(b.size());
d.fill(1);
cc.resize(7,7);
cc(0,0) = 1;
}
private:
lbfgs::lbfgs_parameter_t lbfgs_params;
const double gamma_ = 1., beta_ = 1e3;
Eigen::MatrixXd A,cc;
Eigen::VectorXd b, c, f, mu,d;
double rho = 1;
public:
inline Eigen::VectorXd projectVectorOnCone(const VectorXd& inner)
{
double x0 = inner[0];
VectorXd x1(inner.size() - 1);
x1[0] = inner[1];
x1[1] = inner[2];
x1[2] = inner[3];
x1[3] = inner[4];
x1[4] = inner[5];
x1[5] = inner[6];
x1[6] = inner[7];
VectorXd g(inner.size());
g.setZero();
if(x0 <= -1 * x1.norm())
{
g.setZero() ;
}
else if(abs(x0) < x1.norm())
{
g[0] = x1.norm();
for(int i = 0; i < 7; i++)
{
g[i+1] = x1[i];
}
g = (x0 + x1.norm())/(2 * x1.norm()) * g;
}
else
{
g = inner;
}
return g;
}
static inline double costFunction(void *ptr,
const Eigen::VectorXd &x,
Eigen::VectorXd &g)
{
Alm &obj = *(Alm *)ptr;
g.setZero();
double cost = 0;
g=obj.f - obj.A.transpose()*obj.projectVectorOnCone(obj.mu - obj.rho*(obj.A*x + obj.b));
cost = obj.f.transpose() * x + 0.5 * obj.rho * std::pow(obj.projectVectorOnCone(obj.mu /obj.rho - (obj.A*x + obj.b)).norm(), 2);
return cost;
}
inline double optimize(Eigen::VectorXd &x,
const double &relCostTol)
{
/*********************ALM optimize***********************/
lbfgs::lbfgs_parameter_t lbfgs_params;
lbfgs_params.mem_size = 256;
lbfgs_params.past = 3;
lbfgs_params.min_step = 1.0e-32;
lbfgs_params.max_step = 100;
lbfgs_params.g_epsilon = 1e-6;
lbfgs_params.delta = relCostTol;
lbfgs_params.max_iterations = 100000;
lbfgs_params.max_linesearch = 128;
rho = 1;
double minCost;
const int MAX_COUNT = 100000;
int count = 0;
while (++count < MAX_COUNT)
{
if(((f - A.transpose()*projectVectorOnCone(mu - rho*(A*x + b))).norm()/x.norm() < 1e-4)&&((mu /rho - projectVectorOnCone(mu /rho - (A*x + b))).lpNorm<Eigen::Infinity>() >= 1e-4))
{
break;
}
int ret = lbfgs::lbfgs_optimize(x,
minCost,
&Alm::costFunction,
nullptr,
nullptr,
this,
lbfgs_params);
if (ret < 0)
{
std::cout << "\033[32m" << lbfgs::lbfgs_strerror(ret) << "\033[0m" << std::endl;
return ret;
}
mu = projectVectorOnCone(mu - rho*(A*x + b));
rho = std::min((1. + gamma_) * rho, beta_);
}
if (count == MAX_COUNT)
{
return -1;
}
return minCost;
}
};
}
#endif