bounding_box_common.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__BOUNDING_BOX_COMMON_HPP_
#define GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_
 
#include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
#include <autoware_auto_perception_msgs/msg/shape.hpp>
#include <geometry/visibility_control.hpp>
#include <geometry/common_2d.hpp>
#include <geometry_msgs/msg/pose.hpp>
 
#include <array>
#include <limits>
#include <vector>
 
namespace autoware
{
namespace common
{
namespace geometry
{
namespace bounding_box
{
using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
template<typename IT>
void compute_height(const IT begin, const IT end, BoundingBox & box)
{
  float32_t max_z = -std::numeric_limits<float32_t>::max();
  float32_t min_z = std::numeric_limits<float32_t>::max();
  for (auto it = begin; it != end; ++it) {
    const float32_t z = point_adapter::z_(*it);
    if (z <= min_z) {
      min_z = z;
    }
    if (z >= max_z) {
      max_z = z;
    }
  }
  box.centroid.z = (max_z + min_z) * 0.5F;
  for (auto & corner : box.corners) {
    corner.z = box.centroid.z;
  }
  box.size.z = (max_z - min_z);
}
 
template<typename IT>
void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape)
{
  float32_t max_z = -std::numeric_limits<float32_t>::max();
  float32_t min_z = std::numeric_limits<float32_t>::max();
  for (auto it = begin; it != end; ++it) {
    const float32_t z = point_adapter::z_(*it);
    if (z <= min_z) {
      min_z = z;
    }
    if (z >= max_z) {
      max_z = z;
    }
  }
  for (auto & corner : shape.polygon.points) {
    corner.z = min_z;
  }
  shape.height = max_z - min_z;
}
 
namespace details
{
 
template<typename T>
using base_type = typename std::remove_cv<typename std::remove_reference<T>::type>::type;
 
template<typename PointT>
using Point4 = std::array<PointT, 4U>;
 
template<typename>
struct array_size;
template<typename T, std::size_t N>
struct array_size<std::array<T, N>>
{
  static std::size_t const size = N;
};
static_assert(
  array_size<base_type<decltype(BoundingBox::corners)>>::size == 4U,
  "Bounding box does not have the right number of corners");
 
void GEOMETRY_PUBLIC size_2d(
  const decltype(BoundingBox::corners) & corners,
  geometry_msgs::msg::Point32 & ret);
void GEOMETRY_PUBLIC finalize_box(
  const decltype(BoundingBox::corners) & corners, BoundingBox & box);
 
template<typename IT, typename PointT>
void compute_corners(
  decltype(BoundingBox::corners) & corners,
  const Point4<IT> & supports,
  const Point4<PointT> & directions)
{
  for (uint32_t idx = 0U; idx < corners.size(); ++idx) {
    const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U;
    const auto pt =
      intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]);
    // TODO(c.ho) handle error
    point_adapter::xr_(corners[idx]) = point_adapter::x_(pt);
    point_adapter::yr_(corners[idx]) = point_adapter::y_(pt);
  }
}
// TODO(c.ho) type trait enum base
 
autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box);
 
geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box);
 
autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC make_detected_object(
  const BoundingBox & box);
 
std::vector<geometry_msgs::msg::Point32> GEOMETRY_PUBLIC get_transformed_corners(
  const autoware_auto_perception_msgs::msg::Shape & shape_msg,
  const geometry_msgs::msg::Point & centroid,
  const geometry_msgs::msg::Quaternion & orientation);
 
}  // namespace details
}  // namespace bounding_box
}  // namespace geometry
}  // namespace common
}  // namespace autoware
 
#endif  // GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_