From 75242c1ebf78dfe7262b5d895dab4088ada555b5 Mon Sep 17 00:00:00 2001 From: Andres Date: Thu, 3 Oct 2024 19:31:25 -0400 Subject: [PATCH 01/11] Re-integrated adative clustering for av24 --- package.xml | 2 + param/adaptive_clustering.yaml | 4 +- src/adaptive_clustering.cpp | 277 ++++++++++++++------------------- 3 files changed, 118 insertions(+), 165 deletions(-) diff --git a/package.xml b/package.xml index 6577fee..c89f9a4 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,8 @@ pcl_conversions pcl_ros blackandgold_msgs + tf2 + tf2_geometry_msgs ament_cmake_gtest ament_lint_auto diff --git a/param/adaptive_clustering.yaml b/param/adaptive_clustering.yaml index a6cc5cc..cd4c80e 100644 --- a/param/adaptive_clustering.yaml +++ b/param/adaptive_clustering.yaml @@ -1,6 +1,6 @@ /adaptive_clustering: ros__parameters: - cluster_size_max: 5000 + cluster_size_max: 500 cluster_size_min: 5 generate_bounding_boxes: true k_merging_threshold: 0.10000000149011612 @@ -9,7 +9,7 @@ radius_max: 120.0 radius_min: 0.4000000059604645 sensor_model: VLP-16 - z_axis_max: 10.0 + z_axis_max: 3.0 z_axis_min: -0.800000011920929 z_merging_threshold: 0.0 car_width: 2.0 diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index b0037b8..8b11190 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -26,6 +26,7 @@ #include #include #include +#include // PCL #include "pcl/pcl_config.h" @@ -92,11 +93,10 @@ class AdaptiveClustering : public rclcpp::Node { car_length_ = this->get_parameter("car_length").get_parameter_value().get(); /*** Subscribers ***/ - point_cloud_sub = this->create_subscription("/perception/points_nonground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, + point_cloud_sub = this->create_subscription("/perception/linefit_seg/ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, this, std::placeholders::_1)); //wall_points_sub = this->create_subscription("/perception/wall_point_markers", 10, std::bind(&AdaptiveClustering::wallsCallback, // this, std::placeholders::_1)); - //ros::Subscriber point_cloud_sub = nh.subscribe("velodyne_points", 1, pointCloudCallback); /*** Publishers ***/ @@ -222,7 +222,7 @@ class AdaptiveClustering : public rclcpp::Node { //RCLCPP_INFO(this->get_logger(), "Current time: %ld.%09ld", current_time.seconds(), current_time.nanoseconds()); for(int i = 0; i < region_max_; i++) { - tolerance += 3*0.1; + tolerance += 0.0; //3*0.1; if(indices_array[i].size() > cluster_size_min_) { #if PCL_VERSION_COMPARE(<, 1, 11, 0) boost::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); @@ -231,16 +231,21 @@ class AdaptiveClustering : public rclcpp::Node { #endif pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); tree->setInputCloud(pcl_pc_in, indices_array_ptr); - - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(tolerance); - ec.setMinClusterSize(cluster_size_min_); - ec.setMaxClusterSize(cluster_size_max_); - ec.setSearchMethod(tree); - ec.setInputCloud(pcl_pc_in); - ec.setIndices(indices_array_ptr); - ec.extract(cluster_indices); + auto clustering_start = std::chrono::high_resolution_clock::now(); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(tolerance); + ec.setMinClusterSize(cluster_size_min_); + ec.setMaxClusterSize(cluster_size_max_); + ec.setSearchMethod(tree); + ec.setInputCloud(pcl_pc_in); + ec.setIndices(indices_array_ptr); + ec.extract(cluster_indices); + + auto clustering_end = std::chrono::high_resolution_clock::now(); + auto clustering_duration = std::chrono::duration_cast(clustering_end - clustering_start).count(); + RCLCPP_DEBUG(this->get_logger(), "Clustering took %ld ms", clustering_duration); for(std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++) { pcl::PointCloud::Ptr cluster(new pcl::PointCloud); @@ -333,159 +338,105 @@ class AdaptiveClustering : public rclcpp::Node { if (fabs(centroid[0]) <= car_length_/2 && fabs(centroid[1]) <= car_width_/2) { continue; } + + Eigen::Vector4f min, max; + pcl::getMinMax3D(*clusters[i], min, max); + + autoware_auto_perception_msgs::msg::BoundingBox box; + box.centroid.x = centroid[0]; + box.centroid.y = centroid[1]; + box.centroid.z = centroid[2]; + + // Compute sized of bounding box + box.size.x = max[0] - min[0]; + box.size.y = max[1] - min[1]; + box.size.z = max[2] - min[2]; - geometry_msgs::msg::Pose pose; - pose.position.x = centroid[0]; - pose.position.y = centroid[1]; - pose.position.z = centroid[2]; - pose.orientation.w = 1; - pose_array.poses.push_back(pose); + // Compute roll angle of bounding box + // float roll = atan2(max[1] - min[1], max[0] - min[0]); - #ifdef LOG - Eigen::Vector4f min, max; - pcl::getMinMax3D(*clusters[i], min, max); - std::cerr << ros_pc2_in->header.seq << " " - << ros_pc2_in->header.stamp << " " - << min[0] << " " - << min[1] << " " - << min[2] << " " - << max[0] << " " - << max[1] << " " - << max[2] << " " - << std::endl; - #endif - //} - - if (!generate_bounding_boxes) { - //if(marker_array_pub_->get_subscription_count() > 0) { - Eigen::Vector4f min, max; - pcl::getMinMax3D(*clusters[i], min, max); - - visualization_msgs::msg::Marker marker; - marker.header = ros_pc2_in->header; - marker.ns = "adaptive_clustering"; - marker.id = i; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - - geometry_msgs::msg::Point p[24]; - p[0].x = max[0]; p[0].y = max[1]; p[0].z = max[2]; - p[1].x = min[0]; p[1].y = max[1]; p[1].z = max[2]; - p[2].x = max[0]; p[2].y = max[1]; p[2].z = max[2]; - p[3].x = max[0]; p[3].y = min[1]; p[3].z = max[2]; - p[4].x = max[0]; p[4].y = max[1]; p[4].z = max[2]; - p[5].x = max[0]; p[5].y = max[1]; p[5].z = min[2]; - p[6].x = min[0]; p[6].y = min[1]; p[6].z = min[2]; - p[7].x = max[0]; p[7].y = min[1]; p[7].z = min[2]; - p[8].x = min[0]; p[8].y = min[1]; p[8].z = min[2]; - p[9].x = min[0]; p[9].y = max[1]; p[9].z = min[2]; - p[10].x = min[0]; p[10].y = min[1]; p[10].z = min[2]; - p[11].x = min[0]; p[11].y = min[1]; p[11].z = max[2]; - p[12].x = min[0]; p[12].y = max[1]; p[12].z = max[2]; - p[13].x = min[0]; p[13].y = max[1]; p[13].z = min[2]; - p[14].x = min[0]; p[14].y = max[1]; p[14].z = max[2]; - p[15].x = min[0]; p[15].y = min[1]; p[15].z = max[2]; - p[16].x = max[0]; p[16].y = min[1]; p[16].z = max[2]; - p[17].x = max[0]; p[17].y = min[1]; p[17].z = min[2]; - p[18].x = max[0]; p[18].y = min[1]; p[18].z = max[2]; - p[19].x = min[0]; p[19].y = min[1]; p[19].z = max[2]; - p[20].x = max[0]; p[20].y = max[1]; p[20].z = min[2]; - p[21].x = min[0]; p[21].y = max[1]; p[21].z = min[2]; - p[22].x = max[0]; p[22].y = max[1]; p[22].z = min[2]; - p[23].x = max[0]; p[23].y = min[1]; p[23].z = min[2]; - - for(int i = 0; i < 24; i++) { - marker.points.push_back(p[i]); - } - - marker.scale.x = 0.3; - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.5; + // Create quaternion from roll angle + // tf2::Quaternion quaternion; + // quaternion.setRPY(roll, 0, 0); + // geometry_msgs::msg::Quaternion quat_msg; + // quat_msg.x = quaternion.x(); + // quat_msg.y = quaternion.y(); + // quat_msg.z = quaternion.z(); + // quat_msg.w = quaternion.w(); + + // Print quaternion components: + // RCLCPP_INFO(this->get_logger(), "Quat x: '%f'", quat_msg.x); + + // box.orientation.x = quat_msg.x; + // box.orientation.y = quat_msg.y; + // box.orientation.z = quat_msg.z; + // box.orientation.w = quat_msg.w; + + // geometry_msgs::msg::Pose pose; + // pose.position.x = centroid[0]; + // pose.position.y = centroid[1]; + // pose.position.z = centroid[2]; + // pose.orientation = quat_msg; + // pose_array.poses.push_back(pose); + + // RCLCPP_INFO(this->get_logger(), "Roll: '%f'", roll); + + bounding_boxes.boxes.push_back(box); + + // deal with markers + visualization_msgs::msg::Marker m; + m.header = ros_pc2_in->header; + m.ns = "bbox"; + m.id = i; + m.type = visualization_msgs::msg::Marker::CUBE; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose.position.x = box.centroid.x; + m.pose.position.y = box.centroid.y; + m.pose.position.z = box.centroid.z; + m.pose.orientation.x = box.orientation.x; + m.pose.orientation.y = box.orientation.y; + m.pose.orientation.z = box.orientation.z; + m.pose.orientation.w = box.orientation.w; + + m.scale.x = box.size.x; + m.scale.y = box.size.y; + m.scale.z = box.size.z; + + bool valid = isOutOfBounds_v2(polynomials, box); + + // figure out geometrically if it is a wall + + //if (!valid) + // NOTE May not need this with the addition of the off-map filter (CarProximityReporter) + if ((box.size.x * box.size.y * box.size.z >= 30.0) || box.size.x > 7.0 || (box.size.y / box.size.x > 3.0) || !valid) + { // If this is true, the box is a wall + // marker color + m.color.r = 0.0; + m.color.g = 0.0; + m.color.b = 1.0; + m.color.a = 0.75; + m.lifetime.sec = 0; + m.lifetime.nanosec = 100000000; + wall_bounding_boxes.boxes.push_back(box); - #if PCL_VERSION_COMPARE(<, 1, 11, 0) - marker.lifetime = rclcpp::Duration(0.1); - #else - marker.lifetime = rclcpp::Duration(0.1s); - #endif - marker_array.markers.push_back(marker); - //} - } - - else { - autoware_auto_perception_msgs::msg::BoundingBox box; - box.centroid.x = centroid[0]; - box.centroid.y = centroid[1]; - box.centroid.z = centroid[2]; - - // FIXME: Add a method to compute the orientation of the bounding box - box.orientation.w = 1.0; - - Eigen::Vector4f min, max; - pcl::getMinMax3D(*clusters[i], min, max); - - box.size.x = max[0] - min[0]; - box.size.y = max[1] - min[1]; - box.size.z = max[2] - min[2]; - - bounding_boxes.boxes.push_back(box); - - // deal with markers - visualization_msgs::msg::Marker m; - m.header = ros_pc2_in->header; - m.ns = "bbox"; - m.id = i; - m.type = visualization_msgs::msg::Marker::CUBE; - m.action = visualization_msgs::msg::Marker::ADD; - m.pose.position.x = box.centroid.x; - m.pose.position.y = box.centroid.y; - m.pose.position.z = box.centroid.z; - m.pose.orientation.x = box.orientation.x; - m.pose.orientation.y = box.orientation.y; - m.pose.orientation.z = box.orientation.z; - m.pose.orientation.w = box.orientation.w; - - m.scale.x = box.size.x; - m.scale.y = box.size.y; - m.scale.z = box.size.z; - - bool valid = isOutOfBounds_v2(polynomials, box); - - // figure out geometrically if it is a wall - - //if (!valid) - // NOTE May not need this with the addition of the off-map filter (CarProximityReporter) - if ((box.size.x * box.size.y * box.size.z >= 30.0) || box.size.x > 7.0 || (box.size.y / box.size.x > 3.0) || !valid) - { // If this is true, the box is a wall - // marker color - m.color.r = 0.0; - m.color.g = 0.0; - m.color.b = 1.0; - m.color.a = 0.75; - m.lifetime.sec = 0; - m.lifetime.nanosec = 100000000; - wall_bounding_boxes.boxes.push_back(box); - - } - else// if (abs(box.centroid.y) < 20.0) - { // The box is a vehicle - // marker color - RCLCPP_DEBUG(this->get_logger(), "Poly size: '%i'", polynomials.polynomials.size()); - m.color.r = 1.0; - m.color.g = 0.0; - m.color.b = 0.0; - m.color.a = 0.75; - m.lifetime.sec = 0; - m.lifetime.nanosec = 100000000; - vehicle_bounding_boxes.boxes.push_back(box); - vehicle_markers.markers.push_back(m); - } + else// if (abs(box.centroid.y) < 20.0) + { // The box is a vehicle + // marker color + RCLCPP_DEBUG(this->get_logger(), "Poly size: '%i'", polynomials.polynomials.size()); + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 0.0; + m.color.a = 0.75; + m.lifetime.sec = 0; + m.lifetime.nanosec = 100000000; + vehicle_bounding_boxes.boxes.push_back(box); + vehicle_markers.markers.push_back(m); + } - bounding_boxes_markers.markers.push_back(m); + bounding_boxes_markers.markers.push_back(m); - } } if(bounding_boxes.boxes.size()) { @@ -508,10 +459,10 @@ class AdaptiveClustering : public rclcpp::Node { cluster_array_pub_->publish(cluster_array); } - if(pose_array.poses.size()) { - pose_array.header = ros_pc2_in->header; - pose_array_pub_->publish(pose_array); - } + // if(pose_array.poses.size()) { + // pose_array.header = ros_pc2_in->header; + // pose_array_pub_->publish(pose_array); + // } if(marker_array.markers.size()) { marker_array_pub_->publish(marker_array); From b3f03fb59723e1019a06439ef96d323fc397d17e Mon Sep 17 00:00:00 2001 From: Andres Date: Sun, 6 Oct 2024 19:59:28 -0400 Subject: [PATCH 02/11] More tuning on adaptive clustering --- param/better_clustering_config.yaml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 param/better_clustering_config.yaml diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml new file mode 100644 index 0000000..bdcbe9e --- /dev/null +++ b/param/better_clustering_config.yaml @@ -0,0 +1,16 @@ +/adaptive_clustering: + ros__parameters: + cluster_size_max: 500 + cluster_size_min: 15 + generate_bounding_boxes: true + k_merging_threshold: 0.10000000149011612 + leaf: 1 + print_fps: false + radius_max: 120.0 + radius_min: 0.4000000059604645 + sensor_model: VLP-16 + z_axis_max: 3.0 + z_axis_min: -0.800000011920929 + z_merging_threshold: 0.0 + car_width: 2.0 + car_length: 4.8768 From 54fe7707bb674c14f68d21c104f69acb4a545d1e Mon Sep 17 00:00:00 2001 From: Andres Date: Sat, 26 Oct 2024 18:43:42 -0400 Subject: [PATCH 03/11] Trying with namespaces (untested) --- src/adaptive_clustering.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index 8b11190..919bb17 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -109,10 +109,10 @@ class AdaptiveClustering : public rclcpp::Node { //marker_array_pub_ = private_nh.advertise("markers", 100); marker_array_pub_ = this->create_publisher("clustering_markers", 10); - bounding_boxes_pub_ = this->create_publisher("/perception/lidar_bboxes", 10); - vehicle_boxes_pub_ = this->create_publisher("/perception/lidar_vehicle_bboxes", 10); - wall_boxes_pub_ = this->create_publisher("/perception/lidar_wall_bboxes", 10); - vehicle_marker_array_pub_ = this->create_publisher("/perception/vehicle_lidar_markers", 10); + bounding_boxes_pub_ = this->create_publisher("lidar_bboxes", 10); + vehicle_boxes_pub_ = this->create_publisher("lidar_vehicle_bboxes", 10); + wall_boxes_pub_ = this->create_publisher("lidar_wall_bboxes", 10); + vehicle_marker_array_pub_ = this->create_publisher("vehicle_lidar_markers", 10); regions_[0] = 5; regions_[1] = 20; regions_[2] = 30; regions_[3] = 30; regions_[4] = 30; // FIXME: Add these to parameter files From e7a3a291d63122895d3ed05dfdf3b9f16e4186d7 Mon Sep 17 00:00:00 2001 From: Andres Date: Sun, 27 Oct 2024 23:33:38 -0400 Subject: [PATCH 04/11] Added namespace /perception/post/ --- param/better_clustering_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index bdcbe9e..fb8b505 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,4 +1,4 @@ -/adaptive_clustering: +/*/*/adaptive_clustering: ros__parameters: cluster_size_max: 500 cluster_size_min: 15 From 02cf6325caa66ecf54d702adc5fbdcddf5a2453e Mon Sep 17 00:00:00 2001 From: Andres Date: Tue, 5 Nov 2024 15:06:24 -0500 Subject: [PATCH 05/11] Fixed wrong topic namespace --- src/adaptive_clustering.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index 919bb17..dc76b2b 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -93,7 +93,7 @@ class AdaptiveClustering : public rclcpp::Node { car_length_ = this->get_parameter("car_length").get_parameter_value().get(); /*** Subscribers ***/ - point_cloud_sub = this->create_subscription("/perception/linefit_seg/ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, + point_cloud_sub = this->create_subscription("ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, this, std::placeholders::_1)); //wall_points_sub = this->create_subscription("/perception/wall_point_markers", 10, std::bind(&AdaptiveClustering::wallsCallback, // this, std::placeholders::_1)); @@ -161,6 +161,7 @@ class AdaptiveClustering : public rclcpp::Node { void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in) const { + RCLCPP_INFO(this->get_logger(), "PointCloud2 received"); // Retrieve parameters for "on the run" tuning: sensor_model = this->get_parameter("sensor_model").get_parameter_value().get(); print_fps_ = this->get_parameter("print_fps").get_parameter_value().get(); @@ -274,7 +275,7 @@ class AdaptiveClustering : public rclcpp::Node { cluster->width = cluster->size(); cluster->height = 1; cluster->is_dense = true; - clusters.push_back(cluster); + clusters.push_back(cluster); } /*** Merge z-axis clusters ***/ for(int j = last_clusters_end; j < clusters.size(); j++) { From c2fe10c6d2c9f4f1c41acbb8c3fb5ff002e2aef6 Mon Sep 17 00:00:00 2001 From: Andres Date: Tue, 5 Nov 2024 15:09:47 -0500 Subject: [PATCH 06/11] Removed logger --- src/adaptive_clustering.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index dc76b2b..2d32fe3 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -161,7 +161,6 @@ class AdaptiveClustering : public rclcpp::Node { void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in) const { - RCLCPP_INFO(this->get_logger(), "PointCloud2 received"); // Retrieve parameters for "on the run" tuning: sensor_model = this->get_parameter("sensor_model").get_parameter_value().get(); print_fps_ = this->get_parameter("print_fps").get_parameter_value().get(); From 04882d89a7096d7614e4a7c9964e25e6b962d53e Mon Sep 17 00:00:00 2001 From: Eyan hudson Date: Wed, 13 Nov 2024 11:47:24 -0500 Subject: [PATCH 07/11] updated parameters for euclidean clustering --- param/better_clustering_config.yaml | 10 +++++----- src/adaptive_clustering.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index fb8b505..8add17f 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,16 +1,16 @@ /*/*/adaptive_clustering: ros__parameters: - cluster_size_max: 500 + cluster_size_max: 750 cluster_size_min: 15 generate_bounding_boxes: true - k_merging_threshold: 0.10000000149011612 + k_merging_threshold: 0.10000000149011612 # not used leaf: 1 print_fps: false radius_max: 120.0 - radius_min: 0.4000000059604645 + radius_min: 0.4000000059604645 sensor_model: VLP-16 - z_axis_max: 3.0 + z_axis_max: 2.0 z_axis_min: -0.800000011920929 - z_merging_threshold: 0.0 + z_merging_threshold: 0.0 # not used car_width: 2.0 car_length: 4.8768 diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index 2d32fe3..a3e80cb 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -265,7 +265,7 @@ class AdaptiveClustering : public rclcpp::Node { *cluster += *clusters[j]; clusters.erase(clusters.begin()+j); last_clusters_end--; - //std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; + std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; } } } @@ -408,7 +408,7 @@ class AdaptiveClustering : public rclcpp::Node { //if (!valid) // NOTE May not need this with the addition of the off-map filter (CarProximityReporter) - if ((box.size.x * box.size.y * box.size.z >= 30.0) || box.size.x > 7.0 || (box.size.y / box.size.x > 3.0) || !valid) + if ((box.size.x * box.size.y * box.size.z >= 25.0) || box.size.x > 6.0 || (box.size.y / box.size.x > 1.0) || !valid) { // If this is true, the box is a wall // marker color m.color.r = 0.0; From afcec42407653c780f635cf0cb9d1f937f0765ad Mon Sep 17 00:00:00 2001 From: Eyan hudson Date: Thu, 14 Nov 2024 12:55:40 -0500 Subject: [PATCH 08/11] added to param files and adjusted wall bounding box size --- param/better_clustering_config.yaml | 4 +++- src/adaptive_clustering.cpp | 29 +++++++++++++++++++---------- 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index 8add17f..d2bf469 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -3,7 +3,7 @@ cluster_size_max: 750 cluster_size_min: 15 generate_bounding_boxes: true - k_merging_threshold: 0.10000000149011612 # not used + k_merging_threshold: 0.10000000149011612 leaf: 1 print_fps: false radius_max: 120.0 @@ -14,3 +14,5 @@ z_merging_threshold: 0.0 # not used car_width: 2.0 car_length: 4.8768 + regions: [5, 20, 30, 30, 30] + tolerance: 2.0 diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index a3e80cb..c040f4e 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -75,6 +75,12 @@ class AdaptiveClustering : public rclcpp::Node { this->declare_parameter("generate_bounding_boxes", true); this->declare_parameter("car_width",2.0); this->declare_parameter("car_length",4.8768); + // get regions from param + std::vector default_regions = {5, 20, 30, 30, 30}; // Default values for regions + this->declare_parameter>("regions", default_regions); + // get tolerance from param + this->declare_parameter("tolerance",2.0); + sensor_model = this->get_parameter("sensor_model").get_parameter_value().get(); @@ -91,6 +97,8 @@ class AdaptiveClustering : public rclcpp::Node { generate_bounding_boxes = this->get_parameter("generate_bounding_boxes").get_parameter_value().get(); car_width_ = this->get_parameter("car_width").get_parameter_value().get(); car_length_ = this->get_parameter("car_length").get_parameter_value().get(); + regions_ = this->get_parameter("regions").get_parameter_value().get>(); + tolerance_ = this->get_parameter("tolerance").get_parameter_value().get(); /*** Subscribers ***/ point_cloud_sub = this->create_subscription("ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, @@ -115,8 +123,6 @@ class AdaptiveClustering : public rclcpp::Node { vehicle_marker_array_pub_ = this->create_publisher("vehicle_lidar_markers", 10); - regions_[0] = 5; regions_[1] = 20; regions_[2] = 30; regions_[3] = 30; regions_[4] = 30; // FIXME: Add these to parameter files - reset = true;//fps frames = 0; start_time = clock(); @@ -173,6 +179,9 @@ class AdaptiveClustering : public rclcpp::Node { z_merging_threshold_ = this->get_parameter("z_merging_threshold").get_parameter_value().get(); radius_min_ = this->get_parameter("radius_min").get_parameter_value().get(); radius_max_ = this->get_parameter("radius_max").get_parameter_value().get(); + regions_ = this->get_parameter("regions").get_parameter_value().get>(); + tolerance_ = this->get_parameter("tolerance").get_parameter_value().get(); + if(print_fps_ && reset){frames=0; start_time=clock(); reset=false;}//fps @@ -211,7 +220,7 @@ class AdaptiveClustering : public rclcpp::Node { } /*** Euclidean clustering ***/ - float tolerance = 2.0; // TODO: Retrieve this from param file + //float tolerance = 2.0; // TODO: Retrieve this from param file std::vector::Ptr, Eigen::aligned_allocator::Ptr > > clusters; int last_clusters_begin = 0; int last_clusters_end = 0; @@ -222,7 +231,7 @@ class AdaptiveClustering : public rclcpp::Node { //RCLCPP_INFO(this->get_logger(), "Current time: %ld.%09ld", current_time.seconds(), current_time.nanoseconds()); for(int i = 0; i < region_max_; i++) { - tolerance += 0.0; //3*0.1; + tolerance_ += 0.0; //3*0.1; if(indices_array[i].size() > cluster_size_min_) { #if PCL_VERSION_COMPARE(<, 1, 11, 0) boost::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); @@ -235,7 +244,7 @@ class AdaptiveClustering : public rclcpp::Node { std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(tolerance); + ec.setClusterTolerance(tolerance_); ec.setMinClusterSize(cluster_size_min_); ec.setMaxClusterSize(cluster_size_max_); ec.setSearchMethod(tree); @@ -265,7 +274,7 @@ class AdaptiveClustering : public rclcpp::Node { *cluster += *clusters[j]; clusters.erase(clusters.begin()+j); last_clusters_end--; - std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; + // std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; } } } @@ -408,8 +417,8 @@ class AdaptiveClustering : public rclcpp::Node { //if (!valid) // NOTE May not need this with the addition of the off-map filter (CarProximityReporter) - if ((box.size.x * box.size.y * box.size.z >= 25.0) || box.size.x > 6.0 || (box.size.y / box.size.x > 1.0) || !valid) - { // If this is true, the box is a wall + if ((box.size.x * box.size.y * box.size.z >= 12.0) || box.size.x > 6.0 || (box.size.y / box.size.x > 1.0) || !valid) + { // If this is true, the box is bigger than the car // marker color m.color.r = 0.0; m.color.g = 0.0; @@ -485,11 +494,11 @@ class AdaptiveClustering : public rclcpp::Node { mutable float radius_max_; mutable float car_width_; mutable float car_length_; + mutable std::vector regions_; + mutable float tolerance_; const int region_max_ = 5; // 10 Change this value to match how far you want to detect. - int regions_[4]; - mutable int frames; mutable clock_t start_time; mutable bool reset; From 376a63b3c7eef7a2b2de5b0c7c14b38fb9468e63 Mon Sep 17 00:00:00 2001 From: Eyan hudson Date: Sat, 23 Nov 2024 11:46:24 -0500 Subject: [PATCH 09/11] ransac cylinder work in progress --- src/dev/adaptive_clustering_ransac.cpp | 371 +++++++++++++++++++++++++ 1 file changed, 371 insertions(+) create mode 100644 src/dev/adaptive_clustering_ransac.cpp diff --git a/src/dev/adaptive_clustering_ransac.cpp b/src/dev/adaptive_clustering_ransac.cpp new file mode 100644 index 0000000..ded7922 --- /dev/null +++ b/src/dev/adaptive_clustering_ransac.cpp @@ -0,0 +1,371 @@ + +// Copyright (C) 2018 Zhi Yan and Li Sun + +// This program is free software: you can redistribute it and/or modify it +// under the terms of the GNU General Public License as published by the Free +// Software Foundation, either version 3 of the License, or (at your option) +// any later version. + +// This program is distributed in the hope that it will be useful, but WITHOUT +// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for +// more details. + +// You should have received a copy of the GNU General Public License along +// with this program. If not, see . + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "geometry_msgs/msg/pose_array.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "blackandgold_msgs/msg/cluster_array.hpp" +#include "blackandgold_msgs/msg/polynomial4.hpp" +#include "blackandgold_msgs/msg/polynomial4_array.hpp" +#include +#include +#include +#include +#include + +// PCL +#include "pcl/pcl_config.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/filters/voxel_grid.h" +#include "pcl/filters/passthrough.h" +#include "pcl/segmentation/extract_clusters.h" +#include "pcl/common/common.h" +#include "pcl/common/centroid.h" +#include +#include +#include +#include + +// RANSAC: +#include +#include +#include +#include + +using namespace std::chrono_literals; + +//#define LOG +class AdaptiveClustering : public rclcpp::Node { + + public: + AdaptiveClustering(): Node("adaptive_clustering"){ + + + // rclcpp::Publisher::SharedPtr cylinder_cloud_pub_; + // rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; + // rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; + // rclcpp::Subscription::SharedPtr point_cloud_sub; + + point_cloud_sub = this->create_subscription( + "ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, this, std::placeholders::_1) + ); + + cylinder_cloud_pub_ = this->create_publisher("cylinder_cloud", 10); + vehicle_boxes_pub_ = this->create_publisher("lidar_vehicle_bboxes", 10); + vehicle_marker_array_pub_ = this->create_publisher("vehicle_lidar_markers", 10); + + + //regions_[0] = 5; regions_[1] = 20; regions_[2] = 30; regions_[3] = 30; regions_[4] = 30; // FIXME: Add these to parameter files + + //reset = true;//fps + //frames = 0; + //start_time = clock(); + + } + + private: + + rclcpp::Publisher::SharedPtr cylinder_cloud_pub_; + rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; + rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; + rclcpp::Subscription::SharedPtr point_cloud_sub; + autoware_auto_perception_msgs::msg::BoundingBox box; + sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in; + blackandgold_msgs::msg::Polynomial4Array polynomials; + autoware_auto_perception_msgs::msg::BoundingBoxArray vehicle_bounding_boxes; + visualization_msgs::msg::MarkerArray vehicle_markers; + + + + + // Polynomial structure: + struct RootsAndCount + { + int count; + float roots[3]; + }; + + + bool isOutOfBounds_v2(blackandgold_msgs::msg::Polynomial4Array polynomials, autoware_auto_perception_msgs::msg::BoundingBox box) const + { + for(unsigned int i = 0; i < polynomials.polynomials.size(); i++) + { + if (box.centroid.x >= polynomials.polynomials[i].x_min.data && + box.centroid.x <= polynomials.polynomials[i].x_max.data) + { + auto polynomial = polynomials.polynomials[i].polynomial; + float threshold_value = polynomial.data[0]*pow(box.centroid.x,4) + polynomial.data[1]*pow(box.centroid.x,3) + + polynomial.data[2]*pow(box.centroid.x,2) + polynomial.data[3]*box.centroid.x + polynomial.data[4]; + + + + if (abs(box.centroid.y) >= abs(threshold_value) - 1.0 && std::signbit(box.centroid.y) == std::signbit(threshold_value)) + { + RCLCPP_INFO(this->get_logger(), "Threshold: '%f'", threshold_value); + return false; + } + + } + + } + return true; + } + + + + + pcl::PointCloud::Ptr detectCylinder(const pcl::PointCloud::Ptr& pcl_pc_in) { + + // rclcpp::Publisher::SharedPtr cylinder_cloud_pub_; + // rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; + // rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; + + // Create segmentation object for cylinder + //pcl::SACSegmentationFromNormals seg; + pcl::SACSegmentation seg; + + pcl::PointIndices::Ptr cylinder_inliers(new pcl::PointIndices()); + pcl::ModelCoefficients::Ptr cylinder_coefficients(new pcl::ModelCoefficients()); + + + // Set axis for the cylinder model and epsilon angle tolerance + seg.setAxis(Eigen::Vector3f(0, 1, 0)); // Adjust as needed for expected cylinder orientation + seg.setEpsAngle(0.2); // Allow tolerance in angle + + // Define radius limits + seg.setRadiusLimits(0.1, 0.5); // Expected radius range of the cylinder + + // Normal estimation object + //pcl::NormalEstimation ne; + // pcl::PointCloud::Ptr normals(new pcl::PointCloud()); + + // Create a KdTree for searching neighbors + // pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // ne.setSearchMethod(tree); + // ne.setInputCloud(pcl_pc_in); // Input point cloud + // ne.setRadiusSearch(0.03); // Radius in meters (adjust based on your point cloud scale) + // Alternatively, you can use setKSearch: + // ne.setKSearch(50); // Use 50 nearest neighbors + // ne.compute(*normals); // Compute the normals + + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_CYLINDER); // Use cylinder model + seg.setMethodType(pcl::SAC_RANSAC); // Use RANSAC for model fitting + seg.setMaxIterations(1000); // Set max iterations + seg.setDistanceThreshold(0.2); // Set distance threshold for inliers + + // Set input point cloud and normals + seg.setInputCloud(pcl_pc_in); + // seg.setInputNormals(normals); // Provide the normals + + + seg.segment(*cylinder_inliers, *cylinder_coefficients); + + if (cylinder_inliers->indices.empty()) { + RCLCPP_INFO(this->get_logger(), "No cylinder found."); + return nullptr; + } + + // Step 2: Extract inliers and calculate the length + if (cylinder_inliers->indices.size() > 0) { + pcl::PointCloud::Ptr cylinder_cloud(new pcl::PointCloud()); + pcl::ExtractIndices extract; + extract.setInputCloud(pcl_pc_in); + extract.setIndices(cylinder_inliers); + extract.setNegative(false); + extract.filter(*cylinder_cloud); + + // Calculate cylinder axis direction vector from model coefficients + Eigen::Vector3f cylinder_axis(cylinder_coefficients->values[3], + cylinder_coefficients->values[4], + cylinder_coefficients->values[5]); + + // Project points onto the cylinder axis + Eigen::Vector3f base_point(cylinder_coefficients->values[0], + cylinder_coefficients->values[1], + cylinder_coefficients->values[2]); + + float min_proj = std::numeric_limits::max(); + float max_proj = -std::numeric_limits::max(); + + for (const auto& point : cylinder_cloud->points) { + Eigen::Vector3f pt(point.x, point.y, point.z); + float projection = cylinder_axis.dot(pt - base_point); + + if (projection < min_proj) min_proj = projection; + if (projection > max_proj) max_proj = projection; + } + + // Compute the cylinder length + float cylinder_length = max_proj - min_proj; + + // Step 3: Check if cylinder length matches desired length range + float desired_min_length = .2; // Define minimum length 1.0 + float desired_max_length = 5.0; // Define maximum length 2.5 + if (cylinder_length >= desired_min_length && cylinder_length <= desired_max_length) { + // Cylinder length is acceptable; proceed with processing + sensor_msgs::msg::PointCloud2 cylinder_msg; + pcl::toROSMsg(*cylinder_cloud, cylinder_msg); + cylinder_msg.header.frame_id = "center_of_gravity"; + cylinder_cloud_pub_->publish(cylinder_msg); + + computeBoundingBoxForCylinder(cylinder_cloud, *cylinder_coefficients); + return cylinder_cloud; + + } else { + // Cylinder length is out of desired range; discard + RCLCPP_INFO(this->get_logger(), "Cylinder length is out of desired range; discard"); + return nullptr; + } + } + return nullptr; +} + + + void computeBoundingBoxForCylinder(const pcl::PointCloud::Ptr& cylinder_cloud, + const pcl::ModelCoefficients& coefficients) { + + // autoware_auto_perception_msgs::msg::BoundingBox box; + // rclcpp::Publisher::SharedPtr cylinder_cloud_pub_; + // rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; + // rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; + // pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); + // blackandgold_msgs::msg::Polynomial4Array polynomials; + // autoware_auto_perception_msgs::msg::BoundingBoxArray vehicle_bounding_boxes; + // visualization_msgs::msg::MarkerArray vehicle_markers; + // sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in; + + // Set centroid (x, y, z) + box.centroid.x = coefficients.values[0]; + box.centroid.y = coefficients.values[1]; + box.centroid.z = coefficients.values[2]; + + // Set dimensions based on cylinder radius and approximate height from cylinder_cloud + float cylinder_radius = coefficients.values[6]; + box.size.x = cylinder_radius * 2; // Diameter + box.size.y = cylinder_radius * 2; // Diameter + box.size.z = computeCylinderHeight(cylinder_cloud); // Calculate height + + // Set orientation along cylinder axis + tf2::Quaternion orientation; + orientation.setRPY(0, 0, atan2(coefficients.values[4], coefficients.values[3])); + box.orientation.x = orientation.x(); + box.orientation.y = orientation.y(); + box.orientation.z = orientation.z(); + box.orientation.w = orientation.w(); + + + // deal with markers + visualization_msgs::msg::Marker m; + m.header = ros_pc2_in->header; + m.ns = "bbox"; + // m.id = i; + m.type = visualization_msgs::msg::Marker::CUBE; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose.position.x = box.centroid.x; + m.pose.position.y = box.centroid.y; + m.pose.position.z = box.centroid.z; + m.pose.orientation.x = box.orientation.x; + m.pose.orientation.y = box.orientation.y; + m.pose.orientation.z = box.orientation.z; + m.pose.orientation.w = box.orientation.w; + + m.scale.x = box.size.x; + m.scale.y = box.size.y; + m.scale.z = box.size.z; + + // bool valid = isOutOfBounds_v2(polynomials, box); + + // color the vechicle box red + RCLCPP_DEBUG(this->get_logger(), "Poly size: '%lu'", polynomials.polynomials.size()); + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 0.0; + m.color.a = 0.75; + m.lifetime.sec = 0; + m.lifetime.nanosec = 100000000; + vehicle_bounding_boxes.boxes.push_back(box); + vehicle_markers.markers.push_back(m); + + if(vehicle_bounding_boxes.boxes.size()) { + + // Deal with headers: + vehicle_bounding_boxes.header = ros_pc2_in->header; + + // Publish bounding box + vehicle_boxes_pub_->publish(vehicle_bounding_boxes); + vehicle_marker_array_pub_->publish(vehicle_markers); + } + + } + + float computeCylinderHeight(const pcl::PointCloud::Ptr& cylinder_cloud) { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D(*cylinder_cloud, min_pt, max_pt); + return max_pt[2] - min_pt[2]; + } + + + + + + + + + + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in) + { + + + // visualization_msgs::msg::MarkerArray boundary_points; + // rclcpp::Subscription::SharedPtr point_cloud_sub; + // blackandgold_msgs::msg::Polynomial4Array polynomials; + // rclcpp::Publisher::SharedPtr cylinder_cloud_pub_; + // rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; + // rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; + + /*** Convert ROS message to PCL ***/ + pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); + pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in); + + // check input to see if empty, useful for debugging + if (pcl_pc_in->empty()) { + RCLCPP_WARN(this->get_logger(), "Input cloud is empty. Skipping cylinder detection."); + return; + } + + + // Call RANSAC Cylinder Function + pcl::PointCloud::Ptr ransac_cylinder_cloud = detectCylinder(pcl_pc_in); + + + + + + } +}; + + + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file From caab5f3e1827f82c7209f5e1d1735f5ae64ce094 Mon Sep 17 00:00:00 2001 From: Eyan hudson Date: Tue, 3 Dec 2024 13:21:31 -0500 Subject: [PATCH 10/11] added regions to adaptive clustering --- param/better_clustering_config.yaml | 9 ++++--- src/adaptive_clustering.cpp | 41 ++++++++++++++++++----------- 2 files changed, 30 insertions(+), 20 deletions(-) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index d2bf469..5083f24 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,7 +1,7 @@ /*/*/adaptive_clustering: ros__parameters: - cluster_size_max: 750 - cluster_size_min: 15 + cluster_size_max: [500, 400, 250, 150, 100] #750 + cluster_size_min: [150, 75, 50, 25, 15] #15 generate_bounding_boxes: true k_merging_threshold: 0.10000000149011612 leaf: 1 @@ -14,5 +14,6 @@ z_merging_threshold: 0.0 # not used car_width: 2.0 car_length: 4.8768 - regions: [5, 20, 30, 30, 30] - tolerance: 2.0 + regions: [10, 10, 10, 10, 10] + region_max: 5 + tolerance: 1.0 \ No newline at end of file diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp index c040f4e..7631e60 100644 --- a/src/adaptive_clustering.cpp +++ b/src/adaptive_clustering.cpp @@ -57,10 +57,14 @@ class AdaptiveClustering : public rclcpp::Node { this->declare_parameter("z_axis_min", -0.8); //private_nh.param("z_axis_max", z_axis_max_, 2.0); this->declare_parameter("z_axis_max", 10.0); + + std::vector default_cluster_size_min = {50, 25, 20, 10, 5}; // Default values for cluster_size_min + std::vector default_cluster_size_max = {200, 150, 100, 50, 30}; // Default values for cluster_size_max //private_nh.param("cluster_size_min", s, 3); - this->declare_parameter("cluster_size_min", 10); + this->declare_parameter>("cluster_size_min", default_cluster_size_min); //private_nh.param("cluster_size_max", cluster_size_max_, 2200000); - this->declare_parameter("cluster_size_max", 5000); + this->declare_parameter>("cluster_size_max", default_cluster_size_max); + //private_nh.param("leaf", leaf_, 3); this->declare_parameter("leaf", 3); //private_nh.param("k_merging_threshold", k_merging_threshold_, 0.1); @@ -80,6 +84,7 @@ class AdaptiveClustering : public rclcpp::Node { this->declare_parameter>("regions", default_regions); // get tolerance from param this->declare_parameter("tolerance",2.0); + this->declare_parameter("region_max", 5); // how many regions you want to detect. @@ -87,8 +92,8 @@ class AdaptiveClustering : public rclcpp::Node { print_fps_ = this->get_parameter("print_fps").get_parameter_value().get(); z_axis_min_ = this->get_parameter("z_axis_min").get_parameter_value().get(); z_axis_max_ = this->get_parameter("z_axis_max").get_parameter_value().get(); - cluster_size_min_ = this->get_parameter("cluster_size_min").get_parameter_value().get(); - cluster_size_max_ = this->get_parameter("cluster_size_max").get_parameter_value().get(); + cluster_size_min_ = this->get_parameter("cluster_size_min").get_parameter_value().get>(); + cluster_size_max_ = this->get_parameter("cluster_size_max").get_parameter_value().get>(); leaf_ = this->get_parameter("leaf").get_parameter_value().get(); k_merging_threshold_ = this->get_parameter("k_merging_threshold").get_parameter_value().get(); z_merging_threshold_ = this->get_parameter("z_merging_threshold").get_parameter_value().get(); @@ -99,6 +104,7 @@ class AdaptiveClustering : public rclcpp::Node { car_length_ = this->get_parameter("car_length").get_parameter_value().get(); regions_ = this->get_parameter("regions").get_parameter_value().get>(); tolerance_ = this->get_parameter("tolerance").get_parameter_value().get(); + region_max_ = this->get_parameter("region_max").get_parameter_value().get(); /*** Subscribers ***/ point_cloud_sub = this->create_subscription("ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, @@ -172,8 +178,8 @@ class AdaptiveClustering : public rclcpp::Node { print_fps_ = this->get_parameter("print_fps").get_parameter_value().get(); z_axis_min_ = this->get_parameter("z_axis_min").get_parameter_value().get(); z_axis_max_ = this->get_parameter("z_axis_max").get_parameter_value().get(); - cluster_size_min_ = this->get_parameter("cluster_size_min").get_parameter_value().get(); - cluster_size_max_ = this->get_parameter("cluster_size_max").get_parameter_value().get(); + cluster_size_min_ = this->get_parameter("cluster_size_min").get_parameter_value().get>(); + cluster_size_max_ = this->get_parameter("cluster_size_max").get_parameter_value().get>(); leaf_ = this->get_parameter("leaf").get_parameter_value().get(); k_merging_threshold_ = this->get_parameter("k_merging_threshold").get_parameter_value().get(); z_merging_threshold_ = this->get_parameter("z_merging_threshold").get_parameter_value().get(); @@ -220,7 +226,7 @@ class AdaptiveClustering : public rclcpp::Node { } /*** Euclidean clustering ***/ - //float tolerance = 2.0; // TODO: Retrieve this from param file + float tolerance = tolerance_; std::vector::Ptr, Eigen::aligned_allocator::Ptr > > clusters; int last_clusters_begin = 0; int last_clusters_end = 0; @@ -231,8 +237,8 @@ class AdaptiveClustering : public rclcpp::Node { //RCLCPP_INFO(this->get_logger(), "Current time: %ld.%09ld", current_time.seconds(), current_time.nanoseconds()); for(int i = 0; i < region_max_; i++) { - tolerance_ += 0.0; //3*0.1; - if(indices_array[i].size() > cluster_size_min_) { + tolerance += 0.5; //3*0.1; + if(indices_array[i].size() > cluster_size_min_[i]) { #if PCL_VERSION_COMPARE(<, 1, 11, 0) boost::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); #else @@ -244,9 +250,9 @@ class AdaptiveClustering : public rclcpp::Node { std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(tolerance_); - ec.setMinClusterSize(cluster_size_min_); - ec.setMaxClusterSize(cluster_size_max_); + ec.setClusterTolerance(tolerance); + ec.setMinClusterSize(cluster_size_min_[i]); + ec.setMaxClusterSize(cluster_size_max_[i]); ec.setSearchMethod(tree); ec.setInputCloud(pcl_pc_in); ec.setIndices(indices_array_ptr); @@ -360,6 +366,9 @@ class AdaptiveClustering : public rclcpp::Node { box.size.x = max[0] - min[0]; box.size.y = max[1] - min[1]; box.size.z = max[2] - min[2]; + + // set the number of points to the value of the box + box.value = clusters[i]->points.size(); // Compute roll angle of bounding box // float roll = atan2(max[1] - min[1], max[0] - min[0]); @@ -427,6 +436,7 @@ class AdaptiveClustering : public rclcpp::Node { m.lifetime.sec = 0; m.lifetime.nanosec = 100000000; wall_bounding_boxes.boxes.push_back(box); + } else// if (abs(box.centroid.y) < 20.0) @@ -485,8 +495,8 @@ class AdaptiveClustering : public rclcpp::Node { mutable bool print_fps_; mutable float z_axis_min_; mutable float z_axis_max_; - mutable int cluster_size_min_; - mutable int cluster_size_max_; + mutable std::vector cluster_size_min_; + mutable std::vector cluster_size_max_; mutable int leaf_; mutable float k_merging_threshold_; mutable float z_merging_threshold_; @@ -496,8 +506,7 @@ class AdaptiveClustering : public rclcpp::Node { mutable float car_length_; mutable std::vector regions_; mutable float tolerance_; - - const int region_max_ = 5; // 10 Change this value to match how far you want to detect. + mutable int region_max_ = 5; // 10 Change this value to match how far you want to detect. mutable int frames; mutable clock_t start_time; From afa981b2bb22dc43e08412b323ed6cc7a934202f Mon Sep 17 00:00:00 2001 From: Eyan hudson Date: Tue, 10 Dec 2024 22:06:19 -0500 Subject: [PATCH 11/11] updated the region parameters --- param/better_clustering_config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index 5083f24..ca5f565 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,7 +1,7 @@ /*/*/adaptive_clustering: ros__parameters: - cluster_size_max: [500, 400, 250, 150, 100] #750 - cluster_size_min: [150, 75, 50, 25, 15] #15 + cluster_size_max: [750, 500, 250, 150, 100] #750 + cluster_size_min: [100, 80, 60, 35, 20] #15 generate_bounding_boxes: true k_merging_threshold: 0.10000000149011612 leaf: 1 @@ -16,4 +16,4 @@ car_length: 4.8768 regions: [10, 10, 10, 10, 10] region_max: 5 - tolerance: 1.0 \ No newline at end of file + tolerance: 0.5 \ No newline at end of file