DebugProblem.hpp

#pragma once

#include <cmath>

#include "Implementation/ProblemTraits.hpp"
#include "Types.hpp"


namespace LSLOpt {

template<typename Problem, typename Scalar, typename ErrorHandler>
struct DebugProblem : Problem {

  template <typename ... Args>
  DebugProblem(
      ErrorHandler& error_handler,
      Args&&... args)
  : Problem(std::forward<Args>(args)...)
  , m_error_handler(error_handler)
  {
    this->setDerivativeEpsilon();
    this->setCompareEpsilon();
  }

  void setDerivativeEpsilon(const Scalar& derivative_epsilon = 1e-6)
  {
    m_derivative_epsilon = derivative_epsilon;
  }

  void setCompareEpsilon(const Scalar& compare_epsilon = 1e-3)
  {
    m_compare_epsilon = compare_epsilon;
  }

  const Scalar& getDerivativeEpsilon() const
  {
    return m_derivative_epsilon;
  }

  const Scalar& getCompareEpsilon() const
  {
    return m_compare_epsilon;
  }

  typename Implementation::problem_traits<Problem, Scalar>::gradient_type
  gradient(const Vector<Scalar>& x)
  {
    using std::abs; // we need this for accessing abs()

    typename Implementation::problem_traits<Problem, Scalar>::gradient_type g
        = Problem::gradient(x);

    bool equal = true;
    Vector<Scalar> numerical_gradient(x.size());
    for (Eigen::Index i = 0; i < x.size(); ++i) {
      Vector<Scalar> x_plus(x);
      x_plus(i) += m_derivative_epsilon;

      Vector<Scalar> x_minus(x);
      x_minus(i) -= m_derivative_epsilon;

      if (!x_value_is_ok<Problem>(x_minus, i) || !x_value_is_ok<Problem>(x_plus, i)) {
        continue;
      }

      Scalar score_plus = Problem::value(x_plus);

      Scalar score_minus = Problem::value(x_minus);

      Scalar numeric_first_derivative =
          (score_plus - score_minus) / (Scalar{2} * m_derivative_epsilon);
      numerical_gradient(i) = numeric_first_derivative;

      if (abs(numeric_first_derivative - g(i)) > m_compare_epsilon) {
        equal = false;
        // do not break, we want to calculate and see the rest for debugging
      }
    }

    if (!equal) {
      m_error_handler(x, numerical_gradient, g);
    }


    return g;
  }

  private:

    template<typename U,
        typename std::enable_if<!Implementation::problem_traits<U, Scalar>::has_lower_bound,
            U>::type* = nullptr>
    bool x_value_is_ok(
        const Vector<Scalar>& x,
        Eigen::Index i)
    {
      return true;
    }

    template<typename U,
        typename std::enable_if<Implementation::problem_traits<U, Scalar>::has_lower_bound,
            void>::type* = nullptr>
    bool x_value_is_ok(
        const Vector<Scalar>& x,
        Eigen::Index i)
    {
      return x(i) >= this->lower_bounds()(i) && x(i) <= this->upper_bounds()(i);
    }

    ErrorHandler& m_error_handler;

    Scalar m_derivative_epsilon;
    Scalar m_compare_epsilon;
};

}