common/optimization/include/optimization/utils.hpp

Go to the documentation of this file.

// Copyright 2019 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef OPTIMIZATION__UTILS_HPP_
#define OPTIMIZATION__UTILS_HPP_

#include <common/types.hpp>
#include <optimization/visibility_control.hpp>
#include <Eigen/Core>
#include <functional>
#include <limits>

using autoware::common::types::bool8_t;

namespace autoware
{
namespace common
{
namespace optimization
{
class OPTIMIZATION_PUBLIC ComputeMode
{
public:
  ComputeMode() = default;
  ComputeMode(bool8_t score, bool8_t jacobian, bool8_t hessian);
  ComputeMode & set_score() noexcept;
  ComputeMode & set_jacobian() noexcept;
  ComputeMode & set_hessian() noexcept;

  bool8_t score() const noexcept;
  bool8_t jacobian() const noexcept;
  bool8_t hessian() const noexcept;

  bool8_t operator==(const ComputeMode & other) const noexcept;
  bool8_t operator!=(const ComputeMode & other) const noexcept;

private:
  bool8_t m_score{false};
  bool8_t m_jacobian{false};
  bool8_t m_hessian{false};
};

enum class ExpressionTerm
{
  SCORE,  // Evaluation of the expression
  JACOBIAN,  // Evaluation of the first derivative
  HESSIAN   // Evaluation of the second derivative
};

class OPTIMIZATION_PUBLIC EigenComparator
{
public:
  template<typename T, int H, int W>
  typename std::enable_if_t<std::is_floating_point<T>::value, bool8_t>
  operator()(const Eigen::Matrix<T, H, W> & lhs, const Eigen::Matrix<T, H, W> & rhs) const
  {
    return lhs.isApprox(rhs, std::numeric_limits<T>::epsilon());
  }

  template<typename T, int H, int W>
  typename std::enable_if_t<std::is_integral<T>::value, bool8_t>
  operator()(const Eigen::Matrix<T, H, W> & lhs, const Eigen::Matrix<T, H, W> & rhs) const
  {
    return lhs == rhs;
  }
};

template<typename DomainValue, typename ComparatorT = decltype(std::equal_to<DomainValue>())>
class OPTIMIZATION_PUBLIC CacheStateMachine
{
public:
  explicit CacheStateMachine(const ComparatorT & comparator = ComparatorT())
  : m_comparator(comparator) {}

  CacheStateMachine(
    const DomainValue & value, const ComputeMode & mode,
    const ComparatorT & comparator = ComparatorT())
  : m_comparator(comparator), m_last_mode(mode), m_last_value(value)
  {
  }

  void update(const DomainValue & value, const ComputeMode & mode) noexcept
  {
    m_last_mode = mode;
    m_last_value = value;
  }

  bool8_t is_cached(const DomainValue & x, const ExpressionTerm & term) const noexcept
  {
    bool8_t ret = false;
    if (m_comparator(x, m_last_value)) {
      switch (term) {
        case ExpressionTerm::SCORE:
          ret = m_last_mode.score();
          break;
        case ExpressionTerm::JACOBIAN:
          ret = m_last_mode.jacobian();
          break;
        case ExpressionTerm::HESSIAN:
          ret = m_last_mode.hessian();
          break;
        default:
          ret = false;
      }
    }
    return ret;
  }

private:
  const ComparatorT m_comparator;
  DomainValue m_last_value;
  ComputeMode m_last_mode;
};

}  // namespace optimization
}  // namespace common
}  // namespace autoware

#endif  // OPTIMIZATION__UTILS_HPP_