rotating_calipers.hpp

Go to the documentation of this file.

// Copyright 2017-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 GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
#define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
#include <geometry/convex_hull.hpp>
#include <geometry/common_2d.hpp>
#include <geometry/bounding_box/bounding_box_common.hpp>
 
#include <algorithm>
#include <cstring>
#include <limits>
#include <list>
 
namespace autoware
{
namespace common
{
namespace geometry
{
namespace bounding_box
{
namespace details
{
template<typename PointT>
uint32_t update_angles(const Point4<PointT> & edges, Point4<PointT> & directions)
{
  // find smallest angle to next
  float32_t best_cos_th = -std::numeric_limits<float32_t>::max();
  float32_t best_edge_dir_mag = 0.0F;
  uint32_t advance_idx = 0U;
  for (uint32_t idx = 0U; idx < edges.size(); ++idx) {
    const float32_t edge_dir_mag =
      std::max(norm_2d(edges[idx]) * norm_2d(directions[idx]), autoware::common::types::FEPS);
    const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag;
    if (cos_th > best_cos_th) {
      best_cos_th = cos_th;
      best_edge_dir_mag = edge_dir_mag;
      advance_idx = idx;
    }
  }
 
  // update directions by smallest angle
  const float32_t sin_th =
    cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag;
  for (uint32_t idx = 0U; idx < edges.size(); ++idx) {
    rotate_2d(directions[idx], best_cos_th, sin_th);
  }
 
  return advance_idx;
}
 
template<typename IT, typename PointT>
void init_edges(
  const IT begin,
  const IT end,
  const Point4<IT> & support,
  Point4<PointT> & edges)
{
  for (uint32_t idx = 0U; idx < support.size(); ++idx) {
    auto tmp_it = support[idx];
    ++tmp_it;
    const PointT & q = (tmp_it == end) ? *begin : *tmp_it;
    edges[idx] = minus_2d(q, *support[idx]);
  }
}
 
template<typename IT>
void init_bbox(const IT begin, const IT end, Point4<IT> & support)
{
  // compute initial orientation using first two points
  auto pt_it = begin;
  ++pt_it;
  const auto u = minus_2d(*pt_it, *begin);
  support[0U] = begin;
  std::array<float32_t, 3U> metric{{
    -std::numeric_limits<float32_t>::max(),
    -std::numeric_limits<float32_t>::max(),
    std::numeric_limits<float32_t>::max()
  }};
  // track u * p, fabsf(u x p), and -u * p
  for (pt_it = begin; pt_it != end; ++pt_it) {
    // check points against orientation for run_ptr
    const auto & pt = *pt_it;
    // u * p
    const float32_t up = dot_2d(u, pt);
    if (up > metric[0U]) {
      metric[0U] = up;
      support[1U] = pt_it;
    }
    // -u * p
    if (up < metric[2U]) {
      metric[2U] = up;
      support[3U] = pt_it;
    }
    // u x p
    const float32_t uxp = cross_2d(u, pt);
    if (uxp > metric[1U]) {
      metric[1U] = uxp;
      support[2U] = pt_it;
    }
  }
}
template<typename IT, typename MetricF>
BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn)
{
  using PointT = base_type<decltype(*begin)>;
  // sanity check TODO(c.ho) more checks (up to n = 2?)
  if (begin == end) {
    throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end)));
  }
  // initial scan to get anchor points
  Point4<IT> support;
  init_bbox(begin, end, support);
  // initialize directions accordingly
  Point4<PointT> directions;
  {  // I just don't want the temporary variable floating around
    auto tmp = support[0U];
    ++tmp;
    directions[0U] = minus_2d(*tmp, *support[0U]);
    directions[1U] = get_normal(directions[0U]);
    directions[2U] = minus_2d(directions[0U]);
    directions[3U] = minus_2d(directions[1U]);
  }
  // initialize edges
  Point4<PointT> edges;
  init_edges(begin, end, support, edges);
  // size of initial guess
  BoundingBox bbox;
  decltype(BoundingBox::corners) best_corners;
  compute_corners(best_corners, support, directions);
  size_2d(best_corners, bbox.size);
  bbox.value = metric_fn(bbox.size);
  // rotating calipers step: incrementally advance, update angles, points, compute area
  for (auto it = begin; it != end; ++it) {
    // find smallest angle to next, update directions
    const uint32_t advance_idx = update_angles(edges, directions);
    // recompute area
    decltype(BoundingBox::corners) corners;
    compute_corners(corners, support, directions);
    decltype(BoundingBox::size) tmp_size;
    size_2d(corners, tmp_size);
    const float32_t tmp_value = metric_fn(tmp_size);
    // update best if necessary
    if (tmp_value < bbox.value) {
      // corners: memcpy is fine since I know corners and best_corners are distinct
      (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners));
      // size
      bbox.size = tmp_size;
      bbox.value = tmp_value;
    }
    // Step to next iteration of calipers
    {
      // update smallest support
      auto next_it = support[advance_idx];
      ++next_it;
      const auto run_it = (end == next_it) ? begin : next_it;
      support[advance_idx] = run_it;
      // update edges
      next_it = run_it;
      ++next_it;
      const PointT & next = (end == next_it) ? (*begin) : (*next_it);
      edges[advance_idx] = minus_2d(next, *run_it);
    }
  }
 
  finalize_box(best_corners, bbox);
 
  // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2
  // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did,
  // it would probably get smoothed out by prediction. But, this could cause issues with the
  // collision detection algorithms (i.e GJK), but probably not.
 
  return bbox;
}
}  // namespace details
 
template<typename IT>
BoundingBox minimum_area_bounding_box(const IT begin, const IT end)
{
  const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t
    {
      return pt.x * pt.y;
    };
  return details::rotating_calipers_impl(begin, end, metric_fn);
}
 
template<typename IT>
BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end)
{
  const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t
    {
      return pt.x + pt.y;
    };
  return details::rotating_calipers_impl(begin, end, metric_fn);
}
 
template<typename PointT>
BoundingBox minimum_area_bounding_box(std::list<PointT> & list)
{
  const auto last = convex_hull(list);
  return minimum_area_bounding_box(list.cbegin(), last);
}
 
template<typename PointT>
BoundingBox minimum_perimeter_bounding_box(std::list<PointT> & list)
{
  const auto last = convex_hull(list);
  return minimum_perimeter_bounding_box(list.cbegin(), last);
}
}  // namespace bounding_box
}  // namespace geometry
}  // namespace common
}  // namespace autoware
#endif  // GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_