Autoware.Auto
ray_ground_point_classifier.hpp
Go to the documentation of this file.
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 
19 
20 #ifndef RAY_GROUND_CLASSIFIER__RAY_GROUND_POINT_CLASSIFIER_HPP_
21 #define RAY_GROUND_CLASSIFIER__RAY_GROUND_POINT_CLASSIFIER_HPP_
22 
23 #include <cmath>
24 #include <limits>
25 #include <vector>
26 
27 #include "common/types.hpp"
30 
31 namespace autoware
32 {
35 namespace perception
36 {
40 namespace filters
41 {
49 
52 namespace ray_ground_classifier
53 {
58 inline float32_t angle_distance_rad(const float32_t th_rad, const float32_t phi_rad)
59 {
60  return fmodf((th_rad - phi_rad) + PI + TAU, TAU) - PI;
61 }
62 
63 inline float64_t deg2rad(float64_t degrees)
64 {
65  return degrees * 4.0 * atan(1.0) / 180.0;
66 }
67 
69 struct RAY_GROUND_CLASSIFIER_PUBLIC Config
70 {
86  Config(
87  const float32_t sensor_height_m,
88  const float32_t max_local_slope_deg,
89  const float32_t max_global_slope_deg,
90  const float32_t nonground_retro_thresh_deg,
91  const float32_t min_height_thresh_m,
92  const float32_t max_global_height_thresh_m,
93  const float32_t max_last_local_ground_thresh_m,
94  const float32_t max_provisional_ground_distance_m);
97  float32_t get_sensor_height() const;
98 
101 
105 
109 
113 
116 
119 
122 
125 }; // class Config
126 
129 class RAY_GROUND_CLASSIFIER_PUBLIC PointXYZIFR
130 {
131  friend bool8_t operator<(const PointXYZIFR & lhs, const PointXYZIFR & rhs) noexcept;
132 
133 public:
135  PointXYZIFR() = default;
138  explicit PointXYZIFR(const PointXYZIF * pt);
141  float32_t get_r() const;
144  float32_t get_z() const;
145 
148  const PointXYZIF * get_point_pointer() const;
149 
150 private:
151  const PointXYZIF * m_point;
152  float32_t m_r_xy;
153 }; // class PointXYZIFR
154 
159 inline bool8_t operator<(const PointXYZIFR & lhs, const PointXYZIFR & rhs) noexcept
160 {
161  const auto same_radius = abs_eq(
162  lhs.m_r_xy, rhs.m_r_xy,
163  std::numeric_limits<decltype(lhs.m_r_xy)>::epsilon());
164  return same_radius ? (lhs.m_point->z < rhs.m_point->z) : (lhs.m_r_xy < rhs.m_r_xy);
165 }
166 
167 using Ray = std::vector<PointXYZIFR>;
168 
172 class RAY_GROUND_CLASSIFIER_PUBLIC RayGroundPointClassifier
173 {
174 public:
176  enum class PointLabel : int8_t
177  {
179  GROUND = 0,
181  NONGROUND = 1,
184  PROVISIONAL_GROUND = -1,
186  RETRO_NONGROUND = 2,
188  NONLOCAL_NONGROUND = 3
189  };
190 
194  explicit RayGroundPointClassifier(const Config & config);
195 
197  explicit RayGroundPointClassifier(const RayGroundPointClassifier & original);
198 
200  void reset();
201 
210  PointLabel is_ground(const PointXYZIFR & pt);
211 
215  static bool8_t label_is_ground(const PointLabel label);
216 
217 private:
219  float32_t m_prev_radius_m;
220  float32_t m_prev_height_m;
221  float32_t m_prev_ground_radius_m;
222  float32_t m_prev_ground_height_m;
223  bool8_t m_last_was_ground;
225  const Config m_config;
226 }; // RayGroundPointClassifier
227 } // namespace ray_ground_classifier
228 } // namespace filters
229 } // namespace perception
230 } // namespace autoware
231 
232 #endif // RAY_GROUND_CLASSIFIER__RAY_GROUND_POINT_CLASSIFIER_HPP_
autoware::perception::filters::ray_ground_classifier::deg2rad
float64_t deg2rad(float64_t degrees)
Definition: ray_ground_point_classifier.hpp:63
autoware::perception::filters::ray_ground_classifier::Config::m_max_local_slope
const float32_t m_max_local_slope
Get maximum allowed slope between two nearby points that are both ground, the value is a nondimension...
Definition: ray_ground_point_classifier.hpp:104
autoware::perception::filters::ray_ground_classifier::RayGroundPointClassifier::PointLabel
PointLabel
Return codes from is_ground()
Definition: ray_ground_point_classifier.hpp:176
autoware::perception::filters::ray_ground_classifier::Config::m_max_global_height_thresh_m
const float32_t m_max_global_height_thresh_m
The maximum value for global height threshold.
Definition: ray_ground_point_classifier.hpp:118
autoware::perception::filters::ray_ground_classifier::Ray
std::vector< PointXYZIFR > Ray
Definition: ray_ground_point_classifier.hpp:167
autoware::perception::filters::ray_ground_classifier::Config::m_nonground_retro_thresh
const float32_t m_nonground_retro_thresh
Get minimum slope at which vertical structure is assumed, the value is a nondimensionalized ratio,...
Definition: ray_ground_point_classifier.hpp:112
autoware::common::types::TAU
constexpr float32_t TAU
tau = 2 pi
Definition: types.hpp:54
visibility_control.hpp
types.hpp
This file includes common type definition.
PI
const double PI
Definition: reeds_shepp_impl.cpp:91
autoware::perception::filters::ray_ground_classifier::Config::m_ground_z_m
const float32_t m_ground_z_m
Get z value (meters) of the ground in sensor frame, -sensor_height.
Definition: ray_ground_point_classifier.hpp:100
autoware::perception::filters::ray_ground_classifier::Config::m_max_last_local_ground_thresh_m
const float32_t m_max_last_local_ground_thresh_m
The maximum value for local heigh threshold.
Definition: ray_ground_point_classifier.hpp:121
autoware::common::types::PointXYZIF
Definition: types.hpp:56
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
float_comparisons.hpp
autoware::perception::filters::ray_ground_classifier::Config::m_max_provisional_ground_distance_m
const float32_t m_max_provisional_ground_distance_m
The maximum influence distance for provisional ground point label.
Definition: ray_ground_point_classifier.hpp:124
METRIC::GROUND
@ GROUND
autoware::perception::filters::ray_ground_classifier::Config::m_min_height_thresh_m
const float32_t m_min_height_thresh_m
Get minimum value for local height threshold.
Definition: ray_ground_point_classifier.hpp:115
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
autoware::perception::filters::ray_ground_classifier::operator<
bool8_t operator<(const PointXYZIFR &lhs, const PointXYZIFR &rhs) noexcept
Comparison operator for default sorting.
Definition: ray_ground_point_classifier.hpp:159
autoware::common::helper_functions::comparisons::abs_eq
bool abs_eq(const T &a, const T &b, const T &eps)
Check for approximate equality in absolute terms.
Definition: float_comparisons.hpp:43
autoware::perception::filters::ray_ground_classifier::angle_distance_rad
float32_t angle_distance_rad(const float32_t th_rad, const float32_t phi_rad)
th_rad - phi_rad, normalized to +/- pi
Definition: ray_ground_point_classifier.hpp:58
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
autoware::perception::filters::ray_ground_classifier::PointXYZIFR
This is a simplified point view of a ray. The only information needed is height and projected radial ...
Definition: ray_ground_point_classifier.hpp:129
autoware::perception::filters::ray_ground_classifier::Config::m_max_global_slope
const float32_t m_max_global_slope
Get maximum allowed slope between a ground point and the sensor, the value is a nondimensionalized ra...
Definition: ray_ground_point_classifier.hpp:108
autoware::perception::filters::ray_ground_classifier::Config
A struct that holds configuration parameters for the ground filter.
Definition: ray_ground_point_classifier.hpp:69
autoware::perception::filters::ray_ground_classifier::RayGroundPointClassifier
Simple stateful implementation of ray ground filter: https://github.com/CPFL/Autoware/blob/develop/ro...
Definition: ray_ground_point_classifier.hpp:172
autoware::common::types::PI
constexpr float32_t PI
pi = tau / 2
Definition: types.hpp:50