lookup_table.hpp

Go to the documentation of this file.

// Copyright 2017-2020 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 GEOMETRY__LOOKUP_TABLE_HPP_
#define GEOMETRY__LOOKUP_TABLE_HPP_

#include <cmath>
#include <stdexcept>
#include <vector>

#include "common/types.hpp"
#include "geometry/interval.hpp"

namespace autoware
{
namespace common
{
namespace helper_functions
{

namespace
{

template<typename T, typename U>
T interpolate(const T & a, const T & b, U scaling)
{
  static const geometry::Interval<U> UNIT_INTERVAL(static_cast<U>(0), static_cast<U>(1));
  scaling = geometry::Interval<U>::clamp_to(UNIT_INTERVAL, scaling);

  const auto m = (b - a);
  const auto offset = static_cast<U>(m) * scaling;
  return a + static_cast<T>(offset);
}

// TODO(c.ho) support more forms of interpolation as template functor
// Actual lookup logic, assuming all invariants hold:
// Throw if value is not finite
template<typename T>
T lookup_impl_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
{
  if (!std::isfinite(value)) {
    throw std::domain_error{"Query value is not finite (NAN or INF)"};
  }
  if (value <= domain.front()) {
    return range.front();
  } else if (value >= domain.back()) {
    return range.back();
  } else {
    // Fall through to normal case
  }

  auto second_idx{0U};
  for (auto idx = 1U; idx < domain.size(); ++idx) {
    if (value < domain[idx]) {
      second_idx = idx;
      break;
    }
  }
  // T must be a floating point between 0 and 1
  const auto num = static_cast<double>(value - domain[second_idx - 1U]);
  const auto den = static_cast<double>(domain[second_idx] - domain[second_idx - 1U]);
  const auto t = num / den;
  const auto val = interpolate(range[second_idx - 1U], range[second_idx], t);
  return static_cast<T>(val);
}

// Check invariants for table lookup:
template<typename T>
void check_table_lookup_invariants(const std::vector<T> & domain, const std::vector<T> & range)
{
  if (domain.size() != range.size()) {
    throw std::domain_error{"Domain's size does not match range's"};
  }
  if (domain.empty()) {
    throw std::domain_error{"Empty domain or range"};
  }
  // Check if sorted: Can start at 1 because not empty
  for (auto idx = 1U; idx < domain.size(); ++idx) {
    if (domain[idx] <= domain[idx - 1U]) {  // This is safe because indexing starts at 1
      throw std::domain_error{"Domain is not sorted"};
    }
  }
}
}  // namespace

template<typename T>
T lookup_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
{
  check_table_lookup_invariants(domain, range);

  return lookup_impl_1d(domain, range, value);
}

template<typename T>
class LookupTable1D
{
public:
  LookupTable1D(const std::vector<T> & domain, const std::vector<T> & range)
  : m_domain{domain},
    m_range{range}
  {
    check_table_lookup_invariants(m_domain, m_range);
  }

  LookupTable1D(std::vector<T> && domain, std::vector<T> && range)
  : m_domain{domain},
    m_range{range}
  {
    check_table_lookup_invariants(m_domain, m_range);
  }

  T lookup(const T value) const
  {
    return lookup_impl_1d(m_domain, m_range, value);
  }
  const std::vector<T> & domain() const noexcept {return m_domain;}
  const std::vector<T> & range() const noexcept {return m_range;}

private:
  std::vector<T> m_domain;
  std::vector<T> m_range;
};  // class LookupTable1D

}  // namespace helper_functions
}  // namespace common
}  // namespace autoware


#endif  // GEOMETRY__LOOKUP_TABLE_HPP_