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