NICE_Optimization/MatrixIterativeOptimizer.cpp

//////////////////////////////////////////////////////////////////////
//
//	MatrixIterativeOptimizer.cpp: Implementation of the MatrixIterativeOptimizer
//								  Optimizer class.
//
//	Written by Matthias Wacker
//
//////////////////////////////////////////////////////////////////////

#include "optimization/MatrixIterativeOptimizer.h"
#include <iostream>
#include "optimization/AdditionalIceUtils.h" // for linsolve
#include "core/optimization/blackbox/DownhillSimplexOptimizer.h" //FIXME WACKER: INCLUDE DOWNHILL SIMPLEX FOR THE GET RANK FUNCTION -> EXPORT IT 

using namespace OPTIMIZATION;

MatrixIterativeOptimizer::MatrixIterativeOptimizer(OptLogBase *loger) : SuperClass(loger)
{
  m_IterationMatrix	= matrix_type(m_numberOfParameters,m_numberOfParameters);
  m_hk				= matrix_type(m_numberOfParameters,1);
  m_stepSize			= matrix_type(m_numberOfParameters,1);
  m_oldGradient		= matrix_type(m_numberOfParameters,1);
  m_oldParams			= matrix_type(m_numberOfParameters,1);

  m_lambdak			= 1.0;

}


MatrixIterativeOptimizer::MatrixIterativeOptimizer( const MatrixIterativeOptimizer &opt) : SuperClass(opt)
{

  m_IterationMatrix	= opt.m_IterationMatrix;
  m_hk				= opt.m_hk;
  m_lin				= opt.m_lin;
  m_stepSize			= opt.m_stepSize;
  m_lambdak			= opt.m_lambdak;
  m_oldParams			= opt.m_oldParams;
  m_oldGradient		= opt.m_oldGradient;

}


MatrixIterativeOptimizer::~MatrixIterativeOptimizer()
{
}



void MatrixIterativeOptimizer::init()
{
  if(m_loger)
    m_loger->logTrace("entering MatrixIterativeOptimizer:: init ...  \n");

  SuperClass::init();

  m_lin= ArmijoLineSearcher(m_costFunction,m_loger);

  //if(m_verbose==true) //FIXME commented out as output is not human readable
  //	m_lin.setVerbose(true);
  
  if(m_maximize == true)
  {
    m_lin.setMaximize(true);
  }

  m_IterationMatrix=matrix_type(m_numberOfParameters,m_numberOfParameters);
  for(int i =0;i < static_cast<int>(m_numberOfParameters); i++)
  {
    m_IterationMatrix(i,i) = 1.0;
  }

  
  if(m_loger)
    m_loger->logTrace("leaving MatrixIterativeOptimizer:: init ...\n");
}


MatrixIterativeOptimizer & MatrixIterativeOptimizer::operator=(const MatrixIterativeOptimizer &opt)
{

  /*
      avoid self-copy
  */
  if(this != &opt)
  {
    
    /*
      =operator of SuperClass
    */
    SuperClass::operator=(opt);

    /*
      own values:
    */
    m_IterationMatrix	= opt.m_IterationMatrix;
    m_hk				= opt.m_hk;
    m_lin				= opt.m_lin;
    m_stepSize			= opt.m_stepSize;
    m_lambdak			= opt.m_lambdak;
    m_oldParams			= opt.m_oldParams;
    m_oldGradient		= opt.m_oldGradient;
  }

    return *this;
}

void MatrixIterativeOptimizer::computeDescentDirection()
{
  if(m_loger)
    m_loger->logTrace("entering MatrixIterativeOptimizer::computeDescentDirection ... \n");
  
  if(m_numberOfParameters == 1)
  {
    if(fabs(m_IterationMatrix(0,0)) >= 1.0e-20 )
    {
      m_hk(0,0) = -m_gradient(0,0) / m_IterationMatrix(0,0);
    }
    else
    {
      m_hk = m_gradient;
      m_hk *= -1.0;
    }
  }
  else
  {
    
    //if(getRank(m_IterationMatrix, 1.0e-5 ) == m_numberOfParameters)
    if(true)
    {
      /*
        solve system equation for descent direction
      */
      matrix_type tmp=(m_gradient * -1.0 );
      linSolve(m_IterationMatrix, m_hk, tmp );

      /*
        direction orthogonal to gradient? -> use gradient
      */
      if(fabs((!m_hk * m_gradient)(0,0)) <= 1.0e-3)
      {
        m_hk = m_gradient;
        m_hk *= -1.0;
      }
    }
    else
    {
      m_hk = m_gradient;
      m_hk *= -1.0;
    }
  }

  if(m_loger)
    m_loger->logTrace("leaving MatrixIterativeOptimizer::computeDescentDirection ... \n");
}

void MatrixIterativeOptimizer::resetMatrix()
{
  m_IterationMatrix=matrix_type(m_numberOfParameters,m_numberOfParameters);
  for(int i =0;i < static_cast<int>(m_numberOfParameters); i++)
  {
    m_IterationMatrix(i,i) = 1.0;
  }
}

void MatrixIterativeOptimizer::setStepSize(const matrix_type & stepSize)
{
  m_stepSize = stepSize;
}

int MatrixIterativeOptimizer::optimize()
{
  if(m_loger)
    m_loger->logTrace("entering MatrixIterativeOptimizer:optimize ... \n");

  init();

  bool abort = false;
  
  
  /******************************
    start time criteria
  ******************************/
  m_startTime = clock();

  /******************************
    compute start value 
    and first gradient
  ******************************/
  m_currentCostFunctionValue = evaluateCostFunction(m_parameters);

  m_gradient = (m_analyticalGradients == true && 
        (m_costFunction->hasAnalyticGradient() == true) ) ?
              getAnalyticalGradient(m_parameters) : 
              getNumericalGradient(m_parameters, m_stepSize);


  m_hk = m_gradient * -1.0;

  matrix_type hk_norm;
  double factor = m_hk.frobeniusNorm();
  if(fabs(factor) > 1.0e-12)
  {
    hk_norm = m_hk * (1.0/ factor); 
  }
  matrix_type grad_norm;
  double factor2 = m_gradient.frobeniusNorm();
  if(fabs(factor2) > 1.0e-12)
  {
    grad_norm = m_gradient * (1.0/ factor2); 
  }

  /*******************************
    optimize step length
  *******************************/
  /// ARMIJO
  //m_lin.setXkGkDk(m_parameters, m_gradient, hk_norm);
  m_lin.setXkGkDk(m_parameters, grad_norm, hk_norm);
  m_lambdak = m_lin.optimize();
  if(m_verbose == true)
  {
    std::cout << "stepsize lambda: " << m_lambdak << std::endl;
    std::cout << "in direction: " << std::endl;
    for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
    {
      std::cout<< hk_norm(r,0) << " ";
    }
    std::cout << std::endl;
      
  }

  
  /*******************************
    update the parameters
  *******************************/
  m_oldParams = m_parameters;
  m_parameters = m_parameters + hk_norm * m_lambdak;


  /******************************
    check abort criteria 
      for gradient
  ******************************/	
  if(m_gradientTolActive)
  {
    if(m_gradient.frobeniusNorm() < m_gradientTol){
      m_returnReason = SUCCESS_GRADIENTTOL;
      abort = true;
    }
  }	


  /* do the optimization loop */
  while(abort == false)
  {
    /******************************
      increase iteration 
            counter
    ******************************/
    m_numIter++;
    

    /******************************
      cout actual iteration
    ******************************/

    if(m_verbose == true)
    {
      std::cout << "MatrixIterativeOptimizer :: Iteration: " << m_numIter << " : current parameters : ";
      for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
      {
        std::cout<< m_parameters(r,0) << "\t ";
      }
      std::cout << "    value:   " << m_currentCostFunctionValue << std::endl;
    }


    /******************************
      Check iteration limit
    ******************************/
    if(m_maxNumIterActive)
    {
      if(m_numIter >= m_maxNumIter)
      {
        if(m_verbose == true)
        {
          std::cout << "Gradient Descenct :: aborting because of numIter" << std::endl;
        }


        /* set according return status and return */
        m_returnReason = SUCCESS_MAXITER;
        abort = true;
        continue;
      }
    }

    /******************************
      get the new Gradient
    ******************************/
    m_oldGradient	= m_gradient;
    m_gradient =	(m_analyticalGradients == true && 
            (m_costFunction->hasAnalyticGradient() == true) ) ?
              getAnalyticalGradient(m_parameters) : 
              getNumericalGradient(m_parameters, m_stepSize);


    /******************************
      check gradient tol
    ******************************/
    if(m_gradientTolActive)
    {
      if(m_gradient.frobeniusNorm() < m_gradientTol)
      {
        if(m_verbose == true)
        {
          std::cout << "MatrixIterativeOptimizer :: aborting because of GradientTol" << std::endl;
        }

        m_returnReason = SUCCESS_GRADIENTTOL;
        abort = true;
        continue;
      }
    }

    /******************************
          Update the Iteration 
            Matrix
    ******************************/
    //matrix_type oldMatrix	= m_IterationMatrix;
    updateIterationMatrix();

    /******************************
      compute a descent 
          direction
    ******************************/
    computeDescentDirection();


    // check if descent direction is really a descent direction
    // if not: reset the iteration matrix and take
    // the gradient as iteration direction
    if( (!m_hk *m_gradient)(0,0) > 0.0)
    {
      std::cout << " resetting matrix" << std::endl;
      resetMatrix();
      m_hk=m_gradient * -1.0;
    }
  


    double factor = m_hk.frobeniusNorm();
    if(fabs(factor) > 1.0e-12)
    {
      hk_norm = m_hk * (1.0/ factor); 
    }
    matrix_type grad_norm;
    double factor2 = m_gradient.frobeniusNorm();
    if(fabs(factor2) > 1.0e-12)
    {
      grad_norm = m_gradient * (1.0/ factor2); 
    }

    /*******************************
      optimize step length
    *******************************/
    //m_lin.setXkGkDk(m_parameters, m_gradient, hk_norm);
    m_lin.setXkGkDk(m_parameters, grad_norm, hk_norm);
    m_lambdak = m_lin.optimize();
    

    
    /*******************************
      update the parameters
    *******************************/
    m_oldParams = m_parameters;
    m_parameters = m_parameters + hk_norm * m_lambdak;
    if(m_verbose == true)
    {
      //matrix_type hk_normu=S2U(hk_norm);
      std::cout << "MatrixIterativeOptimizer :: Iterating with lambda = " << m_lambdak << std::endl;

      std::cout << "MatrixIterativeOptimizer :: in direction : ";
      for(int mu = 0; mu < static_cast<int>(m_numberOfParameters); mu++)
      {
        //std::cout<< hk_normu(mu,0) << "\t ";
        std::cout<< hk_norm(mu,0) << "\t ";
      }
      std::cout << std::endl;

      //matrix_type m_gradientu=S2U(m_gradient);
      std::cout << "MatrixIterativeOptimizer :: Gradient was ";
      for(int nu = 0; nu < static_cast<int>(m_numberOfParameters); nu++)
      {
        //std::cout<< m_gradientu(nu,0) << "\t ";
        std::cout<< m_gradient(nu,0) << "\t ";
      }
      std::cout << std::endl;
      
    }

      
        
    /*******************************
      Check new parameters 
        are it is in bounds, 
        paramTol,
        funcTol,
        maxSeconds
    *******************************/
    if(!checkParameters(m_parameters))
    {
      if(m_verbose == true)
      {
        std::cout << "Gradient Descenct :: aborting because of parameter Bounds" << std::endl;
      }

      /* set according return status and the last parameters and return */
      m_returnReason = ERROR_XOUTOFBOUNDS;
      m_parameters = m_oldParams;
      abort = true;
      continue;
    }

    /*
      Get new Costfunction Value
    */
    double oldFuncValue		= m_currentCostFunctionValue;
    m_currentCostFunctionValue = evaluateCostFunction(m_parameters);


    /*
      Check ParamTol
    */
    if(m_paramTolActive)
    {
      if( (m_parameters - m_oldParams).frobeniusNorm() < m_paramTol)
      {
        if(m_verbose == true)
        {
          std::cout << "Gradient Descenct :: aborting because of param Tol" << std::endl;
        }

        /* set according return status and the last parameters and return */
        m_returnReason = SUCCESS_PARAMTOL;
        /*m_parameters = oldParams;*/
        abort = true;
        continue;
      }

    }
  
    if(m_funcTolActive)
    {
      if( abs((m_currentCostFunctionValue - oldFuncValue)) < m_funcTol)
      {
        if(m_verbose == true)
        {
          std::cout << "MatrixIterativeOptimizer :: aborting because of Func Tol" << std::endl;
        }

        /* set according return status and the last parameters and return */
        m_returnReason = SUCCESS_FUNCTOL;
        abort = true;
        continue;
      }
    }

    /*
      Check Optimization Timilimit, if active
    */
    if(m_maxSecondsActive)
    {
      m_currentTime = clock();
    
      /* time limit exceeded ? */
      if(((float)(m_currentTime - m_startTime )/CLOCKS_PER_SEC) >= m_maxSeconds )
      {
        if(m_verbose == true)
        {
          std::cout << "MatrixIterativeOptimizer :: aborting because of Time Limit" << std::endl;
        }

        /* set according return status and the last parameters and return */
        m_returnReason = SUCCESS_TIMELIMIT;
        m_parameters = m_oldParams;
        abort = true;
        continue;
      }
    }

  }

  if(m_verbose)
  {
    std::cout << "MatrixIterativeOptimizer :: RESULT: ";
    for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
    {
      std::cout<< m_parameters(r,0) << " ";
    }
    std::cout << "    value:   " << m_currentCostFunctionValue  << std::endl;
  }

  if(m_loger)
    m_loger->logTrace("leaving MatrixIterativeOptimizer:optimize ... \n");
  
  return m_returnReason;

}