|
| 1 | +// Copyright 2020 TierIV |
| 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 | +/* |
| 16 | + * Copyright 2018-2019 Autoware Foundation. All rights reserved. |
| 17 | + * |
| 18 | + * Licensed under the Apache License, Version 2.0 (the "License"); |
| 19 | + * you may not use this file except in compliance with the License. |
| 20 | + * You may obtain a copy of the License at |
| 21 | + * |
| 22 | + * http://www.apache.org/licenses/LICENSE-2.0 |
| 23 | + * |
| 24 | + * Unless required by applicable law or agreed to in writing, software |
| 25 | + * distributed under the License is distributed on an "AS IS" BASIS, |
| 26 | + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 27 | + * See the License for the specific language governing permissions and |
| 28 | + * limitations under the License. |
| 29 | + */ |
| 30 | + |
| 31 | +/****************************************************************************** |
| 32 | + * Copyright 2017 The Apollo Authors. All Rights Reserved. |
| 33 | + * |
| 34 | + * Licensed under the Apache License, Version 2.0 (the "License"); |
| 35 | + * you may not use this file except in compliance with the License. |
| 36 | + * You may obtain a copy of the License at |
| 37 | + * |
| 38 | + * http://www.apache.org/licenses/LICENSE-2.0 |
| 39 | + * |
| 40 | + * Unless required by applicable law or agreed to in writing, software |
| 41 | + * distributed under the License is distributed on an "AS IS" BASIS, |
| 42 | + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 43 | + * See the License for the specific language governing permissions and |
| 44 | + * limitations under the License. |
| 45 | + *****************************************************************************/ |
| 46 | + |
| 47 | +#include "lidar_apollo_instance_segmentation/cluster2d.hpp" |
| 48 | + |
| 49 | +#include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp> |
| 50 | +#include <autoware_auto_perception_msgs/msg/object_classification.hpp> |
| 51 | + |
| 52 | +#include <pcl_conversions/pcl_conversions.h> |
| 53 | +#include <tf2_geometry_msgs/tf2_geometry_msgs.h> |
| 54 | + |
| 55 | +geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) |
| 56 | +{ |
| 57 | + tf2::Quaternion q; |
| 58 | + q.setRPY(r, p, y); |
| 59 | + return tf2::toMsg(q); |
| 60 | +} |
| 61 | + |
| 62 | +Cluster2D::Cluster2D(const int rows, const int cols, const float range) |
| 63 | +{ |
| 64 | + rows_ = rows; |
| 65 | + cols_ = cols; |
| 66 | + siz_ = rows * cols; |
| 67 | + range_ = range; |
| 68 | + scale_ = 0.5 * static_cast<float>(rows_) / range_; |
| 69 | + inv_res_x_ = 0.5 * static_cast<float>(cols_) / range_; |
| 70 | + inv_res_y_ = 0.5 * static_cast<float>(rows_) / range_; |
| 71 | + point2grid_.clear(); |
| 72 | + id_img_.assign(siz_, -1); |
| 73 | + pc_ptr_.reset(); |
| 74 | + valid_indices_in_pc_ = nullptr; |
| 75 | +} |
| 76 | + |
| 77 | +void Cluster2D::traverse(Node * x) |
| 78 | +{ |
| 79 | + std::vector<Node *> p; |
| 80 | + p.clear(); |
| 81 | + |
| 82 | + while (x->traversed == 0) { |
| 83 | + p.push_back(x); |
| 84 | + x->traversed = 2; |
| 85 | + x = x->center_node; |
| 86 | + } |
| 87 | + if (x->traversed == 2) { |
| 88 | + for (int i = static_cast<int>(p.size()) - 1; i >= 0 && p[i] != x; i--) { |
| 89 | + p[i]->is_center = true; |
| 90 | + } |
| 91 | + x->is_center = true; |
| 92 | + } |
| 93 | + for (size_t i = 0; i < p.size(); i++) { |
| 94 | + Node * y = p[i]; |
| 95 | + y->traversed = 1; |
| 96 | + y->parent = x->parent; |
| 97 | + } |
| 98 | +} |
| 99 | + |
| 100 | +void Cluster2D::cluster( |
| 101 | + const std::shared_ptr< float> & inferred_data, const pcl::PointCloud<pcl::PointXYZI>:: Ptr & pc_ptr, |
| 102 | + const pcl::PointIndices & valid_indices, float objectness_thresh, |
| 103 | + bool use_all_grids_for_clustering) |
| 104 | +{ |
| 105 | + const float * category_pt_data = inferred_data.get(); |
| 106 | + const float * instance_pt_x_data = inferred_data.get() + siz_; |
| 107 | + const float * instance_pt_y_data = inferred_data.get() + siz_ * 2; |
| 108 | + |
| 109 | + pc_ptr_ = pc_ptr; |
| 110 | + |
| 111 | + std::vector<std::vector<Node>> nodes(rows_, std::vector<Node>(cols_, Node())); |
| 112 | + |
| 113 | + valid_indices_in_pc_ = &(valid_indices.indices); |
| 114 | + point2grid_.assign(valid_indices_in_pc_->size(), -1); |
| 115 | + |
| 116 | + for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) { |
| 117 | + int point_id = valid_indices_in_pc_->at(i); |
| 118 | + const auto & point = pc_ptr_->points[point_id]; |
| 119 | + // * the coordinates of x and y have been exchanged in feature generation |
| 120 | + // step, |
| 121 | + // so we swap them back here. |
| 122 | + int pos_x = F2I(point.y, range_, inv_res_x_); // col |
| 123 | + int pos_y = F2I(point.x, range_, inv_res_y_); // row |
| 124 | + if (IsValidRowCol(pos_y, pos_x)) { |
| 125 | + point2grid_[i] = RowCol2Grid(pos_y, pos_x); |
| 126 | + nodes[pos_y][pos_x].point_num++; |
| 127 | + } |
| 128 | + } |
| 129 | + |
| 130 | + for (int row = 0; row < rows_; ++row) { |
| 131 | + for (int col = 0; col < cols_; ++col) { |
| 132 | + int grid = RowCol2Grid(row, col); |
| 133 | + Node * node = &nodes[row][col]; |
| 134 | + DisjointSetMakeSet(node); |
| 135 | + node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) && |
| 136 | + (*(category_pt_data + grid) >= objectness_thresh); |
| 137 | + int center_row = std::round(row + instance_pt_x_data[grid] * scale_); |
| 138 | + int center_col = std::round(col + instance_pt_y_data[grid] * scale_); |
| 139 | + center_row = std::min(std::max(center_row, 0), rows_ - 1); |
| 140 | + center_col = std::min(std::max(center_col, 0), cols_ - 1); |
| 141 | + node->center_node = &nodes[center_row][center_col]; |
| 142 | + } |
| 143 | + } |
| 144 | + |
| 145 | + for (int row = 0; row < rows_; ++row) { |
| 146 | + for (int col = 0; col < cols_; ++col) { |
| 147 | + Node * node = &nodes[row][col]; |
| 148 | + if (node->is_object && node->traversed == 0) { |
| 149 | + traverse(node); |
| 150 | + } |
| 151 | + } |
| 152 | + } |
| 153 | + |
| 154 | + for (int row = 0; row < rows_; ++row) { |
| 155 | + for (int col = 0; col < cols_; ++col) { |
| 156 | + Node * node = &nodes[row][col]; |
| 157 | + if (!node->is_center) { |
| 158 | + continue; |
| 159 | + } |
| 160 | + for (int row2 = row - 1; row2 <= row + 1; ++row2) { |
| 161 | + for (int col2 = col - 1; col2 <= col + 1; ++col2) { |
| 162 | + if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) { |
| 163 | + Node * node2 = &nodes[row2][col2]; |
| 164 | + if (node2->is_center) { |
| 165 | + DisjointSetUnion(node, node2); |
| 166 | + } |
| 167 | + } |
| 168 | + } |
| 169 | + } |
| 170 | + } |
| 171 | + } |
| 172 | + |
| 173 | + int count_obstacles = 0; |
| 174 | + obstacles_.clear(); |
| 175 | + id_img_.assign(siz_, -1); |
| 176 | + for (int row = 0; row < rows_; ++row) { |
| 177 | + for (int col = 0; col < cols_; ++col) { |
| 178 | + Node * node = &nodes[row][col]; |
| 179 | + if (!node->is_object) { |
| 180 | + continue; |
| 181 | + } |
| 182 | + Node * root = DisjointSetFind(node); |
| 183 | + if (root->obstacle_id < 0) { |
| 184 | + root->obstacle_id = count_obstacles++; |
| 185 | + obstacles_.push_back(Obstacle()); |
| 186 | + } |
| 187 | + int grid = RowCol2Grid(row, col); |
| 188 | + id_img_[grid] = root->obstacle_id; |
| 189 | + obstacles_[root->obstacle_id].grids.push_back(grid); |
| 190 | + } |
| 191 | + } |
| 192 | + filter(inferred_data); |
| 193 | + classify(inferred_data); |
| 194 | +} |
| 195 | + |
| 196 | +void Cluster2D::filter(const std::shared_ptr<float> & inferred_data) |
| 197 | +{ |
| 198 | + const float * confidence_pt_data = inferred_data.get() + siz_ * 3; |
| 199 | + const float * heading_pt_x_data = inferred_data.get() + siz_ * 9; |
| 200 | + const float * heading_pt_y_data = inferred_data.get() + siz_ * 10; |
| 201 | + const float * height_pt_data = inferred_data.get() + siz_ * 11; |
| 202 | + |
| 203 | + for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { |
| 204 | + Obstacle * obs = &obstacles_[obstacle_id]; |
| 205 | + double score = 0.0; |
| 206 | + double height = 0.0; |
| 207 | + double vec_x = 0.0; |
| 208 | + double vec_y = 0.0; |
| 209 | + for (int grid : obs->grids) { |
| 210 | + score += static_cast<double>(confidence_pt_data[grid]); |
| 211 | + height += static_cast<double>(height_pt_data[grid]); |
| 212 | + vec_x += heading_pt_x_data[grid]; |
| 213 | + vec_y += heading_pt_y_data[grid]; |
| 214 | + } |
| 215 | + obs->score = score / static_cast<double>(obs->grids.size()); |
| 216 | + obs->height = height / static_cast<double>(obs->grids.size()); |
| 217 | + obs->heading = std::atan2(vec_y, vec_x) * 0.5; |
| 218 | + obs-> cloud_ptr. reset( new pcl::PointCloud<pcl::PointXYZI>); |
| 219 | + } |
| 220 | +} |
| 221 | + |
| 222 | +void Cluster2D::classify(const std::shared_ptr<float> & inferred_data) |
| 223 | +{ |
| 224 | + const float * classify_pt_data = inferred_data.get() + siz_ * 4; |
| 225 | + int num_classes = 5; |
| 226 | + for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { |
| 227 | + Obstacle * obs = &obstacles_[obs_id]; |
| 228 | + |
| 229 | + for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { |
| 230 | + int grid = obs->grids[grid_id]; |
| 231 | + for (int k = 0; k < num_classes; k++) { |
| 232 | + obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid]; |
| 233 | + } |
| 234 | + } |
| 235 | + int meta_type_id = 0; |
| 236 | + for (int k = 0; k < num_classes; k++) { |
| 237 | + obs->meta_type_probs[k] /= obs->grids.size(); |
| 238 | + if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { |
| 239 | + meta_type_id = k; |
| 240 | + } |
| 241 | + } |
| 242 | + obs->meta_type = static_cast<MetaType>(meta_type_id); |
| 243 | + } |
| 244 | +} |
| 245 | + |
| 246 | +autoware_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObject( |
| 247 | + const Obstacle & in_obstacle, const std_msgs::msg::Header & in_header) |
| 248 | +{ |
| 249 | + using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; |
| 250 | + using autoware_auto_perception_msgs::msg::ObjectClassification; |
| 251 | + |
| 252 | + autoware_perception_msgs::msg::DetectedObjectWithFeature resulting_object; |
| 253 | + // pcl::PointCloud<pcl::PointXYZI> in_cluster = *(in_obstacle.cloud_ptr); |
| 254 | + |
| 255 | + resulting_object.object.classification.emplace_back( |
| 256 | + autoware_auto_perception_msgs::build<ObjectClassification>() |
| 257 | + .label(ObjectClassification::UNKNOWN) |
| 258 | + .probability(in_obstacle.score)); |
| 259 | + if (in_obstacle.meta_type == MetaType::META_PEDESTRIAN) { |
| 260 | + resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; |
| 261 | + } else if (in_obstacle.meta_type == MetaType::META_NONMOT) { |
| 262 | + resulting_object.object.classification.front().label = ObjectClassification::MOTORCYCLE; |
| 263 | + } else if (in_obstacle.meta_type == MetaType::META_SMALLMOT) { |
| 264 | + resulting_object.object.classification.front().label = ObjectClassification::CAR; |
| 265 | + } else if (in_obstacle.meta_type == MetaType::META_BIGMOT) { |
| 266 | + resulting_object.object.classification.front().label = ObjectClassification::BUS; |
| 267 | + } else { |
| 268 | + // resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; |
| 269 | + resulting_object.object.classification.front().label = ObjectClassification::UNKNOWN; |
| 270 | + } |
| 271 | + |
| 272 | + pcl::PointXYZ min_point; |
| 273 | + pcl::PointXYZ max_point; |
| 274 | + for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); |
| 275 | + ++pit) { |
| 276 | + if (pit->x < min_point.x) { |
| 277 | + min_point.x = pit->x; |
| 278 | + } |
| 279 | + if (pit->y < min_point.y) { |
| 280 | + min_point.y = pit->y; |
| 281 | + } |
| 282 | + if (pit->z < min_point.z) { |
| 283 | + min_point.z = pit->z; |
| 284 | + } |
| 285 | + if (pit->x > max_point.x) { |
| 286 | + max_point.x = pit->x; |
| 287 | + } |
| 288 | + if (pit->y > max_point.y) { |
| 289 | + max_point.y = pit->y; |
| 290 | + } |
| 291 | + if (pit->z > max_point.z) { |
| 292 | + max_point.z = pit->z; |
| 293 | + } |
| 294 | + } |
| 295 | + |
| 296 | + // cluster and ground filtering |
| 297 | + pcl::PointCloud<pcl::PointXYZI> cluster; |
| 298 | + const float min_height = min_point.z + ((max_point.z - min_point.z) * 0.1f); |
| 299 | + for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); |
| 300 | + ++pit) { |
| 301 | + if (min_height < pit->z) { |
| 302 | + cluster.points.push_back(*pit); |
| 303 | + } |
| 304 | + } |
| 305 | + min_point.z = 0.0; |
| 306 | + max_point.z = 0.0; |
| 307 | + for (auto pit = cluster.points.begin(); pit != cluster.points.end(); ++pit) { |
| 308 | + if (pit->z < min_point.z) { |
| 309 | + min_point.z = pit->z; |
| 310 | + } |
| 311 | + if (pit->z > max_point.z) { |
| 312 | + max_point.z = pit->z; |
| 313 | + } |
| 314 | + } |
| 315 | + sensor_msgs::msg::PointCloud2 ros_pc; |
| 316 | + pcl::toROSMsg(cluster, ros_pc); |
| 317 | + resulting_object.feature.cluster = ros_pc; |
| 318 | + resulting_object.feature.cluster.header = in_header; |
| 319 | + |
| 320 | + // position |
| 321 | + const float height = max_point.z - min_point.z; |
| 322 | + const float length = max_point.x - min_point.x; |
| 323 | + const float width = max_point.y - min_point.y; |
| 324 | + resulting_object.object.kinematics.pose_with_covariance.pose.position.x = |
| 325 | + min_point.x + length / 2; |
| 326 | + resulting_object.object.kinematics.pose_with_covariance.pose.position.y = min_point.y + width / 2; |
| 327 | + resulting_object.object.kinematics.pose_with_covariance.pose.position.z = |
| 328 | + min_point.z + height / 2; |
| 329 | + |
| 330 | + resulting_object.object.kinematics.pose_with_covariance.pose.orientation = |
| 331 | + getQuaternionFromRPY(0.0, 0.0, in_obstacle.heading); |
| 332 | + resulting_object.object.kinematics.orientation_availability = |
| 333 | + DetectedObjectKinematics::SIGN_UNKNOWN; |
| 334 | + |
| 335 | + return resulting_object; |
| 336 | +} |
| 337 | + |
| 338 | +void Cluster2D::getObjects( |
| 339 | + const float confidence_thresh, const float height_thresh, const int min_pts_num, |
| 340 | + autoware_perception_msgs::msg::DetectedObjectsWithFeature & objects, |
| 341 | + const std_msgs::msg::Header & in_header) |
| 342 | +{ |
| 343 | + for (size_t i = 0; i < point2grid_.size(); ++i) { |
| 344 | + int grid = point2grid_[i]; |
| 345 | + if (grid < 0) { |
| 346 | + continue; |
| 347 | + } |
| 348 | + |
| 349 | + int obstacle_id = id_img_[grid]; |
| 350 | + |
| 351 | + int point_id = valid_indices_in_pc_->at(i); |
| 352 | + |
| 353 | + if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) { |
| 354 | + if ( |
| 355 | + height_thresh < 0 || |
| 356 | + pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) { |
| 357 | + obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]); |
| 358 | + } |
| 359 | + } |
| 360 | + } |
| 361 | + |
| 362 | + for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { |
| 363 | + Obstacle * obs = &obstacles_[obstacle_id]; |
| 364 | + if (static_cast<int>(obs->cloud_ptr->size()) < min_pts_num) { |
| 365 | + continue; |
| 366 | + } |
| 367 | + autoware_perception_msgs::msg::DetectedObjectWithFeature out_obj = |
| 368 | + obstacleToObject(*obs, in_header); |
| 369 | + objects.feature_objects.push_back(out_obj); |
| 370 | + } |
| 371 | + objects.header = in_header; |
| 372 | +} |
0 commit comments