Branch data     Line data    Source code

       1                 :            : // Copyright 2019-2021 the Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : /// \file
      17                 :            : /// \brief This file defines the euclidean cluster algorithm for object detection
      18                 :            : 
      19                 :            : #ifndef EUCLIDEAN_CLUSTER__EUCLIDEAN_CLUSTER_HPP_
      20                 :            : #define EUCLIDEAN_CLUSTER__EUCLIDEAN_CLUSTER_HPP_
      21                 :            : #include <euclidean_cluster/visibility_control.hpp>
      22                 :            : 
      23                 :            : #include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
      24                 :            : #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
      25                 :            : #include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
      26                 :            : #include <geometry/spatial_hash.hpp>
      27                 :            : #include <common/types.hpp>
      28                 :            : #include <limits>
      29                 :            : #include <string>
      30                 :            : #include <utility>
      31                 :            : #include <vector>
      32                 :            : 
      33                 :            : namespace autoware
      34                 :            : {
      35                 :            : namespace perception
      36                 :            : {
      37                 :            : namespace segmentation
      38                 :            : {
      39                 :            : /// \brief Supporting classes for euclidean clustering, an object detection algorithm
      40                 :            : namespace euclidean_cluster
      41                 :            : {
      42                 :            : using autoware::common::types::float32_t;
      43                 :            : using autoware::common::types::bool8_t;
      44                 :            : /// \brief Simple point struct for memory mapping to and from PointCloud2 type
      45                 :            : struct PointXYZI
      46                 :            : {
      47                 :            :   float32_t x = 0.0f;
      48                 :            :   float32_t y = 0.0f;
      49                 :            :   float32_t z = 0.0f;
      50                 :            :   float32_t intensity = 0.0f;
      51                 :            : };  // struct PointXYZI
      52                 :            : 
      53                 :            : /// \brief Helper point for which euclidean distance is computed only once
      54                 :            : class EUCLIDEAN_CLUSTER_PUBLIC PointXYZIR
      55                 :            : {
      56                 :            : public:
      57                 :            :   PointXYZIR() = default;
      58                 :            :   /// \brief Conversion constructor
      59                 :            :   /// \param[in] pt The point to convert
      60                 :            :   explicit PointXYZIR(const common::types::PointXYZIF & pt);
      61                 :            :   /// \brief Conversion constructor
      62                 :            :   /// \param[in] pt The point to convert
      63                 :            :   explicit PointXYZIR(const PointXYZI & pt);
      64                 :            :   /// \brief Constructor
      65                 :            :   /// \param[in] x The x position of the point
      66                 :            :   /// \param[in] y The y position of the point
      67                 :            :   /// \param[in] z The z position of the point
      68                 :            :   /// \param[in] intensity The intensity value of the point
      69                 :            :   PointXYZIR(const float32_t x, const float32_t y, const float32_t z, const float32_t intensity);
      70                 :            :   /// \brief Getter for radius
      71                 :            :   /// \return The projected radial distance
      72                 :            :   float32_t get_r() const;
      73                 :            :   /// \brief Get core point
      74                 :            :   /// \return Reference to internally stored point
      75                 :            :   const PointXYZI & get_point() const;
      76                 :            :   /// \brief Explicit conversion operator from PointXYZIR to msg type PointXYZIF
      77                 :            :   /// \return a PointXYZIF type
      78                 :            :   explicit operator autoware_auto_perception_msgs::msg::PointXYZIF() const;
      79                 :            : 
      80                 :            : private:
      81                 :            :   // This could instead be a pointer; I'm pretty sure ownership would work out, but I'm
      82                 :            :   // uncomfortable doing it that way (12 vs 20 bytes)
      83                 :            :   PointXYZI m_point;
      84                 :            :   float32_t m_r_xy;
      85                 :            : };  // class PointXYZIR
      86                 :            : 
      87                 :            : using HashConfig = autoware::common::geometry::spatial_hash::Config2d;
      88                 :            : using Hash = autoware::common::geometry::spatial_hash::SpatialHash2d<PointXYZIR>;
      89                 :            : using Clusters = autoware_auto_perception_msgs::msg::PointClusters;
      90                 :            : 
      91                 :            : class EUCLIDEAN_CLUSTER_PUBLIC FilterConfig
      92                 :            : {
      93                 :            : public:
      94                 :            :   /// \brief Constructor
      95                 :            :   /// \param[in] min_filter_x The minimum size of the x component of bounding box
      96                 :            :   /// \param[in] min_filter_y The minimum size of the y component of bounding box
      97                 :            :   /// \param[in] min_filter_z The minimum size of the z component of bounding box
      98                 :            :   /// \param[in] max_filter_x The maximum size of the x component of bounding box
      99                 :            :   /// \param[in] max_filter_y The maximum size of the y component of bounding box
     100                 :            :   /// \param[in] max_filter_z The maximum size of the z component of bounding box
     101                 :            :   FilterConfig(
     102                 :            :     const float32_t min_filter_x,
     103                 :            :     const float32_t min_filter_y,
     104                 :            :     const float32_t min_filter_z,
     105                 :            :     const float32_t max_filter_x,
     106                 :            :     const float32_t max_filter_y,
     107                 :            :     const float32_t max_filter_z);
     108                 :            :   /// \brief Gets minimum filter in direction of x
     109                 :            :   /// \return minimum filter of x direction
     110                 :            :   float32_t min_filter_x() const;
     111                 :            :   /// \brief Gets minimum filter in direction of y
     112                 :            :   /// \return minimum filter of y direction
     113                 :            :   float32_t min_filter_y() const;
     114                 :            :   /// \brief Gets minimum filter in direction of z
     115                 :            :   /// \return minimum filter of z direction
     116                 :            :   float32_t min_filter_z() const;
     117                 :            :   /// \brief Gets maximum filter in direction of x
     118                 :            :   /// \return maximum filter of x direction
     119                 :            :   float32_t max_filter_x() const;
     120                 :            :   /// \brief Gets maximum filter in direction of y
     121                 :            :   /// \return maximum filter of y direction
     122                 :            :   float32_t max_filter_y() const;
     123                 :            :   /// \brief Gets maximum filter in direction of z
     124                 :            :   /// \return maximum filter of z direction
     125                 :            :   float32_t max_filter_z() const;
     126                 :            : 
     127                 :            : private:
     128                 :            :   const float32_t m_min_filter_x;
     129                 :            :   const float32_t m_min_filter_y;
     130                 :            :   const float32_t m_min_filter_z;
     131                 :            :   const float32_t m_max_filter_x;
     132                 :            :   const float32_t m_max_filter_y;
     133                 :            :   const float32_t m_max_filter_z;
     134                 :            : };  // class FilterConfig
     135                 :            : 
     136                 :            : /// \brief Configuration class for euclidean cluster
     137                 :            : /// In the future this can become a base class with subclasses defining different
     138                 :            : /// threshold functions. This configuration's threshold function currently assumes isotropy, and
     139                 :            : /// minor details in the clustering implementation also assume this property.
     140                 :            : class EUCLIDEAN_CLUSTER_PUBLIC Config
     141                 :            : {
     142                 :            : public:
     143                 :            :   /// \brief Constructor
     144                 :            :   /// \param[in] frame_id The frame id for which all clusters are initialized with
     145                 :            :   /// \param[in] min_number_of_points_in_cluster The number of points that must be in a cluster
     146                 :            :   ///                                            before it is not considered noise
     147                 :            :   /// \param[in] max_num_clusters The maximum preallocated number of clusters in a scene
     148                 :            :   /// \param[in] min_cluster_threshold_m The minimum connectivity threshold when r = 0
     149                 :            :   /// \param[in] max_cluster_threshold_m The maximum connectivity threshold when
     150                 :            :   ///                                    r = cluster_threshold_saturation_distance
     151                 :            :   /// \param[in] cluster_threshold_saturation_distance_m The distance at which the cluster threshold
     152                 :            :   ///                                                    is clamped to the maximum value
     153                 :            :   Config(
     154                 :            :     const std::string & frame_id,
     155                 :            :     const std::size_t min_number_of_points_in_cluster,
     156                 :            :     const std::size_t max_num_clusters,
     157                 :            :     const float32_t min_cluster_threshold_m,
     158                 :            :     const float32_t max_cluster_threshold_m,
     159                 :            :     const float32_t cluster_threshold_saturation_distance_m);
     160                 :            :   /// \brief Gets minimum number of points needed for a cluster to not be considered noise
     161                 :            :   /// \return Minimum cluster size
     162                 :            :   std::size_t min_number_of_points_in_cluster() const;
     163                 :            :   /// \brief Gets maximum preallocated number of clusters
     164                 :            :   /// \return Maximum number of clusters
     165                 :            :   std::size_t max_num_clusters() const;
     166                 :            :   /// \brief Compute the connectivity threshold for a given point
     167                 :            :   /// \param[in] pt The point whose connectivity criterion will be calculated
     168                 :            :   /// \return The connectivity threshold, in meters
     169                 :            :   float32_t threshold(const PointXYZIR & pt) const;
     170                 :            :   /// \brief Compute the connectivity threshold for a given point
     171                 :            :   /// \param[in] r The projected radial distance of the point
     172                 :            :   /// \return The connectivity threshold, in meters
     173                 :            :   float32_t threshold(const float32_t r) const;
     174                 :            :   /// \brief Get frame id
     175                 :            :   /// \return The frame id
     176                 :            :   const std::string & frame_id() const;
     177                 :            :   /// \brief Check the external clusters size with the EuclideanClusters configuration
     178                 :            :   /// \param[in] clusters The clusters object
     179                 :            :   /// \return True if config is valid
     180                 :            :   bool match_clusters_size(const Clusters & clusters) const;
     181                 :            : 
     182                 :            : private:
     183                 :            :   const std::string m_frame_id;
     184                 :            :   const std::size_t m_min_number_of_points_in_cluster;
     185                 :            :   const std::size_t m_max_num_clusters;
     186                 :            :   const float32_t m_min_thresh_m;
     187                 :            :   const float32_t m_max_distance_m;
     188                 :            :   const float32_t m_thresh_rate;
     189                 :            : };  // class Config
     190                 :            : 
     191                 :            : /// \brief implementation of euclidean clustering for point cloud segmentation
     192                 :            : /// This clas implicitly projects points onto a 2D (x-y) plane, and segments
     193                 :            : /// according to euclidean distance. This can be thought of as a graph-based
     194                 :            : /// approach where points are vertices and edges are defined by euclidean distance
     195                 :            : /// The input to this should be nonground points pased through a voxel grid.
     196                 :            : class EUCLIDEAN_CLUSTER_PUBLIC EuclideanCluster
     197                 :            : {
     198                 :            : public:
     199                 :            :   enum class Error : uint8_t
     200                 :            :   {
     201                 :            :     NONE = 0U,
     202                 :            :     TOO_MANY_CLUSTERS
     203                 :            :   };  // enum class Error
     204                 :            :   /// \brief Constructor
     205                 :            :   /// \param[in] cfg The configuration of the clustering algorithm, contains threshold function
     206                 :            :   /// \param[in] hash_cfg The configuration of the underlying spatial hash, controls the maximum
     207                 :            :   ///                     number of points in a scene
     208                 :            :   /// \param[in] filter_cfg The configuration of the min/max size limit of the bounding boxes
     209                 :            :   EuclideanCluster(
     210                 :            :     const Config & cfg, const HashConfig & hash_cfg,
     211                 :            :     const FilterConfig & filter_cfg);
     212                 :            :   /// \brief Insert an individual point
     213                 :            :   /// \param[in] pt The point to insert
     214                 :            :   /// \throw std::length_error If the underlying spatial hash is full
     215                 :            :   void insert(const PointXYZIR & pt);
     216                 :            :   /// \brief Multi-insert
     217                 :            :   /// \param[in] begin Iterator pointing to to the first point to insert
     218                 :            :   /// \param[in] end Iterator pointing to one past the last point to insert
     219                 :            :   /// \throw std::length_error If the underlying spatial hash is full
     220                 :            :   /// \tparam IT The type of the iterator
     221                 :            :   template<typename IT>
     222                 :            :   void insert(const IT begin, const IT end)
     223                 :            :   {
     224                 :            :     if ((static_cast<std::size_t>(std::distance(begin, end)) + m_hash.size()) > m_hash.capacity()) {
     225                 :            :       throw std::length_error{"EuclideanCluster: Multi insert would overrun capacity"};
     226                 :            :     }
     227                 :            :     for (auto it = begin; it != end; ++it) {
     228                 :            :       insert(PointXYZIR{*it});
     229                 :            :     }
     230                 :            :   }
     231                 :            : 
     232                 :            :   /// \brief Compute the clusters from the inserted points, where the final clusters object lives in
     233                 :            :   ///        another scope.
     234                 :            :   /// \param[inout] clusters The clusters object
     235                 :            :   void cluster(Clusters & clusters);
     236                 :            : 
     237                 :            :   /// \brief Gets last error, intended to be used with clustering with internal cluster result
     238                 :            :   /// This is a separate function rather than using an exception because the main error mode is
     239                 :            :   /// exceeding preallocated cluster capacity. However, throwing an exception would throw away
     240                 :            :   /// perfectly valid information that is still usable in an error state.
     241                 :            :   Error get_error() const;
     242                 :            : 
     243                 :            :   /// \brief Gets the internal configuration class, for use when it was inline generated
     244                 :            :   /// \return Internal configuration class
     245                 :            :   const Config & get_config() const;
     246                 :            : 
     247                 :            :   /// \brief Gets internal configuration class for filters
     248                 :            :   /// \return Internal FilterConfiguration class
     249                 :            :   const FilterConfig & get_filter_config() const;
     250                 :            : 
     251                 :            :   /// \brief Throw the stored error during clustering process
     252                 :            :   /// \throw std::runtime_error If the maximum number of clusters may have been exceeded
     253                 :            :   void throw_stored_error() const;
     254                 :            : 
     255                 :            : private:
     256                 :            :   /// \brief Internal struct instead of pair since I can guarantee some memory stuff
     257                 :            :   struct PointXY
     258                 :            :   {
     259                 :            :     float32_t x = 0.0f;
     260                 :            :     float32_t y = 0.0f;
     261                 :            :   };  // struct PointXYZ
     262                 :            :   /// \brief Do the clustering process, with no error checking
     263                 :            :   EUCLIDEAN_CLUSTER_LOCAL void cluster_impl(Clusters & clusters);
     264                 :            :   /// \brief Compute the next cluster, seeded by the given point, and grown using the remaining
     265                 :            :   ///         points still contained in the hash
     266                 :            :   EUCLIDEAN_CLUSTER_LOCAL void cluster(Clusters & clusters, const Hash::IT it);
     267                 :            :   /// \brief Add all near neighbors of a point to a given cluster
     268                 :            :   EUCLIDEAN_CLUSTER_LOCAL void add_neighbors_to_last_cluster(
     269                 :            :     Clusters & clusters, const PointXY pt);
     270                 :            :   /// \brief Adds a point to the last cluster, internal version since no error checking is needed
     271                 :            :   EUCLIDEAN_CLUSTER_LOCAL static void add_point_to_last_cluster(
     272                 :            :     Clusters & clusters, const PointXYZIR & pt);
     273                 :            :   /// \brief Get a specified point from the cluster
     274                 :            :   EUCLIDEAN_CLUSTER_LOCAL static PointXY get_point_from_last_cluster(
     275                 :            :     const Clusters & clusters, const std::size_t cls_pt_idx);
     276                 :            :   EUCLIDEAN_CLUSTER_LOCAL static std::size_t last_cluster_size(const Clusters & clusters);
     277                 :            : 
     278                 :            :   const Config m_config;
     279                 :            :   Hash m_hash;
     280                 :            :   const FilterConfig m_filter_config;
     281                 :            :   Error m_last_error;
     282                 :            :   std::vector<bool8_t> m_seen;
     283                 :            : };  // class EuclideanCluster
     284                 :            : 
     285                 :            : /// \brief Common euclidean cluster functions not intended for external use
     286                 :            : namespace details
     287                 :            : {
     288                 :            : using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
     289                 :            : using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray;
     290                 :            : using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
     291                 :            : 
     292                 :            : enum class BboxMethod
     293                 :            : {
     294                 :            :   Eigenbox,
     295                 :            :   LFit,
     296                 :            : };
     297                 :            : /// \brief Compute bounding boxes from clusters
     298                 :            : /// \param[in] method Whether to use the eigenboxes or L-Fit algorithm.
     299                 :            : /// \param[in] compute_height Compute the height of the bounding box as well.
     300                 :            : /// \param[inout] clusters A set of clusters for which to compute the bounding boxes. Individual
     301                 :            : ///                        clusters may get their points shuffled.
     302                 :            : /// \param[in] size_filter true if you want to clamp the bounding boxes size by min/max parameters
     303                 :            : /// \param[in] filter_config The max/min limit of the xyz component of bounding box
     304                 :            : /// \returns Bounding boxes
     305                 :            : EUCLIDEAN_CLUSTER_PUBLIC
     306                 :            : BoundingBoxArray compute_bounding_boxes(
     307                 :            :   Clusters & clusters, const BboxMethod method, const bool compute_height,
     308                 :            :   const bool size_filter = false,
     309                 :            :   const FilterConfig & filter_config = {0.0F, 0.0F, 0.0F,
     310                 :            :     std::numeric_limits<float>::max(),
     311                 :            :     std::numeric_limits<float>::max(),
     312                 :            :     std::numeric_limits<float>::max()});
     313                 :            : 
     314                 :            : /// \brief Convert this bounding box to a DetectedObjects message
     315                 :            : /// \param[in] boxes A bounding box array
     316                 :            : /// \returns A DetectedObjects message with the bounding boxes inside
     317                 :            : EUCLIDEAN_CLUSTER_PUBLIC
     318                 :            : DetectedObjects convert_to_detected_objects(const BoundingBoxArray & boxes);
     319                 :            : /// \brief Convert this clusters to polygon prisms as a DetectedObjects message
     320                 :            : /// \param[in] clusters A set of clusters for which to compute the polygon prisms
     321                 :            : /// \returns A DetectedObjects message with the polygon prism inside
     322                 :            : EUCLIDEAN_CLUSTER_PUBLIC
     323                 :            : DetectedObjects convert_to_polygon_prisms(const Clusters & clusters);
     324                 :            : }  // namespace details
     325                 :            : }  // namespace euclidean_cluster
     326                 :            : }  // namespace segmentation
     327                 :            : }  // namespace perception
     328                 :            : namespace common
     329                 :            : {
     330                 :            : namespace geometry
     331                 :            : {
     332                 :            : namespace point_adapter
     333                 :            : {
     334                 :            : template<>
     335                 :            : inline EUCLIDEAN_CLUSTER_PUBLIC auto x_(
     336                 :            :   const perception::segmentation::euclidean_cluster::PointXYZIR & pt)
     337                 :            : {
     338                 :        647 :   return pt.get_point().x;
     339                 :            : }
     340                 :            : template<>
     341                 :            : inline EUCLIDEAN_CLUSTER_PUBLIC auto y_(
     342                 :            :   const perception::segmentation::euclidean_cluster::PointXYZIR & pt)
     343                 :            : {
     344                 :        647 :   return pt.get_point().y;
     345                 :            : }
     346                 :            : template<>
     347                 :            : inline EUCLIDEAN_CLUSTER_PUBLIC auto z_(
     348                 :            :   const perception::segmentation::euclidean_cluster::PointXYZIR & pt)
     349                 :            : {
     350                 :        647 :   return pt.get_point().z;
     351                 :            : }
     352                 :            : }  // namespace point_adapter
     353                 :            : }  // namespace geometry
     354                 :            : }  // namespace common
     355                 :            : }  // namespace autoware
     356                 :            : #endif  // EUCLIDEAN_CLUSTER__EUCLIDEAN_CLUSTER_HPP_