Branch data     Line data    Source code

       1                 :            : // Copyright 2017-2020 the Autoware Foundation, Arm Limited
       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                 :            : 
      17                 :            : //lint -e537 NOLINT  // cpplint vs pclint
      18                 :            : #include <utility>
      19                 :            : //lint -e537 NOLINT  // cpplint vs pclint
      20                 :            : #include <algorithm>
      21                 :            : #include <cstdint>
      22                 :            : #include <stdexcept>
      23                 :            : 
      24                 :            : #include "common/types.hpp"
      25                 :            : #include "ray_ground_classifier/ray_ground_classifier.hpp"
      26                 :            : #include "ray_ground_classifier/ray_ground_point_classifier.hpp"
      27                 :            : 
      28                 :            : using autoware::common::types::bool8_t;
      29                 :            : using autoware::common::types::float32_t;
      30                 :            : 
      31                 :            : namespace autoware
      32                 :            : {
      33                 :            : namespace perception
      34                 :            : {
      35                 :            : namespace filters
      36                 :            : {
      37                 :            : namespace ray_ground_classifier
      38                 :            : {
      39                 :            : 
      40                 :            : ////////////////////////////////////////////////////////////////////////////////
      41                 :         29 : RayGroundClassifier::RayGroundClassifier(const Config & cfg)
      42                 :            : : m_sort_array(autoware::common::types::POINT_BLOCK_CAPACITY),
      43                 :            :   m_ray_sorter(autoware::common::types::POINT_BLOCK_CAPACITY),
      44   [ +  -  +  - ]:         29 :   m_point_classifier(cfg)
      45                 :            : {
      46                 :            :   m_sort_array.clear();
      47                 :         29 : }
      48                 :            : ////////////////////////////////////////////////////////////////////////////////
      49                 :          0 : RayGroundClassifier::RayGroundClassifier(const RayGroundClassifier & original)
      50                 :          0 : : m_sort_array(original.m_sort_array),
      51                 :            :   m_ray_sorter(original.m_ray_sorter.capacity()),
      52   [ #  #  #  #  :          0 :   m_point_classifier(original.m_point_classifier)
                   #  # ]
      53                 :            : {
      54                 :            :   m_sort_array.clear();
      55                 :          0 : }
      56                 :            : ////////////////////////////////////////////////////////////////////////////////
      57                 :     359434 : void RayGroundClassifier::insert(const PointXYZIF * pt)
      58                 :            : {
      59                 :     359434 :   insert(PointXYZIFR{pt});
      60                 :     359433 : }
      61                 :            : ////////////////////////////////////////////////////////////////////////////////
      62                 :     359434 : void RayGroundClassifier::insert(const PointXYZIFR & pt)
      63                 :            : {
      64         [ +  + ]:     359434 :   if (m_sort_array.size() >= m_sort_array.capacity()) {
      65         [ +  - ]:          1 :     throw std::runtime_error("RayGroundClassifier: cannot insert into full array");
      66                 :            :   }
      67                 :     359433 :   m_sort_array.push_back(pt);
      68                 :     359433 : }
      69                 :            : ////////////////////////////////////////////////////////////////////////////////
      70                 :        774 : bool8_t RayGroundClassifier::can_fit_result(
      71                 :            :   const Ray & ray,
      72                 :            :   const PointPtrBlock & ground_block,
      73                 :            :   const PointPtrBlock & nonground_block) const
      74                 :            : {
      75         [ +  + ]:        774 :   return (ray.size() + std::max(ground_block.size(), nonground_block.size())) <=
      76                 :        774 :          autoware::common::types::POINT_BLOCK_CAPACITY;
      77                 :            : }
      78                 :            : ////////////////////////////////////////////////////////////////////////////////
      79                 :         25 : bool8_t RayGroundClassifier::can_fit_result(
      80                 :            :   const PointPtrBlock & ground_block,
      81                 :            :   const PointPtrBlock & nonground_block) const
      82                 :            : {
      83                 :         25 :   return can_fit_result(m_sort_array, ground_block, nonground_block);
      84                 :            : }
      85                 :            : ////////////////////////////////////////////////////////////////////////////////
      86                 :         25 : void RayGroundClassifier::partition(
      87                 :            :   PointPtrBlock & ground_block,
      88                 :            :   PointPtrBlock & nonground_block,
      89                 :            :   const bool8_t presorted)
      90                 :            : {
      91         [ +  + ]:         25 :   if (!presorted) {
      92                 :            :     sort_ray();
      93                 :            :   }
      94                 :            :   segment_ray(ground_block, nonground_block);
      95                 :         23 : }
      96                 :            : ////////////////////////////////////////////////////////////////////////////////
      97         [ +  + ]:        703 : void RayGroundClassifier::structured_partition(
      98                 :            :   const PointPtrBlock & raw_block,
      99                 :            :   PointPtrBlock & ground_block,
     100                 :            :   PointPtrBlock & nonground_block)
     101                 :            : {
     102                 :            :   // reset blocks
     103                 :            :   ground_block.clear();
     104                 :            :   nonground_block.clear();
     105                 :            :   // insert points and segment rays
     106                 :        703 :   uint16_t last_id = raw_block[0U]->id;
     107         [ +  + ]:     359167 :   for (const PointXYZIF * pt : raw_block) {
     108                 :     358465 :     const uint16_t id = pt->id;
     109                 :            :     // terminal scan, add to both
     110         [ +  + ]:     358465 :     if (static_cast<uint16_t>(autoware::common::types::PointXYZIF::END_OF_SCAN_ID) == id) {
     111                 :          1 :       insert(ground_block, pt);
     112                 :          1 :       insert(nonground_block, pt);
     113                 :            :       break;
     114                 :            :     }
     115         [ +  + ]:     358464 :     if (id != last_id) {
     116                 :            :       // we have started a new ray; last ray is complete
     117                 :            :       sort_ray();
     118                 :            :       segment_ray(ground_block, nonground_block);
     119                 :            :       // reset stuff
     120                 :            :       last_id = id;
     121                 :            :     }
     122                 :     358464 :     insert(pt);
     123                 :            :   }
     124                 :            :   // segment last ray
     125                 :            :   sort_ray();
     126                 :            :   segment_ray(ground_block, nonground_block);
     127                 :        703 : }
     128                 :            : 
     129                 :            : ////////////////////////////////////////////////////////////////////////////////
     130                 :          0 : void RayGroundClassifier::sort_ray()
     131                 :            : {
     132                 :            :   // sort by radial distance
     133                 :        728 :   m_ray_sorter.sort(m_sort_array.begin(), m_sort_array.end());
     134                 :         24 : }
     135                 :            : ////////////////////////////////////////////////////////////////////////////////
     136                 :          0 : void RayGroundClassifier::insert(PointPtrBlock & block, const PointXYZIF * pt)
     137                 :            : {
     138                 :     359454 :   block.push_back(pt);
     139                 :          0 : }
     140                 :            : ////////////////////////////////////////////////////////////////////////////////
     141                 :          0 : void RayGroundClassifier::segment_ray(PointPtrBlock & ground_block, PointPtrBlock & nonground_block)
     142                 :            : {
     143                 :        729 :   partition(m_sort_array, ground_block, nonground_block);
     144                 :            :   // reset internal array
     145                 :            :   m_sort_array.clear();  // capacity unchanged
     146                 :          0 : }
     147                 :            : ////////////////////////////////////////////////////////////////////////////////
     148                 :        749 : void RayGroundClassifier::partition(
     149                 :            :   const Ray & ray,
     150                 :            :   PointPtrBlock & ground_block,
     151                 :            :   PointPtrBlock & nonground_block)
     152                 :            : {
     153                 :            :   // Make sure result can fit
     154         [ +  + ]:        749 :   if (!can_fit_result(ray, ground_block, nonground_block)) {
     155         [ +  - ]:          2 :     throw std::runtime_error("RayGroundClassifier: Blocks cannot fit partition result");
     156                 :            :   }
     157                 :            :   // reset classifier
     158                 :        747 :   m_point_classifier.reset();
     159                 :            :   // filter and push to appropriate queues
     160                 :            :   const PointXYZIF * last_point_ptr = nullptr;
     161                 :            :   RayGroundPointClassifier::PointLabel last_label =
     162                 :            :     RayGroundPointClassifier::PointLabel::NONGROUND;
     163         [ +  + ]:     360200 :   for (std::size_t idx = 0U; idx < ray.size(); ++idx) {
     164                 :            :     const std::size_t jdx = idx;  // fixes PCLint FP: idx modified in loop
     165                 :            :     const PointXYZIFR & pt = ray[jdx];
     166                 :     359453 :     const RayGroundPointClassifier::PointLabel label = m_point_classifier.is_ground(pt);
     167                 :            :     // modify label of last point
     168                 :     359453 :     if (((label == RayGroundPointClassifier::PointLabel::NONGROUND) &&
     169   [ +  +  +  + ]:     359453 :       (last_label == RayGroundPointClassifier::PointLabel::PROVISIONAL_GROUND)) ||
     170                 :            :       (label == RayGroundPointClassifier::PointLabel::RETRO_NONGROUND))
     171                 :            :     {
     172                 :            :       last_label = RayGroundPointClassifier::PointLabel::NONGROUND;
     173                 :            :     }
     174                 :            :     // push last point accordingly
     175         [ +  + ]:     359453 :     if (last_point_ptr != nullptr) {
     176         [ +  + ]:     358708 :       if (RayGroundPointClassifier::label_is_ground(last_label)) {
     177                 :     146973 :         insert(ground_block, last_point_ptr);
     178                 :            :       } else {
     179                 :     211735 :         insert(nonground_block, last_point_ptr);
     180                 :            :       }
     181                 :            :     }
     182                 :            :     // update state
     183                 :     359453 :     last_point_ptr = pt.get_point_pointer();
     184                 :            :     last_label = label;
     185                 :            :   }
     186                 :            :   // push trailing point
     187         [ +  + ]:        747 :   if (last_point_ptr != nullptr) {
     188         [ +  + ]:        745 :     if (RayGroundPointClassifier::label_is_ground(last_label)) {
     189                 :        731 :       insert(ground_block, last_point_ptr);
     190                 :            :     } else {
     191                 :         14 :       insert(nonground_block, last_point_ptr);
     192                 :            :     }
     193                 :            :   }
     194                 :        747 : }
     195                 :            : 
     196                 :            : }  // namespace ray_ground_classifier
     197                 :            : }  // namespace filters
     198                 :            : }  // namespace perception
     199                 :            : }  // namespace autoware