Skip to content

Commit a60b580

Browse files
taisa1pre-commit-ci[bot]atsushi421
authored andcommitted
perf(common_ground_filter): performance tuning (autowarefoundation#5861)
* perf(scan_ground_filter): performance tuning Signed-off-by: taisa1 <kento.osa@tier4.jp> * perf(scan_ground_filter): improved performance Signed-off-by: taisa1 <kento.osa@tier4.jp> * chore: refactoring Signed-off-by: taisa1 <kento.osa@tier4.jp> * chore: refactoring Signed-off-by: taisa1 <kento.osa@tier4.jp> * refactor: reflect review Signed-off-by: taisa1 <kento.osa@tier4.jp> * style(pre-commit): autofix * fix: remove unnecessary comment Signed-off-by: taisa1 <kento.osa@tier4.jp> * test: change unit test to use faster_filter Signed-off-by: taisa1 <kento.osa@tier4.jp> * test: fixed compile error Signed-off-by: taisa1 <kento.osa@tier4.jp> * fix: add lacking parameter callback Signed-off-by: taisa1 <kento.osa@tier4.jp> * fix: uninitialized valuabe Signed-off-by: atsushi421 <atsushi.yano.2@tier4.jp> --------- Signed-off-by: taisa1 <kento.osa@tier4.jp> Signed-off-by: atsushi421 <atsushi.yano.2@tier4.jp> Co-authored-by: taisa1 <kento.osa@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: atsushi yano <55824710+atsushi421@users.noreply.github.com> Co-authored-by: atsushi421 <atsushi.yano.2@tier4.jp>
1 parent 128ad80 commit a60b580

File tree

4 files changed

+310
-183
lines changed

4 files changed

+310
-183
lines changed

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

+57-29
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_
1717

1818
#include "pointcloud_preprocessor/filter.hpp"
19+
#include "pointcloud_preprocessor/transform_info.hpp"
1920

2021
#include <vehicle_info_util/vehicle_info.hpp>
2122

@@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
5051
// classified point label
5152
// (0: not classified, 1: ground, 2: not ground, 3: follow previous point,
5253
// 4: unkown(currently not used), 5: virtual ground)
53-
enum class PointLabel {
54+
enum class PointLabel : uint16_t {
5455
INIT = 0,
5556
GROUND,
5657
NON_GROUND,
@@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
5960
VIRTUAL_GROUND,
6061
OUT_OF_RANGE
6162
};
62-
struct PointRef
63+
64+
struct PointData
6365
{
64-
float grid_size; // radius of grid
65-
uint16_t grid_id; // id of grid in vertical
66-
float radius; // cylindrical coords on XY Plane
67-
float theta; // angle deg on XY plane
68-
size_t radial_div; // index of the radial division to which this point belongs to
66+
float radius; // cylindrical coords on XY Plane
6967
PointLabel point_state{PointLabel::INIT};
70-
68+
uint16_t grid_id; // id of grid in vertical
7169
size_t orig_index; // index of this point in the source pointcloud
72-
pcl::PointXYZ * orig_point;
7370
};
74-
using PointCloudRefVector = std::vector<PointRef>;
71+
using PointCloudVector = std::vector<PointData>;
7572

7673
struct GridCenter
7774
{
@@ -144,33 +141,57 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
144141

145142
pcl::PointIndices getIndices() { return pcl_indices; }
146143
std::vector<float> getHeightList() { return height_list; }
144+
145+
pcl::PointIndices & getIndicesRef() { return pcl_indices; }
146+
std::vector<float> & getHeightListRef() { return height_list; }
147147
};
148148

149149
void filter(
150150
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
151151

152+
// TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes
153+
// conform to new API
154+
virtual void faster_filter(
155+
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
156+
const pointcloud_preprocessor::TransformInfo & transform_info);
157+
152158
tf2_ros::Buffer tf_buffer_{get_clock()};
153159
tf2_ros::TransformListener tf_listener_{tf_buffer_};
154160

161+
int x_offset_;
162+
int y_offset_;
163+
int z_offset_;
164+
int intensity_offset_;
165+
bool offset_initialized_;
166+
167+
void set_field_offsets(const PointCloud2ConstPtr & input);
168+
169+
void get_point_from_global_offset(
170+
const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset);
171+
155172
const uint16_t gnd_grid_continual_thresh_ = 3;
156173
bool elevation_grid_mode_;
157174
float non_ground_height_threshold_;
158175
float grid_size_rad_;
176+
float tan_grid_size_rad_;
159177
float grid_size_m_;
160178
float low_priority_region_x_;
161179
uint16_t gnd_grid_buffer_size_;
162180
float grid_mode_switch_grid_id_;
163181
float grid_mode_switch_angle_rad_;
164182
float virtual_lidar_z_;
165183
float detection_range_z_max_;
166-
float center_pcl_shift_; // virtual center of pcl to center mass
167-
float grid_mode_switch_radius_; // non linear grid size switching distance
168-
double global_slope_max_angle_rad_; // radians
169-
double local_slope_max_angle_rad_; // radians
184+
float center_pcl_shift_; // virtual center of pcl to center mass
185+
float grid_mode_switch_radius_; // non linear grid size switching distance
186+
double global_slope_max_angle_rad_; // radians
187+
double local_slope_max_angle_rad_; // radians
188+
double global_slope_max_ratio_;
189+
double local_slope_max_ratio_;
170190
double radial_divider_angle_rad_; // distance in rads between dividers
171191
double split_points_distance_tolerance_; // distance in meters between concentric divisions
172-
double // minimum height threshold regardless the slope,
173-
split_height_distance_; // useful for close points
192+
double split_points_distance_tolerance_square_;
193+
double // minimum height threshold regardless the slope,
194+
split_height_distance_; // useful for close points
174195
bool use_virtual_ground_point_;
175196
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
176197
size_t radial_dividers_num_;
@@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
186207
*/
187208

188209
/*!
189-
* Convert pcl::PointCloud to sorted PointCloudRefVector
210+
* Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector
190211
* @param[in] in_cloud Input Point Cloud to be organized in radial segments
191212
* @param[out] out_radial_ordered_points_manager Vector of Points Clouds,
192213
* each element will contain the points ordered
193214
*/
194215
void convertPointcloud(
195-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
196-
std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
216+
const PointCloud2ConstPtr & in_cloud,
217+
std::vector<PointCloudVector> & out_radial_ordered_points_manager);
197218
void convertPointcloudGridScan(
198-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
199-
std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
219+
const PointCloud2ConstPtr & in_cloud,
220+
std::vector<PointCloudVector> & out_radial_ordered_points_manager);
200221
/*!
201222
* Output ground center of front wheels as the virtual ground point
202223
* @param[out] point Virtual ground origin point
203224
*/
204225
void calcVirtualGroundOrigin(pcl::PointXYZ & point);
205226

227+
float calcGridSize(const PointData & p);
228+
206229
/*!
207230
* Classifies Points in the PointCloud as Ground and Not Ground
208231
* @param in_radial_ordered_clouds Vector of an Ordered PointsCloud
@@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
214237
void initializeFirstGndGrids(
215238
const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);
216239

217-
void checkContinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
218-
void checkDiscontinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
219-
void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
240+
void checkContinuousGndGrid(
241+
PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
242+
void checkDiscontinuousGndGrid(
243+
PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
244+
void checkBreakGndGrid(
245+
PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
220246
void classifyPointCloud(
221-
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
247+
const PointCloud2ConstPtr & in_cloud_ptr,
248+
std::vector<PointCloudVector> & in_radial_ordered_clouds,
222249
pcl::PointIndices & out_no_ground_indices);
223250
void classifyPointCloudGridScan(
224-
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
251+
const PointCloud2ConstPtr & in_cloud_ptr,
252+
std::vector<PointCloudVector> & in_radial_ordered_clouds,
225253
pcl::PointIndices & out_no_ground_indices);
226254
/*!
227255
* Re-classifies point of ground cluster based on their height
@@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
237265
* and the other removed as indicated in the indices
238266
* @param in_cloud_ptr Input PointCloud to which the extraction will be performed
239267
* @param in_indices Indices of the points to be both removed and kept
240-
* @param out_object_cloud_ptr Resulting PointCloud with the indices kept
268+
* @param out_object_cloud Resulting PointCloud with the indices kept
241269
*/
242270
void extractObjectPoints(
243-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
244-
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
271+
const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices,
272+
PointCloud2 & out_object_cloud);
245273

246274
/** \brief Parameter service callback result : needed to be hold */
247275
OnSetParametersCallbackHandle::SharedPtr set_param_res_;

0 commit comments

Comments
 (0)