IPM

ipm.cpp

#include <Eigen/Core>
#include <Eigen/Sparse>
using namespace std;
using namespace Eigen;

void sinitialize(VectorXd &s,double mu,unsigned int sizex,VectorXd &x)
{
  for (unsigned int i=0;i<sizex;i++)
    if (x[i]!=0)s[i] = mu/x[i];
    else s[i]=0.1;
}

void Jinitialize(SparseMatrix<double> &J,unsigned int sizex,unsigned int sizea,MatrixXd &Q,MatrixXd &A)
{
  J.reserve(sizex*(sizex+1+2*sizea+2));
  VectorXd d(2*sizex+sizea);
  for(unsigned int i=0;i<sizex;i++)
    {
      for(unsigned int j=0;j<sizex;j++)
	{
	  J.insert(i,j)=Q(i,j);//(0,0)
	  if (i==j)J.insert(i,sizex+sizea+i)= -1;//(0,2)
	}
      for(unsigned int j=0;j<sizea;j++)
	  J.insert(i,sizex+j)=-A(j,i);//(0,1)
    }
  for (unsigned int i=0;i<sizea;i++)
      for (unsigned int j=0;j<sizex;j++)
	  J.insert(sizex+i,j)=A(i,j);//(1,0)

}

//min{1/2*x'*Q*x+q'*x} subject to A*x=a
void InteriorPointMethod(MatrixXd &Q, VectorXd &q,VectorXd &x,MatrixXd &A,VectorXd &a)
{
  double mu = 0.1,alpha = 1;
  const double eta = 0.5,epsilon = 0.001,sig_min = 0.01,sig_max = 0.75;
  unsigned int sizex = x.size(),sizea = a.size();
  VectorXd lmd = VectorXd::Zero(sizea);
  //set si subject to xisi=mu
  VectorXd s(sizex);
  sinitialize(s,mu,sizex,x);
  //           Q  -A'  -I
  //J (x,l,s)= A   O   O
  //           Sk O   Xk
  SparseMatrix<double,ColMajor,int> J(2*sizex+sizea,2*sizex+sizea);
  Jinitialize(J,sizex,sizea,Q,A);
  VectorXd d(2*sizex+sizea),F(2*sizex+sizea);
  
  cerr << "[initial value]\n"<<x.head(1.0*sizex/2).transpose()<<endl;
  cerr << "Solving QP";
  for (int k=0;mu>epsilon;k++)
    {
      cerr <<".";
      cerr.flush();
      double sigma = (sig_min+sig_max)/2;//5
      for(unsigned int i=0;i<sizex;i++)
	{
	  J.coeffRef(sizex+sizea+i,i)=s[i];//update Sk
	  J.coeffRef(sizex+sizea+i,sizex+sizea+i)=x[i];//update Xk
	}
      F <<
	Q * x + q -A.transpose()*lmd-s,
	A*x-a,
	(VectorXd)(x.array()*s.array())-VectorXd::Ones(sizex)*mu*sigma;
      F=-F;
      //     Fx
      //F=   Fl
      //     Fs
      J.makeCompressed();
      SparseQR<SparseMatrix<double>,COLAMDOrdering<int> > solver;
      solver.analyzePattern(J);
      solver.factorize(J);
      d = solver.solve(F);//determine dk =(Δxk,Δλk,Δsk)
      alpha=1;
      for (unsigned int i=0;i<sizex;i++)
	if(d[i]<0&&x[i]+alpha*d[i]<0)
	  alpha = -x[i]/d[i];
      for (unsigned int i=0;i<sizea;i++)
	  if (d[i+sizex+sizea]<0&&s[i]+alpha*d[i+sizex+sizea]<0)
	    alpha = -s[i]/d[i+sizex+sizea];
      alpha = eta*alpha < 1 ? eta*alpha : 1;
      mu = (x.transpose()*s)[0]/sizex;
      for (unsigned int i=0;i<sizex;i++)x[i]+=alpha*d[i];
      for (unsigned int i=0;i<sizea;i++)lmd[i]+=alpha*d[sizex+i];
      for (unsigned int i=0;i<sizex;i++)s[i]+=alpha*d[sizex+sizea+i];
    }
  cerr << endl;
}