BFGSStorage.hpp

#pragma once

#include "../ScalarTraits.hpp"
#include "../Types.hpp"


namespace LSLOpt {

namespace Implementation {

template<typename Scalar, typename OutputFunction>
struct BFGSStorage {
    BFGSStorage(
        Eigen::Index n,
        Eigen::Index,
        Scalar,
        OutputFunction& output_function);

    void reset();

    Vector<Scalar> calculate_Hv(
        const Vector<Scalar>& v);

    Scalar calculate_vHv(
        const Vector<Scalar>& v);

    const Matrix<Scalar>& calculate_H();

    Vector<Scalar> calculate_Bv(
        const Vector<Scalar>& v);

    Scalar calculate_vBv(
        const Vector<Scalar>& v);

    const Matrix<Scalar>& calculate_B();

    bool update(
        const Vector<Scalar>& s,
        const Vector<Scalar>& y,
        const Vector<Scalar>& g);

    Eigen::Index n;

    Matrix<Scalar> H;
    Matrix<Scalar> B;
    bool initial;

    OutputFunction& output_function;
};

template<typename Scalar, typename OutputFunction>
BFGSStorage<Scalar, OutputFunction>::BFGSStorage(
    Eigen::Index n,
    Eigen::Index,
    Scalar,
    OutputFunction& output_function)
: n(n)
, initial(true)
, output_function(output_function)
{

}

template<typename Scalar, typename OutputFunction>
void BFGSStorage<Scalar, OutputFunction>::reset()
{
  H = Matrix<Scalar>::Identity(n, n);
  B = Matrix<Scalar>::Identity(n, n);
  initial = true;
}

template<typename Scalar, typename OutputFunction>
Vector<Scalar> BFGSStorage<Scalar, OutputFunction>::calculate_Hv(
    const Vector<Scalar>& v)
{
  // matrix vector product
  return H * v;
}

template<typename Scalar, typename OutputFunction>
Scalar BFGSStorage<Scalar, OutputFunction>::calculate_vHv(
    const Vector<Scalar>& v)
{
  // matrix vector and dot product
  return v.dot(calculate_Hv(v));
}

template<typename Scalar, typename OutputFunction>
const Matrix<Scalar>& BFGSStorage<Scalar, OutputFunction>::calculate_H()
{
  return H;
}

template<typename Scalar, typename OutputFunction>
Vector<Scalar> BFGSStorage<Scalar, OutputFunction>::calculate_Bv(
    const Vector<Scalar>& v)
{
  // matrix vector product
  return B * v;
}

template<typename Scalar, typename OutputFunction>
Scalar BFGSStorage<Scalar, OutputFunction>::calculate_vBv(
    const Vector<Scalar>& v)
{
  // matrix vector and dot product
  return v.dot(calculate_Bv(v));
}

template<typename Scalar, typename OutputFunction>
const Matrix<Scalar>& BFGSStorage<Scalar, OutputFunction>::calculate_B()
{
  return B;
}

template<typename Scalar, typename OutputFunction>
bool BFGSStorage<Scalar, OutputFunction>::update(
    const Vector<Scalar>& s,
    const Vector<Scalar>& y,
    const Vector<Scalar>& g)
{
  if (initial) {
    // scale the initial (inverse) Hessian
    H *= y.dot(s) / y.dot(y);
    B *= y.dot(y) / y.dot(s);
    initial = false;
  }

  // update the Hessian approximation
  B = B - (B * s * s.transpose() * B)/(s.transpose() * B * s)
      + (y * y.transpose()) / (s.transpose() * y);

  Scalar rho = Scalar{1} / y.dot(s);

  // update the inverse Hessian approximation
  H = (Matrix<Scalar>::Identity(n, n) - s * y.transpose() * rho)
      * H * (Matrix<Scalar>::Identity(n, n) - y * s.transpose() * rho)
      + rho * s * s.transpose();
  return !H.unaryExpr(&scalar_traits<Scalar>::is_nan).any()
      && !B.unaryExpr(&scalar_traits<Scalar>::is_nan).any();
}

}

}