euclidean_cluster.hpp

Go to the documentation of this file.

// Copyright 2019-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 EUCLIDEAN_CLUSTER__EUCLIDEAN_CLUSTER_HPP_
#define EUCLIDEAN_CLUSTER__EUCLIDEAN_CLUSTER_HPP_

#include <autoware_auto_msgs/msg/bounding_box_array.hpp>
#include <autoware_auto_msgs/msg/point_clusters.hpp>
#include <geometry/spatial_hash.hpp>
#include <euclidean_cluster/visibility_control.hpp>
#include <common/types.hpp>
#include <string>
#include <vector>
#include <utility>

namespace autoware
{
namespace perception
{
namespace segmentation
{
namespace euclidean_cluster
{
using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
struct PointXYZI
{
  float32_t x = 0.0f;
  float32_t y = 0.0f;
  float32_t z = 0.0f;
  float32_t intensity = 0.0f;
};  // struct PointXYZI

class EUCLIDEAN_CLUSTER_PUBLIC PointXYZII
{
public:
  PointXYZII() = default;
  PointXYZII(const PointXYZI & pt, const uint32_t id);
  PointXYZII(
    const float32_t x,
    const float32_t y,
    const float32_t z,
    const float32_t intensity,
    const uint32_t id);
  uint32_t get_id() const;
  const PointXYZI & get_point() const;

private:
  // This could instead be a pointer; I'm pretty sure ownership would work out, but I'm
  // uncomfortable doing it that way (12 vs 20 bytes)
  PointXYZI m_point;
  uint32_t m_id = 0;
};  // class PointXYZII

using HashConfig = autoware::common::geometry::spatial_hash::Config2d;
using Hash = autoware::common::geometry::spatial_hash::SpatialHash2d<PointXYZII>;
using Clusters = autoware_auto_msgs::msg::PointClusters;
using Cluster = decltype(Clusters::clusters)::value_type;

class EUCLIDEAN_CLUSTER_PUBLIC Config
{
public:
  Config(
    const std::string & frame_id,
    const std::size_t min_cluster_size,
    const std::size_t max_num_clusters);
  std::size_t min_cluster_size() const;
  std::size_t max_num_clusters() const;
  const std::string & frame_id() const;

private:
  const std::string m_frame_id;
  const std::size_t m_min_cluster_size;
  const std::size_t m_max_num_clusters;
};  // class Config

class EUCLIDEAN_CLUSTER_PUBLIC EuclideanCluster
{
public:
  enum class Error : uint8_t
  {
    NONE = 0U,
    TOO_MANY_CLUSTERS
  };  // enum class Error
  EuclideanCluster(const Config & cfg, const HashConfig & hash_cfg);
  template<typename ... Args>
  void insert(Args && ... args)
  {
    // can't do anything with return values
    (void)m_hash.insert(
      PointXYZII{std::forward<Args>(args)..., static_cast<uint32_t>(m_seen.size())});
    m_seen.push_back(false);
  }

  template<typename IT>
  void insert(const IT begin, const IT end)
  {
    if ((static_cast<std::size_t>(std::distance(begin, end)) + m_hash.size()) > m_hash.capacity()) {
      throw std::length_error{"EuclideanCluster: Multi insert would overrun capacity"};
    }
    for (auto it = begin; it != end; ++it) {
      insert(*it);
    }
  }

  const Clusters & cluster(const builtin_interfaces::msg::Time stamp);

  void cluster(Clusters & clusters);

  Error get_error() const;

  void cleanup(Clusters & clusters);

  const Config & get_config() const;

private:
  struct PointXY
  {
    float32_t x = 0.0f;
    float32_t y = 0.0f;
  };  // struct PointXYZ
  EUCLIDEAN_CLUSTER_LOCAL void cluster_impl(Clusters & clusters);
  EUCLIDEAN_CLUSTER_LOCAL void cluster(Clusters & clusters, const PointXYZII & pt);
  EUCLIDEAN_CLUSTER_LOCAL void add_neighbors(Cluster & cls, const PointXY pt);
  EUCLIDEAN_CLUSTER_LOCAL static void add_point(Cluster & cls, const PointXYZII & pt);
  EUCLIDEAN_CLUSTER_LOCAL static PointXY get_point(const Cluster & cls, const std::size_t idx);
  EUCLIDEAN_CLUSTER_LOCAL void return_clusters(Clusters & clusters);

  const Config m_config;
  Hash m_hash;
  Clusters m_clusters;
  decltype(Clusters::clusters) m_cluster_pool;
  Error m_last_error;
  std::vector<bool8_t> m_seen;
};  // class EuclideanCluster

namespace details
{
using BoundingBox = autoware_auto_msgs::msg::BoundingBox;
using BoundingBoxArray = autoware_auto_msgs::msg::BoundingBoxArray;
EUCLIDEAN_CLUSTER_PUBLIC BoundingBox compute_lfit_bounding_box(Cluster & cls);
EUCLIDEAN_CLUSTER_PUBLIC BoundingBox compute_eigenbox(const Cluster & cls);
EUCLIDEAN_CLUSTER_PUBLIC
void compute_lfit_bounding_boxes(Clusters & clusters, BoundingBoxArray & boxes);
EUCLIDEAN_CLUSTER_PUBLIC
void compute_lfit_bounding_boxes_with_z(Clusters & clusters, BoundingBoxArray & boxes);
EUCLIDEAN_CLUSTER_PUBLIC
void compute_eigenboxes(const Clusters & clusters, BoundingBoxArray & boxes);
EUCLIDEAN_CLUSTER_PUBLIC
void compute_eigenboxes_with_z(const Clusters & clusters, BoundingBoxArray & boxes);
}  // namespace details
}  // namespace euclidean_cluster
}  // namespace segmentation
}  // namespace perception
namespace common
{
namespace geometry
{
namespace point_adapter
{
template<>
inline EUCLIDEAN_CLUSTER_PUBLIC auto x_(
  const perception::segmentation::euclidean_cluster::PointXYZII & pt)
{
  return pt.get_point().x;
}
template<>
inline EUCLIDEAN_CLUSTER_PUBLIC auto y_(
  const perception::segmentation::euclidean_cluster::PointXYZII & pt)
{
  return pt.get_point().y;
}
template<>
inline EUCLIDEAN_CLUSTER_PUBLIC auto z_(
  const perception::segmentation::euclidean_cluster::PointXYZII & pt)
{
  return pt.get_point().z;
}
}  // namespace point_adapter
}  // namespace geometry
}  // namespace common
}  // namespace autoware
#endif  // EUCLIDEAN_CLUSTER__EUCLIDEAN_CLUSTER_HPP_