LCOV - code coverage report
Current view: top level - src/perception/filters/ray_ground_classifier/src - ray_ground_point_classifier.cpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 44 49 89.8 %
Date: 2023-03-03 05:44:19 Functions: 4 5 80.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 27 28 96.4 %

           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 <algorithm>
      19                 :            : #include <cmath>
      20                 :            : #include <limits>
      21                 :            : #include <stdexcept>
      22                 :            : 
      23                 :            : #include "common/types.hpp"
      24                 :            : #include "geometry/interval.hpp"
      25                 :            : #include "ray_ground_classifier/ray_ground_point_classifier.hpp"
      26                 :            : 
      27                 :            : using autoware::common::types::bool8_t;
      28                 :            : using autoware::common::types::float32_t;
      29                 :            : using autoware::common::geometry::Interval_f;
      30                 :            : 
      31                 :            : namespace autoware
      32                 :            : {
      33                 :            : namespace perception
      34                 :            : {
      35                 :            : namespace filters
      36                 :            : {
      37                 :            : namespace ray_ground_classifier
      38                 :            : {
      39                 :            : 
      40                 :            : ////////////////////////////////////////////////////////////////////////////////
      41                 :         30 : RayGroundPointClassifier::RayGroundPointClassifier(
      42                 :         30 :   const Config & config)
      43                 :         30 : : m_config(config)
      44                 :            : {
      45                 :         30 :   reset();
      46                 :         30 : }
      47                 :            : 
      48                 :            : ////////////////////////////////////////////////////////////////////////////////
      49                 :          0 : RayGroundPointClassifier::RayGroundPointClassifier(
      50                 :          0 :   const RayGroundPointClassifier & original)
      51                 :          0 : : m_config(original.m_config)
      52                 :            : {
      53                 :          0 :   reset();
      54                 :          0 : }
      55                 :            : 
      56                 :            : ////////////////////////////////////////////////////////////////////////////////
      57                 :        777 : void RayGroundPointClassifier::reset()
      58                 :            : {
      59                 :        777 :   m_prev_radius_m = 0.0F;
      60                 :        777 :   m_prev_height_m = m_config.m_ground_z_m;
      61                 :        777 :   m_last_was_ground = true;
      62                 :        777 :   m_prev_ground_radius_m = 0.0F;
      63                 :        777 :   m_prev_ground_height_m = m_config.m_ground_z_m;
      64                 :        777 : }
      65                 :            : 
      66                 :            : ////////////////////////////////////////////////////////////////////////////////
      67                 :     359459 : RayGroundPointClassifier::PointLabel RayGroundPointClassifier::is_ground(const PointXYZIFR & pt)
      68                 :            : {
      69                 :            :   // consider inline if benchmarkings shows that this is slow
      70                 :            :   PointLabel ret;
      71                 :     359459 :   const float32_t height_m = pt.get_z();
      72                 :     359459 :   const float32_t radius_m = pt.get_r();
      73                 :            : 
      74                 :            :   // a small fudge factor is added because we check in the sorting process for "almost zero"
      75                 :            :   // This is because points which are almost collinear are sorted by height
      76                 :     359459 :   const float32_t dr_m = (radius_m - m_prev_radius_m) +
      77                 :            :     std::numeric_limits<decltype(dr_m)>::epsilon();
      78         [ +  + ]:     359459 :   if (dr_m < 0.0F) {
      79         [ +  - ]:          1 :     throw std::runtime_error("Ray Ground filter must receive points in increasing radius");
      80                 :            :   }
      81                 :            : 
      82                 :     359458 :   const float32_t dh_m = fabsf(height_m - m_prev_height_m);
      83                 :     359458 :   const Interval_f range(m_config.m_min_height_thresh_m, m_config.m_max_global_height_thresh_m);
      84         [ +  + ]:     359458 :   const auto dr_m_clamped = Interval_f::clamp_to(range, m_config.m_max_local_slope * dr_m);
      85                 :            :   const bool8_t is_local = (dh_m < dr_m_clamped);
      86                 :            :   const float32_t global_height_thresh_m =
      87         [ +  + ]:     359458 :     std::min(m_config.m_max_global_slope * radius_m, m_config.m_max_global_height_thresh_m);
      88                 :     359458 :   const bool8_t has_vertical_structure = (dh_m > (dr_m * m_config.m_nonground_retro_thresh));
      89         [ +  + ]:     359458 :   if (m_last_was_ground) {
      90         [ +  + ]:     231351 :     if (is_local) {
      91                 :            :       // local in height, so ground
      92                 :            :       ret = PointLabel::GROUND;
      93         [ +  + ]:     142155 :     } else if (has_vertical_structure) {
      94                 :            :       // vertical structure, so nonground, need to retroactively annotate provisional gorund
      95                 :            :       ret = PointLabel::RETRO_NONGROUND;
      96                 :            :     } else {
      97                 :            :       // check global cone
      98         [ +  + ]:      58324 :       ret = (fabsf(height_m - m_config.m_ground_z_m) < global_height_thresh_m) ?
      99                 :            :         PointLabel::GROUND :
     100         [ +  + ]:      16409 :         (dr_m < m_config.m_max_provisional_ground_distance_m) ?
     101                 :            :         PointLabel::NONGROUND :
     102                 :            :         PointLabel::NONLOCAL_NONGROUND;
     103                 :            :     }
     104                 :            :   } else {
     105                 :     128107 :     const float32_t drg_m = (radius_m - m_prev_ground_radius_m);
     106                 :     128107 :     const float32_t dhg_m = fabsf(height_m - m_prev_ground_height_m);
     107                 :            :     const Interval_f range(
     108                 :     128107 :       m_config.m_min_height_thresh_m, m_config.m_max_last_local_ground_thresh_m);
     109                 :            :     const auto drg_m_clamped =
     110         [ +  + ]:     128107 :       Interval_f::clamp_to(range, m_config.m_max_local_slope * drg_m);
     111                 :            :     const bool8_t is_local_to_last_ground = (dhg_m <= drg_m_clamped);
     112         [ +  + ]:     128107 :     if (is_local_to_last_ground) {
     113                 :            :       // local to last ground: provisional ground
     114                 :            :       ret = PointLabel::PROVISIONAL_GROUND;
     115         [ +  + ]:      83003 :     } else if (is_local) {
     116                 :            :       // local in height, so nonground
     117                 :            :       ret = PointLabel::NONGROUND;
     118                 :            :     } else {
     119                 :            :       // global ground: provisionally ground
     120         [ +  + ]:      75086 :       ret = ((fabsf(height_m - m_config.m_ground_z_m) < global_height_thresh_m)) ?
     121                 :            :         PointLabel::PROVISIONAL_GROUND :
     122                 :            :         PointLabel::NONGROUND;
     123                 :            :     }
     124                 :            :   }
     125                 :            :   // update state
     126                 :     359458 :   m_last_was_ground = label_is_ground(ret);
     127                 :     359458 :   m_prev_radius_m = radius_m;
     128                 :     359458 :   m_prev_height_m = height_m;
     129         [ +  + ]:     359458 :   if (m_last_was_ground) {
     130                 :     231336 :     m_prev_ground_radius_m = radius_m;
     131                 :     231336 :     m_prev_ground_height_m = height_m;
     132                 :            :   }
     133                 :            : 
     134                 :     359458 :   return ret;
     135                 :            : }
     136                 :            : 
     137                 :            : ////////////////////////////////////////////////////////////////////////////////
     138                 :     718911 : bool8_t RayGroundPointClassifier::label_is_ground(const RayGroundPointClassifier::PointLabel label)
     139                 :            : {
     140                 :     718911 :   return static_cast<int8_t>(label) <= static_cast<int8_t>(0);
     141                 :            : }
     142                 :            : 
     143                 :            : }  // namespace ray_ground_classifier
     144                 :            : }  // namespace filters
     145                 :            : }  // namespace perception
     146                 :            : }  // namespace autoware

Generated by: LCOV version 1.14