@@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
40
40
min_points_number_per_voxel_(min_points_number_per_voxel)
41
41
{
42
42
}
43
-
43
+ // TODO(badai-nguyen): remove this function when field copying also implemented for
44
+ // euclidean_cluster.cpp
44
45
bool VoxelGridBasedEuclideanCluster::cluster (
45
46
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
46
47
std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters)
48
+ {
49
+ (void )pointcloud;
50
+ (void )clusters;
51
+ return false ;
52
+ }
53
+
54
+ bool VoxelGridBasedEuclideanCluster::cluster (
55
+ const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
56
+ tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects)
47
57
{
48
58
// TODO(Saito) implement use_height is false version
49
59
50
60
// create voxel
61
+ pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud (new pcl::PointCloud<pcl::PointXYZ>);
62
+ int point_step = pointcloud_msg->point_step ;
63
+ pcl::fromROSMsg (*pointcloud_msg, *pointcloud);
51
64
pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr (new pcl::PointCloud<pcl::PointXYZ>);
52
65
voxel_grid_.setLeafSize (voxel_leaf_size_, voxel_leaf_size_, 100000.0 );
53
66
voxel_grid_.setMinimumPointsNumberPerVoxel (min_points_number_per_voxel_);
@@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster(
81
94
82
95
// create map to search cluster index from voxel grid index
83
96
std::unordered_map</* voxel grid index */ int , /* cluster index */ int > map;
97
+ std::vector<sensor_msgs::msg::PointCloud2> temporary_clusters; // no check about cluster size
98
+ std::vector<size_t > clusters_data_size;
99
+ temporary_clusters.resize (cluster_indices.size ());
84
100
for (size_t cluster_idx = 0 ; cluster_idx < cluster_indices.size (); ++cluster_idx) {
85
101
const auto & cluster = cluster_indices.at (cluster_idx);
102
+ auto & temporary_cluster = temporary_clusters.at (cluster_idx);
86
103
for (const auto & point_idx : cluster.indices ) {
87
104
map[point_idx] = cluster_idx;
88
105
}
106
+ temporary_cluster.height = pointcloud_msg->height ;
107
+ temporary_cluster.fields = pointcloud_msg->fields ;
108
+ temporary_cluster.point_step = point_step;
109
+ temporary_cluster.data .resize (max_cluster_size_ * point_step);
110
+ clusters_data_size.push_back (0 );
89
111
}
90
112
91
113
// create vector of point cloud cluster. vector index is voxel grid index.
92
- std::vector<pcl::PointCloud<pcl::PointXYZ>> temporary_clusters; // no check about cluster size
93
- temporary_clusters.resize (cluster_indices.size ());
94
- for (const auto & point : pointcloud->points ) {
114
+ for (size_t i = 0 ; i < pointcloud->points .size (); ++i) {
115
+ const auto & point = pointcloud->points .at (i);
95
116
const int index =
96
117
voxel_grid_.getCentroidIndexAt (voxel_grid_.getGridCoordinates (point.x , point.y , point.z ));
97
118
if (map.find (index ) != map.end ()) {
98
- temporary_clusters.at (map[index ]).points .push_back (point);
119
+ auto & cluster_data_size = clusters_data_size.at (map[index ]);
120
+ if (cluster_data_size + point_step > std::size_t (max_cluster_size_ * point_step)) {
121
+ continue ;
122
+ }
123
+ std::memcpy (
124
+ &temporary_clusters.at (map[index ]).data [cluster_data_size],
125
+ &pointcloud_msg->data [i * point_step], point_step);
126
+ cluster_data_size += point_step;
99
127
}
100
128
}
101
129
102
130
// build output and check cluster size
103
131
{
104
- for (const auto & cluster : temporary_clusters) {
105
- if (!(min_cluster_size_ <= static_cast <int >(cluster.points .size ()) &&
106
- static_cast <int >(cluster.points .size ()) <= max_cluster_size_)) {
132
+ for (size_t i = 0 ; i < temporary_clusters.size (); ++i) {
133
+ auto & i_cluster_data_size = clusters_data_size.at (i);
134
+ if (!(min_cluster_size_ <= static_cast <int >(i_cluster_data_size / point_step) &&
135
+ static_cast <int >(i_cluster_data_size / point_step) <= max_cluster_size_)) {
107
136
continue ;
108
137
}
109
- clusters.push_back (cluster);
110
- clusters.back ().width = cluster.points .size ();
111
- clusters.back ().height = 1 ;
112
- clusters.back ().is_dense = false ;
138
+ const auto & cluster = temporary_clusters.at (i);
139
+ tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
140
+ feature_object.feature .cluster = cluster;
141
+ feature_object.feature .cluster .data .resize (i_cluster_data_size);
142
+ feature_object.feature .cluster .header = pointcloud_msg->header ;
143
+ feature_object.feature .cluster .is_bigendian = pointcloud_msg->is_bigendian ;
144
+ feature_object.feature .cluster .is_dense = pointcloud_msg->is_dense ;
145
+ feature_object.feature .cluster .point_step = point_step;
146
+ feature_object.feature .cluster .row_step = i_cluster_data_size / pointcloud_msg->height ;
147
+ feature_object.feature .cluster .width =
148
+ i_cluster_data_size / point_step / pointcloud_msg->height ;
149
+
150
+ feature_object.object .kinematics .pose_with_covariance .pose .position =
151
+ getCentroid (feature_object.feature .cluster );
152
+ autoware_auto_perception_msgs::msg::ObjectClassification classification;
153
+ classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
154
+ classification.probability = 1 .0f ;
155
+ feature_object.object .classification .emplace_back (classification);
156
+
157
+ objects.feature_objects .push_back (feature_object);
113
158
}
159
+ objects.header = pointcloud_msg->header ;
114
160
}
115
161
116
162
return true ;
0 commit comments