From ed1944be64a05cde37e20c2c5c217dc0be88e655 Mon Sep 17 00:00:00 2001 From: drake Date: Tue, 17 Dec 2024 13:37:22 -0500 Subject: [PATCH 1/5] added new adaptive parmams for k_means and tolerance --- .gitignore | 32 -- .travis.yml | 23 -- CMakeLists.txt | 87 ----- LICENSE | 29 -- README.md | 34 -- msg/ClusterArray.msg | 2 - package.xml | 40 -- param/adaptive_clustering.yaml | 16 - param/better_clustering_config.yaml | 19 - param/better_clustering_config_B.yaml | 27 -- src/adaptive_clustering.cpp | 542 -------------------------- src/dev/adaptive_clustering2.cpp | 371 ------------------ 12 files changed, 1222 deletions(-) delete mode 100644 .gitignore delete mode 100644 .travis.yml delete mode 100644 CMakeLists.txt delete mode 100644 LICENSE delete mode 100644 README.md delete mode 100644 msg/ClusterArray.msg delete mode 100644 package.xml delete mode 100644 param/adaptive_clustering.yaml delete mode 100644 param/better_clustering_config.yaml delete mode 100644 param/better_clustering_config_B.yaml delete mode 100644 src/adaptive_clustering.cpp delete mode 100644 src/dev/adaptive_clustering2.cpp diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 259148f..0000000 --- a/.gitignore +++ /dev/null @@ -1,32 +0,0 @@ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 94af1d0..0000000 --- a/.travis.yml +++ /dev/null @@ -1,23 +0,0 @@ -# DEPRECATED -dist: xenial -sudo: required -language: generic -cache: apt - -install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - - sudo apt-get update - - sudo apt-get install ros-kinetic-desktop-full - - source /opt/ros/kinetic/setup.bash - - pip install catkin_pkg empy - - mkdir -p ~/catkin_ws/src - - cd ~/catkin_ws/ - - catkin_make - - source devel/setup.bash - -script: - - cd ~/catkin_ws/src - - git clone https://github.com/yzrobot/adaptive_clustering.git - - cd ~/catkin_ws - - catkin_make diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 45ba110..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,87 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(adaptive_clustering) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -ament_export_dependencies(rosidl_default_runtime) - -ament_auto_find_build_dependencies() - -set(msg_files - "msg/ClusterArray.msg" - DEPENDENCIES - std_msgs - sensor_msgs -) - -rosidl_generate_interfaces(${PROJECT_NAME} - ${msg_files} -) - -if(NOT COMMAND rosidl_get_typesupport_target) - function(rosidl_get_typesupport_target var generate_interfaces_target typesupport_name) - if(NOT TARGET ${generate_interfaces_target}) - message(FATAL_ERROR - "${generate_interfaces_target} is not a CMake target. Maybe rosidl_generate_interfaces was given a different target name?") - endif() - - set(output_target "${generate_interfaces_target}__${typesupport_name}") - - if(NOT TARGET ${output_target}) - # CMake if() evaluates strings ending in `-NOTFOUND` as false - set(output_target "${output_target}-NOTFOUND") - endif() - - set("${var}" "${output_target}" PARENT_SCOPE) - endfunction() -endif() - -rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") - -ament_auto_add_executable(${PROJECT_NAME}_node - src/adaptive_clustering.cpp -) - -#ament_auto_add_executable(${PROJECT_NAME}_node -# src/adaptive_clustering.cpp -#) - -target_link_libraries(${PROJECT_NAME}_node "${cpp_typesupport_target}") - -# export information to downstream packages -ament_auto_package( - INSTALL_TO_SHARE param -) - -# tests -if(AMENT_ENABLE_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - - #ament_add_gtest(${PROJECT_NAME}_gtest test/my_test.cpp) - #target_link_libraries(${PROJECT_NAME}_gtest ${rclcpp_LIBRARIES} ${rmw_connext_cpp_LIBRARIES} ${std_interfaces}) - - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() \ No newline at end of file diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 2b5f1d6..0000000 --- a/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2020, Zhi Yan -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md deleted file mode 100644 index 4708a9d..0000000 --- a/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# Adaptive Clustering: A lightweight and accurate point cloud clustering method # - -[![Build Status](https://travis-ci.org/yzrobot/adaptive_clustering.svg?branch=master)](https://travis-ci.org/yzrobot/adaptive_clustering) -[![Codacy Badge](https://api.codacy.com/project/badge/Grade/61a01a79a7ac41fd9deded9050ef6030)](https://www.codacy.com/app/yzrobot/adaptive_clustering?utm_source=github.com&utm_medium=referral&utm_content=yzrobot/adaptive_clustering&utm_campaign=Badge_Grade) -[![License](https://img.shields.io/badge/License-BSD%203--Clause-green.svg)](https://opensource.org/licenses/BSD-3-Clause) - -[![YouTube Video](https://img.youtube.com/vi/rmPn7mWssto/0.jpg)](https://www.youtube.com/watch?v=rmPn7mWssto) - -## Changelog ## - -* **\[Apr 14, 2022\]:** Two new branches, [gpu](https://github.com/yzrobot/adaptive_clustering/tree/gpu) and [agx](https://github.com/yzrobot/adaptive_clustering/tree/agx), have been created for GPU-based implementations: - * [gpu](https://github.com/yzrobot/adaptive_clustering/tree/gpu) is based on [PCL-GPU](https://pcl.readthedocs.io/projects/tutorials/en/master/#gpu) and has been tested with an NVIDIA TITAN Xp. - * [agx](https://github.com/yzrobot/adaptive_clustering/tree/agx) is based on [CUDA-PCL](https://github.com/NVIDIA-AI-IOT/cuda-pcl) and has been tested with an NVIDIA Jetson AGX Xavier. - -* **\[Feb 25, 2019\]:** A new branch, [devel](https://github.com/yzrobot/adaptive_clustering/tree/devel), faster (by downsampling) and better (by merging clusters split by nested regions and on the z-axis). - -## How to build ## -```sh -cd ~/catkin_ws/src/ -git clone https://github.com/yzrobot/adaptive_clustering.git -cd ~/catkin_ws -catkin_make -``` - -## Citation ## -If you are considering using this code, please reference the following: -``` -@article{yz19auro, - author = {Zhi Yan and Tom Duckett and Nicola Bellotto}, - title = {Online learning for 3D LiDAR-based human detection: Experimental analysis of point cloud clustering and classification methods}, - journal = {Autonomous Robots}, - year = {2019} -} -``` diff --git a/msg/ClusterArray.msg b/msg/ClusterArray.msg deleted file mode 100644 index eea433b..0000000 --- a/msg/ClusterArray.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -sensor_msgs/PointCloud2[] clusters diff --git a/package.xml b/package.xml deleted file mode 100644 index 1d7dc8b..0000000 --- a/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - adaptive_clustering - 0.0.2 - Light-weight real-time high-accuracy point cloud clustering. - Zhi Yan - Li Sun - Andres Hoyos - Haoguang Yang - BSD - - ament_cmake - ament_cmake_auto - - rosidl_default_generators - rclcpp - std_msgs - sensor_msgs - geometry_msgs - visualization_msgs - autoware_auto_perception_msgs - pcl_conversions - pcl_ros - blackandgold_msgs - tf2 - tf2_geometry_msgs - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - - rosidl_default_runtime - - rosidl_interface_packages - - - ament_cmake - - \ No newline at end of file diff --git a/param/adaptive_clustering.yaml b/param/adaptive_clustering.yaml deleted file mode 100644 index c6796c4..0000000 --- a/param/adaptive_clustering.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/adaptive_clustering: - ros__parameters: - cluster_size_max: 500 - cluster_size_min: 5 - 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 \ No newline at end of file diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml deleted file mode 100644 index 3b939d7..0000000 --- a/param/better_clustering_config.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/*/*/adaptive_clustering: - ros__parameters: - 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 - print_fps: false - radius_max: 120.0 - radius_min: 0.4000000059604645 - sensor_model: VLP-16 - z_axis_max: 2.0 - z_axis_min: -0.800000011920929 - z_merging_threshold: 0.0 # not used - car_width: 2.0 - car_length: 4.8768 - regions: [10, 18, 12, 10, 10] - region_max: 5 - tolerance: 0.5 diff --git a/param/better_clustering_config_B.yaml b/param/better_clustering_config_B.yaml deleted file mode 100644 index 7e9b2f2..0000000 --- a/param/better_clustering_config_B.yaml +++ /dev/null @@ -1,27 +0,0 @@ -car_length: 4.876800060272217 -car_width: 2.0 -cluster_size_max: 5000 -cluster_size_min: 200 -generate_bounding_boxes: true -k_merging_threshold: 0.10000000149011612 -leaf: 3 -print_fps: false -qos_overrides./parameter_events.publisher.depth: 1000 -qos_overrides./parameter_events.publisher.durability: volatile -qos_overrides./parameter_events.publisher.history: keep_last -qos_overrides./parameter_events.publisher.reliability: reliable -radius_max: 120.0 -radius_min: 0.3500000059604645 -regions: !!python/object/apply:array.array -- q -- - 5 - - 20 - - 30 - - 30 - - 30 -sensor_model: VLP-16 -tolerance: 2.0 -use_sim_time: false -z_axis_max: 10.0 -z_axis_min: -0.800000011920929 -z_merging_threshold: 0.0 diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp deleted file mode 100644 index 594d42d..0000000 --- a/src/adaptive_clustering.cpp +++ /dev/null @@ -1,542 +0,0 @@ - -// 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 - - -using namespace std::chrono_literals; - -//#define LOG -class AdaptiveClustering : public rclcpp::Node { - - public: - AdaptiveClustering(): Node("adaptive_clustering"){ - - - //private_nh.param("sensor_model", sensor_model, "VLP-16"); // VLP-16, HDL-32E, HDL-64E - this->declare_parameter("sensor_model", "VLP-16"); - //private_nh.param("print_fps", print_fps_, false); - this->declare_parameter("print_fps", false); - //private_nh.param("z_axis_min", z_axis_min_, -0.8); - 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", default_cluster_size_min); - //private_nh.param("cluster_size_max", cluster_size_max_, 2200000); - 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); - this->declare_parameter("k_merging_threshold", 0.1); - //private_nh.param("z_merging_threshold", z_merging_threshold_, 0.0); - this->declare_parameter("z_merging_threshold", 0.0); - //private_nh.param("radius_min", radius_min_, 0.0); - this->declare_parameter("radius_min", 0.4); - //private_nh.param("radius_max", radius_max_, 30.0); - this->declare_parameter("radius_max", 120.0); - // Whether we want to output bounding boxes, or the original algorithm line markers - 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); - this->declare_parameter("region_max", 5); // how many regions you want to detect. - - - - sensor_model = this->get_parameter("sensor_model").get_parameter_value().get(); - 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>(); - 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(); - radius_min_ = this->get_parameter("radius_min").get_parameter_value().get(); - radius_max_ = this->get_parameter("radius_max").get_parameter_value().get(); - 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(); - 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, - 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 ***/ - //cluster_array_pub_ = private_nh.advertise("clusters", 100); - cluster_array_pub_ = this->create_publisher("clusters", 10); - //cloud_filtered_pub_ = private_nh.advertise("cloud_filtered", 100); - cloud_filtered_pub_ = this->create_publisher("cloud_filtered", 10); - //pose_array_pub_ = private_nh.advertise("poses", 100); - pose_array_pub_ = this->create_publisher("poses", 10); - //marker_array_pub_ = private_nh.advertise("markers", 100); - marker_array_pub_ = this->create_publisher("clustering_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); - - - reset = true;//fps - frames = 0; - start_time = clock(); - - } - - private: - - // 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; - } - - - void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in) const - { - // 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(); - 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>(); - 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(); - 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 - - /*** Convert ROS message to PCL ***/ - pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); - pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in); - - - - pcl::IndicesPtr pc_indices(new std::vector); - for(unsigned int i = 0; i < pcl_pc_in->size(); ++i) { - pc_indices->push_back(i); - } - - - /*** Divide the point cloud into nested circular regions ***/ - #if PCL_VERSION_COMPARE(<, 1, 11, 0) - boost::array, 5> indices_array; - #else - std::array, 5> indices_array; - #endif - - for(unsigned int i = 0; i < pc_indices->size(); i++) { - float range = 0.0; - for(int j = 0; j < region_max_; j++) { - float d2 = pcl_pc_in->points[(*pc_indices)[i]].x * pcl_pc_in->points[(*pc_indices)[i]].x + - pcl_pc_in->points[(*pc_indices)[i]].y * pcl_pc_in->points[(*pc_indices)[i]].y + - pcl_pc_in->points[(*pc_indices)[i]].z * pcl_pc_in->points[(*pc_indices)[i]].z; - if(d2 > radius_min_ * radius_min_ && d2 < radius_max_ * radius_max_ && - d2 > range * range && d2 <= (range+regions_[j]) * (range+regions_[j])) { - indices_array[j].push_back((*pc_indices)[i]); - break; - } - range += regions_[j]; - } - } - - /*** Euclidean clustering ***/ - float tolerance = tolerance_; - std::vector::Ptr, Eigen::aligned_allocator::Ptr > > clusters; - int last_clusters_begin = 0; - int last_clusters_end = 0; - - auto pre_time = rclcpp::Clock{}.now(); - //auto current_time = clock->now(); - - //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.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 - std::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); - #endif - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(pcl_pc_in, indices_array_ptr); - auto clustering_start = std::chrono::high_resolution_clock::now(); - - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - 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); - 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); - for(std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { - cluster->points.push_back(pcl_pc_in->points[*pit]); - } - /*** Merge clusters separated by nested regions ***/ - for(int j = last_clusters_begin; j < last_clusters_end; j++) { - pcl::KdTreeFLANN kdtree; - int K = 1; //the number of neighbors to search for - std::vector k_indices(K); - std::vector k_sqr_distances(K); - kdtree.setInputCloud(cluster); - if(clusters[j]->points.size() >= 1) { - if(kdtree.nearestKSearch(*clusters[j], clusters[j]->points.size()-1, K, k_indices, k_sqr_distances) > 0) { - if(k_sqr_distances[0] < k_merging_threshold_) { - *cluster += *clusters[j]; - clusters.erase(clusters.begin()+j); - last_clusters_end--; - // std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; - } - } - } - } - /**************************************************/ - cluster->width = cluster->size(); - cluster->height = 1; - cluster->is_dense = true; - clusters.push_back(cluster); - } - /*** Merge z-axis clusters ***/ - for(int j = last_clusters_end; j < clusters.size(); j++) { - Eigen::Vector4f j_min, j_max; - pcl::getMinMax3D(*clusters[j], j_min, j_max); - for(int k = j+1; k < clusters.size(); k++) { - Eigen::Vector4f k_min, k_max; - pcl::getMinMax3D(*clusters[k], k_min, k_max); - if(std::max(std::min((double)j_max[0], (double)k_max[0]) - std::max((double)j_min[0], (double)k_min[0]), 0.0) * std::max(std::min((double)j_max[1], (double)k_max[1]) - std::max((double)j_min[1], (double)k_min[1]), 0.0) > z_merging_threshold_) { - *clusters[j] += *clusters[k]; - clusters.erase(clusters.begin()+k); - //std::cerr << "z-merging: clusters " << k << " is merged into " << j << std::endl; - } - } - } - /*****************************/ - last_clusters_begin = last_clusters_end; - last_clusters_end = clusters.size(); - } - } - - /*** Output ***/ - - //if(cloud_filtered_pub_->get_subscription_count() > 0) { - pcl::PointCloud::Ptr pcl_pc_out(new pcl::PointCloud); - sensor_msgs::msg::PointCloud2 ros_pc2_out; - //pcl::copyPointCloud(*pcl_pc_in, *pc_indices, *pcl_pc_out); - pcl::toROSMsg(*pcl_pc_in, ros_pc2_out); - - auto post_time = rclcpp::Clock{}.now(); - - auto time_elapsed = post_time.seconds() - pre_time.seconds(); - // RCLCPP_INFO(this->get_logger(), "Time taken: '%f'", time_elapsed); - - cloud_filtered_pub_->publish(ros_pc2_out); - //} - - - blackandgold_msgs::msg::ClusterArray cluster_array; - geometry_msgs::msg::PoseArray pose_array; - visualization_msgs::msg::MarkerArray marker_array; - autoware_auto_perception_msgs::msg::BoundingBoxArray bounding_boxes; - autoware_auto_perception_msgs::msg::BoundingBoxArray wall_bounding_boxes; - autoware_auto_perception_msgs::msg::BoundingBoxArray vehicle_bounding_boxes; - visualization_msgs::msg::MarkerArray bounding_boxes_markers; - visualization_msgs::msg::MarkerArray vehicle_markers; - - for(int i = 0; i < clusters.size(); i++) { - //if(cluster_array_pub_->get_subscription_count() > 0) { - sensor_msgs::msg::PointCloud2 ros_pc2_out; - pcl::toROSMsg(*clusters[i], ros_pc2_out); - cluster_array.clusters.push_back(ros_pc2_out); - //} - - //if(pose_array_pub_->get_subscription_count() > 0) { - - Eigen::Vector4f centroid; - pcl::compute3DCentroid(*clusters[i], centroid); - - // filter out the detection of ourselves - 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]; - - // 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]); - - // 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 >= 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; - 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.65; - 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); - - } - - if (bounding_boxes.boxes.size()) { - bounding_boxes.header = ros_pc2_in->header; - bounding_boxes_pub_->publish(bounding_boxes); - wall_bounding_boxes.header = ros_pc2_in->header; - marker_array_pub_->publish(bounding_boxes_markers); - wall_boxes_pub_->publish(wall_bounding_boxes); - } - - // Always publish vehicle bounding boxes, even if empty - vehicle_bounding_boxes.header = ros_pc2_in->header; - vehicle_boxes_pub_->publish(vehicle_bounding_boxes); - - // Always publish vehicle markers, even if empty - vehicle_marker_array_pub_->publish(vehicle_markers); - - - if(cluster_array.clusters.size()) { - cluster_array.header = ros_pc2_in->header; - 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(marker_array.markers.size()) { - marker_array_pub_->publish(marker_array); - } - - if(print_fps_)if(++frames>10){std::cerr<<"[adaptive_clustering] fps = "< cluster_size_min_; - mutable std::vector cluster_size_max_; - mutable int leaf_; - mutable float k_merging_threshold_; - mutable float z_merging_threshold_; - mutable float radius_min_; - mutable float radius_max_; - mutable float car_width_; - mutable float car_length_; - mutable std::vector regions_; - mutable float tolerance_; - 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; - mutable bool reset; - - visualization_msgs::msg::MarkerArray boundary_points; - - - bool generate_bounding_boxes; - mutable blackandgold_msgs::msg::Polynomial4Array polynomials; - rclcpp::Publisher::SharedPtr cloud_filtered_pub_; - rclcpp::Publisher::SharedPtr cluster_array_pub_; - rclcpp::Publisher::SharedPtr pose_array_pub_; - rclcpp::Publisher::SharedPtr marker_array_pub_; - rclcpp::Publisher::SharedPtr bounding_boxes_pub_; - rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; - rclcpp::Publisher::SharedPtr wall_boxes_pub_; - rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; - - rclcpp::Subscription::SharedPtr point_cloud_sub; - //rclcpp::Subscription::SharedPtr wall_points_sub; - -}; - - -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 diff --git a/src/dev/adaptive_clustering2.cpp b/src/dev/adaptive_clustering2.cpp deleted file mode 100644 index ded7922..0000000 --- a/src/dev/adaptive_clustering2.cpp +++ /dev/null @@ -1,371 +0,0 @@ - -// 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 977e239106dc2e8c6ae55e53a7f5e82377e6f1b3 Mon Sep 17 00:00:00 2001 From: drake Date: Tue, 17 Dec 2024 13:41:43 -0500 Subject: [PATCH 2/5] new tweaks --- .gitignore | 32 ++ .travis.yml | 23 ++ CMakeLists.txt | 87 ++++ LICENSE | 29 ++ README.md | 34 ++ msg/ClusterArray.msg | 2 + package.xml | 40 ++ param/adaptive_clustering.yaml | 16 + param/better_clustering_config.yaml | 44 +++ param/better_clustering_config_B.yaml | 27 ++ src/adaptive_clustering.cpp | 545 ++++++++++++++++++++++++++ src/dev/adaptive_clustering2.cpp | 371 ++++++++++++++++++ 12 files changed, 1250 insertions(+) create mode 100644 .gitignore create mode 100644 .travis.yml create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 README.md create mode 100644 msg/ClusterArray.msg create mode 100644 package.xml create mode 100644 param/adaptive_clustering.yaml create mode 100644 param/better_clustering_config.yaml create mode 100644 param/better_clustering_config_B.yaml create mode 100644 src/adaptive_clustering.cpp create mode 100644 src/dev/adaptive_clustering2.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..259148f --- /dev/null +++ b/.gitignore @@ -0,0 +1,32 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..94af1d0 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,23 @@ +# DEPRECATED +dist: xenial +sudo: required +language: generic +cache: apt + +install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + - sudo apt-get update + - sudo apt-get install ros-kinetic-desktop-full + - source /opt/ros/kinetic/setup.bash + - pip install catkin_pkg empy + - mkdir -p ~/catkin_ws/src + - cd ~/catkin_ws/ + - catkin_make + - source devel/setup.bash + +script: + - cd ~/catkin_ws/src + - git clone https://github.com/yzrobot/adaptive_clustering.git + - cd ~/catkin_ws + - catkin_make diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..45ba110 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.5) +project(adaptive_clustering) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +ament_export_dependencies(rosidl_default_runtime) + +ament_auto_find_build_dependencies() + +set(msg_files + "msg/ClusterArray.msg" + DEPENDENCIES + std_msgs + sensor_msgs +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} +) + +if(NOT COMMAND rosidl_get_typesupport_target) + function(rosidl_get_typesupport_target var generate_interfaces_target typesupport_name) + if(NOT TARGET ${generate_interfaces_target}) + message(FATAL_ERROR + "${generate_interfaces_target} is not a CMake target. Maybe rosidl_generate_interfaces was given a different target name?") + endif() + + set(output_target "${generate_interfaces_target}__${typesupport_name}") + + if(NOT TARGET ${output_target}) + # CMake if() evaluates strings ending in `-NOTFOUND` as false + set(output_target "${output_target}-NOTFOUND") + endif() + + set("${var}" "${output_target}" PARENT_SCOPE) + endfunction() +endif() + +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") + +ament_auto_add_executable(${PROJECT_NAME}_node + src/adaptive_clustering.cpp +) + +#ament_auto_add_executable(${PROJECT_NAME}_node +# src/adaptive_clustering.cpp +#) + +target_link_libraries(${PROJECT_NAME}_node "${cpp_typesupport_target}") + +# export information to downstream packages +ament_auto_package( + INSTALL_TO_SHARE param +) + +# tests +if(AMENT_ENABLE_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + #ament_add_gtest(${PROJECT_NAME}_gtest test/my_test.cpp) + #target_link_libraries(${PROJECT_NAME}_gtest ${rclcpp_LIBRARIES} ${rmw_connext_cpp_LIBRARIES} ${std_interfaces}) + + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() \ No newline at end of file diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..2b5f1d6 --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2020, Zhi Yan +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..4708a9d --- /dev/null +++ b/README.md @@ -0,0 +1,34 @@ +# Adaptive Clustering: A lightweight and accurate point cloud clustering method # + +[![Build Status](https://travis-ci.org/yzrobot/adaptive_clustering.svg?branch=master)](https://travis-ci.org/yzrobot/adaptive_clustering) +[![Codacy Badge](https://api.codacy.com/project/badge/Grade/61a01a79a7ac41fd9deded9050ef6030)](https://www.codacy.com/app/yzrobot/adaptive_clustering?utm_source=github.com&utm_medium=referral&utm_content=yzrobot/adaptive_clustering&utm_campaign=Badge_Grade) +[![License](https://img.shields.io/badge/License-BSD%203--Clause-green.svg)](https://opensource.org/licenses/BSD-3-Clause) + +[![YouTube Video](https://img.youtube.com/vi/rmPn7mWssto/0.jpg)](https://www.youtube.com/watch?v=rmPn7mWssto) + +## Changelog ## + +* **\[Apr 14, 2022\]:** Two new branches, [gpu](https://github.com/yzrobot/adaptive_clustering/tree/gpu) and [agx](https://github.com/yzrobot/adaptive_clustering/tree/agx), have been created for GPU-based implementations: + * [gpu](https://github.com/yzrobot/adaptive_clustering/tree/gpu) is based on [PCL-GPU](https://pcl.readthedocs.io/projects/tutorials/en/master/#gpu) and has been tested with an NVIDIA TITAN Xp. + * [agx](https://github.com/yzrobot/adaptive_clustering/tree/agx) is based on [CUDA-PCL](https://github.com/NVIDIA-AI-IOT/cuda-pcl) and has been tested with an NVIDIA Jetson AGX Xavier. + +* **\[Feb 25, 2019\]:** A new branch, [devel](https://github.com/yzrobot/adaptive_clustering/tree/devel), faster (by downsampling) and better (by merging clusters split by nested regions and on the z-axis). + +## How to build ## +```sh +cd ~/catkin_ws/src/ +git clone https://github.com/yzrobot/adaptive_clustering.git +cd ~/catkin_ws +catkin_make +``` + +## Citation ## +If you are considering using this code, please reference the following: +``` +@article{yz19auro, + author = {Zhi Yan and Tom Duckett and Nicola Bellotto}, + title = {Online learning for 3D LiDAR-based human detection: Experimental analysis of point cloud clustering and classification methods}, + journal = {Autonomous Robots}, + year = {2019} +} +``` diff --git a/msg/ClusterArray.msg b/msg/ClusterArray.msg new file mode 100644 index 0000000..eea433b --- /dev/null +++ b/msg/ClusterArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +sensor_msgs/PointCloud2[] clusters diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..1d7dc8b --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + + adaptive_clustering + 0.0.2 + Light-weight real-time high-accuracy point cloud clustering. + Zhi Yan + Li Sun + Andres Hoyos + Haoguang Yang + BSD + + ament_cmake + ament_cmake_auto + + rosidl_default_generators + rclcpp + std_msgs + sensor_msgs + geometry_msgs + visualization_msgs + autoware_auto_perception_msgs + pcl_conversions + pcl_ros + blackandgold_msgs + tf2 + tf2_geometry_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/param/adaptive_clustering.yaml b/param/adaptive_clustering.yaml new file mode 100644 index 0000000..c6796c4 --- /dev/null +++ b/param/adaptive_clustering.yaml @@ -0,0 +1,16 @@ +/adaptive_clustering: + ros__parameters: + cluster_size_max: 500 + cluster_size_min: 5 + 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 \ No newline at end of file diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml new file mode 100644 index 0000000..f6e26ac --- /dev/null +++ b/param/better_clustering_config.yaml @@ -0,0 +1,44 @@ +/*/*/adaptive_clustering: + ros__parameters: + cluster_size_max: [1500, 500, 400, 200, 100] # Increase max cluster sizes for farther regions + cluster_size_min: [80, 45, 30, 20, 15] # Allow smaller clusters in farther regions + generate_bounding_boxes: true + k_merging_threshold: [0.65, 0.75, 0.75, 0.75, 0.75] # Increase merging threshold for farther regions + leaf: 1 + print_fps: false + radius_max: 50.0 + radius_min: 0.2 + sensor_model: VLP-16 + z_axis_max: 6.0 + z_axis_min: -1.0 + z_merging_threshold: 0.0 + car_width: 2.0 + car_length: 4.8768 + regions: [10, 10, 10, 10, 10] # Equal 10m-sized regions for consistent tuning + region_max: 5 + tolerance: [1.0, 1.2, 1.3, 1.4, 1.5] # Gradually relaxed clustering tolerance + + + + + + +#/*/*/adaptive_clustering: +# ros__parameters: +# 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 +# print_fps: false +# radius_max: 120.0 +# radius_min: 0.4000000059604645 +# sensor_model: VLP-16 +# z_axis_max: 2.0 +# z_axis_min: -0.800000011920929 +# z_merging_threshold: 0.0 # not used +# car_width: 2.0 +# car_length: 4.8768 +# regions: [10, 8, 12, 10, 10] +# region_max: 5 +# tolerance: 0.5 \ No newline at end of file diff --git a/param/better_clustering_config_B.yaml b/param/better_clustering_config_B.yaml new file mode 100644 index 0000000..7e9b2f2 --- /dev/null +++ b/param/better_clustering_config_B.yaml @@ -0,0 +1,27 @@ +car_length: 4.876800060272217 +car_width: 2.0 +cluster_size_max: 5000 +cluster_size_min: 200 +generate_bounding_boxes: true +k_merging_threshold: 0.10000000149011612 +leaf: 3 +print_fps: false +qos_overrides./parameter_events.publisher.depth: 1000 +qos_overrides./parameter_events.publisher.durability: volatile +qos_overrides./parameter_events.publisher.history: keep_last +qos_overrides./parameter_events.publisher.reliability: reliable +radius_max: 120.0 +radius_min: 0.3500000059604645 +regions: !!python/object/apply:array.array +- q +- - 5 + - 20 + - 30 + - 30 + - 30 +sensor_model: VLP-16 +tolerance: 2.0 +use_sim_time: false +z_axis_max: 10.0 +z_axis_min: -0.800000011920929 +z_merging_threshold: 0.0 diff --git a/src/adaptive_clustering.cpp b/src/adaptive_clustering.cpp new file mode 100644 index 0000000..d16afef --- /dev/null +++ b/src/adaptive_clustering.cpp @@ -0,0 +1,545 @@ + +// 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 + + +using namespace std::chrono_literals; + +//#define LOG +class AdaptiveClustering : public rclcpp::Node { + + public: + AdaptiveClustering(): Node("adaptive_clustering"){ + + + //private_nh.param("sensor_model", sensor_model, "VLP-16"); // VLP-16, HDL-32E, HDL-64E + this->declare_parameter("sensor_model", "VLP-16"); + //private_nh.param("print_fps", print_fps_, false); + this->declare_parameter("print_fps", false); + //private_nh.param("z_axis_min", z_axis_min_, -0.8); + 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 + std::vector default_tolerance = {0.5,0.5,0.75,0.80,0.85}; + std::vector default_k_merging_threshold = {0.10000000149011612,0.10000000149011612,0.10000000149011612,0.64,0.64}; + + //private_nh.param("cluster_size_min", s, 3); + 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", 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); + this->declare_parameter>("k_merging_threshold",default_k_merging_threshold); + //private_nh.param("z_merging_threshold", z_merging_threshold_, 0.0); + this->declare_parameter("z_merging_threshold", 0.0); + //private_nh.param("radius_min", radius_min_, 0.0); + this->declare_parameter("radius_min", 0.4); + //private_nh.param("radius_max", radius_max_, 30.0); + this->declare_parameter("radius_max", 120.0); + // Whether we want to output bounding boxes, or the original algorithm line markers + 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",default_tolerance); + this->declare_parameter("region_max", 5); // how many regions you want to detect. + + + + sensor_model = this->get_parameter("sensor_model").get_parameter_value().get(); + 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>(); + 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(); + radius_min_ = this->get_parameter("radius_min").get_parameter_value().get(); + radius_max_ = this->get_parameter("radius_max").get_parameter_value().get(); + 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>(); + 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, + 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 ***/ + //cluster_array_pub_ = private_nh.advertise("clusters", 100); + cluster_array_pub_ = this->create_publisher("clusters", 10); + //cloud_filtered_pub_ = private_nh.advertise("cloud_filtered", 100); + cloud_filtered_pub_ = this->create_publisher("cloud_filtered", 10); + //pose_array_pub_ = private_nh.advertise("poses", 100); + pose_array_pub_ = this->create_publisher("poses", 10); + //marker_array_pub_ = private_nh.advertise("markers", 100); + marker_array_pub_ = this->create_publisher("clustering_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); + + + reset = true;//fps + frames = 0; + start_time = clock(); + + } + + private: + + // 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; + } + + + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr ros_pc2_in) const + { + // 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(); + 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>(); + 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(); + 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 + + /*** Convert ROS message to PCL ***/ + pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); + pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in); + + + + pcl::IndicesPtr pc_indices(new std::vector); + for(unsigned int i = 0; i < pcl_pc_in->size(); ++i) { + pc_indices->push_back(i); + } + + + /*** Divide the point cloud into nested circular regions ***/ + #if PCL_VERSION_COMPARE(<, 1, 11, 0) + boost::array, 5> indices_array; + #else + std::array, 5> indices_array; + #endif + + for(unsigned int i = 0; i < pc_indices->size(); i++) { + float range = 0.0; + for(int j = 0; j < region_max_; j++) { + float d2 = pcl_pc_in->points[(*pc_indices)[i]].x * pcl_pc_in->points[(*pc_indices)[i]].x + + pcl_pc_in->points[(*pc_indices)[i]].y * pcl_pc_in->points[(*pc_indices)[i]].y + + pcl_pc_in->points[(*pc_indices)[i]].z * pcl_pc_in->points[(*pc_indices)[i]].z; + if(d2 > radius_min_ * radius_min_ && d2 < radius_max_ * radius_max_ && + d2 > range * range && d2 <= (range+regions_[j]) * (range+regions_[j])) { + indices_array[j].push_back((*pc_indices)[i]); + break; + } + range += regions_[j]; + } + } + + /*** Euclidean clustering ***/ + double tolerance = 0.5; + std::vector::Ptr, Eigen::aligned_allocator::Ptr > > clusters; + int last_clusters_begin = 0; + int last_clusters_end = 0; + + auto pre_time = rclcpp::Clock{}.now(); + //auto current_time = clock->now(); + + //RCLCPP_INFO(this->get_logger(), "Current time: %ld.%09ld", current_time.seconds(), current_time.nanoseconds()); + + for(int i = 0; i < region_max_; i++) { + tolerance = tolerance_[i]; + 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 + std::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); + #endif + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(pcl_pc_in, indices_array_ptr); + auto clustering_start = std::chrono::high_resolution_clock::now(); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + 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); + 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); + for(std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { + cluster->points.push_back(pcl_pc_in->points[*pit]); + } + /*** Merge clusters separated by nested regions ***/ + for(int j = last_clusters_begin; j < last_clusters_end; j++) { + pcl::KdTreeFLANN kdtree; + int K = 1; //the number of neighbors to search for + std::vector k_indices(K); + std::vector k_sqr_distances(K); + kdtree.setInputCloud(cluster); + if(clusters[j]->points.size() >= 1) { + if(kdtree.nearestKSearch(*clusters[j], clusters[j]->points.size()-1, K, k_indices, k_sqr_distances) > 0) { + if(k_sqr_distances[0] < k_merging_threshold_[i]) { + *cluster += *clusters[j]; + clusters.erase(clusters.begin()+j); + last_clusters_end--; + // std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; + } + } + } + } + /**************************************************/ + cluster->width = cluster->size(); + cluster->height = 1; + cluster->is_dense = true; + clusters.push_back(cluster); + } + /*** Merge z-axis clusters ***/ + for(int j = last_clusters_end; j < clusters.size(); j++) { + Eigen::Vector4f j_min, j_max; + pcl::getMinMax3D(*clusters[j], j_min, j_max); + for(int k = j+1; k < clusters.size(); k++) { + Eigen::Vector4f k_min, k_max; + pcl::getMinMax3D(*clusters[k], k_min, k_max); + if(std::max(std::min((double)j_max[0], (double)k_max[0]) - std::max((double)j_min[0], (double)k_min[0]), 0.0) * std::max(std::min((double)j_max[1], (double)k_max[1]) - std::max((double)j_min[1], (double)k_min[1]), 0.0) > z_merging_threshold_) { + *clusters[j] += *clusters[k]; + clusters.erase(clusters.begin()+k); + //std::cerr << "z-merging: clusters " << k << " is merged into " << j << std::endl; + } + } + } + /*****************************/ + last_clusters_begin = last_clusters_end; + last_clusters_end = clusters.size(); + } + } + + /*** Output ***/ + + //if(cloud_filtered_pub_->get_subscription_count() > 0) { + pcl::PointCloud::Ptr pcl_pc_out(new pcl::PointCloud); + sensor_msgs::msg::PointCloud2 ros_pc2_out; + //pcl::copyPointCloud(*pcl_pc_in, *pc_indices, *pcl_pc_out); + pcl::toROSMsg(*pcl_pc_in, ros_pc2_out); + + auto post_time = rclcpp::Clock{}.now(); + + auto time_elapsed = post_time.seconds() - pre_time.seconds(); + // RCLCPP_INFO(this->get_logger(), "Time taken: '%f'", time_elapsed); + + cloud_filtered_pub_->publish(ros_pc2_out); + //} + + + blackandgold_msgs::msg::ClusterArray cluster_array; + geometry_msgs::msg::PoseArray pose_array; + visualization_msgs::msg::MarkerArray marker_array; + autoware_auto_perception_msgs::msg::BoundingBoxArray bounding_boxes; + autoware_auto_perception_msgs::msg::BoundingBoxArray wall_bounding_boxes; + autoware_auto_perception_msgs::msg::BoundingBoxArray vehicle_bounding_boxes; + visualization_msgs::msg::MarkerArray bounding_boxes_markers; + visualization_msgs::msg::MarkerArray vehicle_markers; + + for(int i = 0; i < clusters.size(); i++) { + //if(cluster_array_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 ros_pc2_out; + pcl::toROSMsg(*clusters[i], ros_pc2_out); + cluster_array.clusters.push_back(ros_pc2_out); + //} + + //if(pose_array_pub_->get_subscription_count() > 0) { + + Eigen::Vector4f centroid; + pcl::compute3DCentroid(*clusters[i], centroid); + + // filter out the detection of ourselves + 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]; + + // 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]); + + // 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 >= 12.0) || box.size.x > 6.0 || (box.size.y / box.size.x > 1.5) || !valid) + { // If this is true, the box is bigger than the car + // 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.65; + 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); + + } + + if (bounding_boxes.boxes.size()) { + bounding_boxes.header = ros_pc2_in->header; + bounding_boxes_pub_->publish(bounding_boxes); + wall_bounding_boxes.header = ros_pc2_in->header; + marker_array_pub_->publish(bounding_boxes_markers); + wall_boxes_pub_->publish(wall_bounding_boxes); + } + + // Always publish vehicle bounding boxes, even if empty + vehicle_bounding_boxes.header = ros_pc2_in->header; + vehicle_boxes_pub_->publish(vehicle_bounding_boxes); + + // Always publish vehicle markers, even if empty + vehicle_marker_array_pub_->publish(vehicle_markers); + + + if(cluster_array.clusters.size()) { + cluster_array.header = ros_pc2_in->header; + 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(marker_array.markers.size()) { + marker_array_pub_->publish(marker_array); + } + + if(print_fps_)if(++frames>10){std::cerr<<"[adaptive_clustering] fps = "< cluster_size_min_; + mutable std::vector cluster_size_max_; + mutable int leaf_; + mutable std::vector k_merging_threshold_; + mutable float z_merging_threshold_; + mutable float radius_min_; + mutable float radius_max_; + mutable float car_width_; + mutable float car_length_; + mutable std::vector regions_; + mutable std::vector tolerance_; + 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; + mutable bool reset; + + visualization_msgs::msg::MarkerArray boundary_points; + + + bool generate_bounding_boxes; + mutable blackandgold_msgs::msg::Polynomial4Array polynomials; + rclcpp::Publisher::SharedPtr cloud_filtered_pub_; + rclcpp::Publisher::SharedPtr cluster_array_pub_; + rclcpp::Publisher::SharedPtr pose_array_pub_; + rclcpp::Publisher::SharedPtr marker_array_pub_; + rclcpp::Publisher::SharedPtr bounding_boxes_pub_; + rclcpp::Publisher::SharedPtr vehicle_boxes_pub_; + rclcpp::Publisher::SharedPtr wall_boxes_pub_; + rclcpp::Publisher::SharedPtr vehicle_marker_array_pub_; + + rclcpp::Subscription::SharedPtr point_cloud_sub; + //rclcpp::Subscription::SharedPtr wall_points_sub; + +}; + + +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 diff --git a/src/dev/adaptive_clustering2.cpp b/src/dev/adaptive_clustering2.cpp new file mode 100644 index 0000000..ded7922 --- /dev/null +++ b/src/dev/adaptive_clustering2.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 4df6ada072c3893b84380ef60bb412167cc58ce9 Mon Sep 17 00:00:00 2001 From: drake Date: Tue, 17 Dec 2024 20:01:43 -0500 Subject: [PATCH 3/5] tuned params for new point cloud --- param/better_clustering_config.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index f6e26ac..ef9e7b3 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,9 +1,9 @@ /*/*/adaptive_clustering: ros__parameters: - cluster_size_max: [1500, 500, 400, 200, 100] # Increase max cluster sizes for farther regions - cluster_size_min: [80, 45, 30, 20, 15] # Allow smaller clusters in farther regions + cluster_size_max: [6000, 5000, 3000, 2000, 100] # Increase max cluster sizes for farther regions + cluster_size_min: [300, 200, 55, 28, 28] # Allow smaller clusters in farther regions generate_bounding_boxes: true - k_merging_threshold: [0.65, 0.75, 0.75, 0.75, 0.75] # Increase merging threshold for farther regions + k_merging_threshold: [0.1, 0.75, 0.75, 0.75, 0.75] # Increase merging threshold for farther regions leaf: 1 print_fps: false radius_max: 50.0 @@ -16,7 +16,7 @@ car_length: 4.8768 regions: [10, 10, 10, 10, 10] # Equal 10m-sized regions for consistent tuning region_max: 5 - tolerance: [1.0, 1.2, 1.3, 1.4, 1.5] # Gradually relaxed clustering tolerance + tolerance: [0.5, 1.2, 1.3, 1.3, 1.3] # Gradually relaxed clustering tolerance From d3331b11ba21aa5e4da2095febd0c88295573eb1 Mon Sep 17 00:00:00 2001 From: drake Date: Wed, 18 Dec 2024 02:42:21 -0500 Subject: [PATCH 4/5] tweaks to account for new cloud transform --- param/better_clustering_config.yaml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index ef9e7b3..48503f0 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,5 +1,6 @@ /*/*/adaptive_clustering: ros__parameters: +<<<<<<< Updated upstream cluster_size_max: [6000, 5000, 3000, 2000, 100] # Increase max cluster sizes for farther regions cluster_size_min: [300, 200, 55, 28, 28] # Allow smaller clusters in farther regions generate_bounding_boxes: true @@ -11,12 +12,29 @@ sensor_model: VLP-16 z_axis_max: 6.0 z_axis_min: -1.0 +======= + cluster_size_max: [3000, 3000, 3000, 2000, 100] # Increase max cluster sizes for farther regions + cluster_size_min: [75, 30, 30, 15, 15] # Allow smaller clusters in farther regions + generate_bounding_boxes: true + k_merging_threshold: [0.8, 0.78, 0.75, 0.75, 0.75] # Increase merging threshold for farther regions + leaf: 1 + print_fps: false + radius_max: 50.0 + radius_min: 0.1 + sensor_model: VLP-16 + z_axis_max: 6.0 + z_axis_min: 0.2 +>>>>>>> Stashed changes z_merging_threshold: 0.0 car_width: 2.0 car_length: 4.8768 regions: [10, 10, 10, 10, 10] # Equal 10m-sized regions for consistent tuning region_max: 5 +<<<<<<< Updated upstream tolerance: [0.5, 1.2, 1.3, 1.3, 1.3] # Gradually relaxed clustering tolerance +======= + tolerance: [0.9, 1.1, 1.1, 1.3, 1.3] # Gradually relaxed clustering tolerance +>>>>>>> Stashed changes From 3ac5ee3f791147ba27208beab20028940331cdd4 Mon Sep 17 00:00:00 2001 From: drake Date: Wed, 18 Dec 2024 04:48:41 -0500 Subject: [PATCH 5/5] final pre track tuning changes --- CMakeLists.txt | 2 +- package.xml | 2 +- param/adaptive_clustering.yaml | 2 +- param/better_clustering_config.yaml | 18 - param/better_clustering_config_B.yaml | 27 - src/dev/adaptive_clustering2.cpp | 725 ++++++++++++++------------ 6 files changed, 408 insertions(+), 368 deletions(-) delete mode 100644 param/better_clustering_config_B.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 45ba110..506fd59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,4 +84,4 @@ if(AMENT_ENABLE_TESTING) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -endif() \ No newline at end of file +endif() diff --git a/package.xml b/package.xml index 1d7dc8b..c89f9a4 100644 --- a/package.xml +++ b/package.xml @@ -37,4 +37,4 @@ ament_cmake - \ No newline at end of file + diff --git a/param/adaptive_clustering.yaml b/param/adaptive_clustering.yaml index c6796c4..cd4c80e 100644 --- a/param/adaptive_clustering.yaml +++ b/param/adaptive_clustering.yaml @@ -13,4 +13,4 @@ z_axis_min: -0.800000011920929 z_merging_threshold: 0.0 car_width: 2.0 - car_length: 4.8768 \ No newline at end of file + car_length: 4.8768 diff --git a/param/better_clustering_config.yaml b/param/better_clustering_config.yaml index 48503f0..fadce18 100644 --- a/param/better_clustering_config.yaml +++ b/param/better_clustering_config.yaml @@ -1,18 +1,5 @@ /*/*/adaptive_clustering: ros__parameters: -<<<<<<< Updated upstream - cluster_size_max: [6000, 5000, 3000, 2000, 100] # Increase max cluster sizes for farther regions - cluster_size_min: [300, 200, 55, 28, 28] # Allow smaller clusters in farther regions - generate_bounding_boxes: true - k_merging_threshold: [0.1, 0.75, 0.75, 0.75, 0.75] # Increase merging threshold for farther regions - leaf: 1 - print_fps: false - radius_max: 50.0 - radius_min: 0.2 - sensor_model: VLP-16 - z_axis_max: 6.0 - z_axis_min: -1.0 -======= cluster_size_max: [3000, 3000, 3000, 2000, 100] # Increase max cluster sizes for farther regions cluster_size_min: [75, 30, 30, 15, 15] # Allow smaller clusters in farther regions generate_bounding_boxes: true @@ -24,17 +11,12 @@ sensor_model: VLP-16 z_axis_max: 6.0 z_axis_min: 0.2 ->>>>>>> Stashed changes z_merging_threshold: 0.0 car_width: 2.0 car_length: 4.8768 regions: [10, 10, 10, 10, 10] # Equal 10m-sized regions for consistent tuning region_max: 5 -<<<<<<< Updated upstream - tolerance: [0.5, 1.2, 1.3, 1.3, 1.3] # Gradually relaxed clustering tolerance -======= tolerance: [0.9, 1.1, 1.1, 1.3, 1.3] # Gradually relaxed clustering tolerance ->>>>>>> Stashed changes diff --git a/param/better_clustering_config_B.yaml b/param/better_clustering_config_B.yaml deleted file mode 100644 index 7e9b2f2..0000000 --- a/param/better_clustering_config_B.yaml +++ /dev/null @@ -1,27 +0,0 @@ -car_length: 4.876800060272217 -car_width: 2.0 -cluster_size_max: 5000 -cluster_size_min: 200 -generate_bounding_boxes: true -k_merging_threshold: 0.10000000149011612 -leaf: 3 -print_fps: false -qos_overrides./parameter_events.publisher.depth: 1000 -qos_overrides./parameter_events.publisher.durability: volatile -qos_overrides./parameter_events.publisher.history: keep_last -qos_overrides./parameter_events.publisher.reliability: reliable -radius_max: 120.0 -radius_min: 0.3500000059604645 -regions: !!python/object/apply:array.array -- q -- - 5 - - 20 - - 30 - - 30 - - 30 -sensor_model: VLP-16 -tolerance: 2.0 -use_sim_time: false -z_axis_max: 10.0 -z_axis_min: -0.800000011920929 -z_merging_threshold: 0.0 diff --git a/src/dev/adaptive_clustering2.cpp b/src/dev/adaptive_clustering2.cpp index ded7922..75d4983 100644 --- a/src/dev/adaptive_clustering2.cpp +++ b/src/dev/adaptive_clustering2.cpp @@ -1,5 +1,5 @@ - // Copyright (C) 2018 Zhi Yan and Li Sun +// Copyright (C) 2023 Andres Hoyos and Haoguang Yang // 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 @@ -14,355 +14,440 @@ // 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 +// ROS 2 +#include +#include +#include #include -#include +#include + +#include "adaptive_clustering/msg/cluster_array.hpp" // 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 +#include +#include +#include +#include +#include +#include + +// for portability -- not every project is shipped with autoware.auto +#if __has_include() +#include +#include -using namespace std::chrono_literals; +using autoware_auto_perception_msgs::msg::BoundingBox; +using autoware_auto_perception_msgs::msg::BoundingBoxArray; +#endif -//#define LOG -class AdaptiveClustering : public rclcpp::Node { +using namespace std::chrono_literals; +using adaptive_clustering::msg::ClusterArray; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseArray; +using sensor_msgs::msg::PointCloud2; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; - public: - AdaptiveClustering(): Node("adaptive_clustering"){ +// #define LOG +class AdaptiveClustering : public rclcpp::Node { + public: + AdaptiveClustering() : Node("adaptive_clustering") { + print_fps_ = this->declare_parameter("print_fps", false); + cluster_size_min_ = + static_cast(this->declare_parameter("cluster_size_min", 10)); + cluster_size_max_ = + static_cast(this->declare_parameter("cluster_size_max", 5000)); + // pre-processing options (ground removal) + do_ground_ceiling_rm_ = this->declare_parameter("perform_ground_ceiling_removal", true); + z_axis_min_ = this->declare_parameter("z_axis_min", -0.8); + z_axis_max_ = this->declare_parameter("z_axis_max", 10.0); + // Divide the point cloud into nested circular regions centred at the sensor for region-wise + // clustering For more details, see our IROS-17 paper "Online learning for human classification + // in 3D LiDAR-based tracking" + sensor_model_ = this->declare_parameter("sensor_model", ""); + if (sensor_model_.compare("VLP-16") == 0) { + regions_ = std::vector{2, 3, 3, 3, 3, 3, 3, 2, 3, 3, 3, 3, 3, 3}; + } else if (sensor_model_.compare("HDL-32E") == 0) { + regions_ = std::vector{4, 5, 4, 5, 4, 5, 5, 4, 5, 4, 5, 5, 5, 5}; + } else if (sensor_model_.compare("HDL-64E") == 0) { + regions_ = std::vector{14, 14, 14, 15, 14}; + } else { + regions_ = this->declare_parameter>( + "circular_region_width", std::vector{5, 20, 20, 30, 10}); + } + // post-processing options + leaf_ = this->declare_parameter("leaf", 3); + k_merging_threshold_ = this->declare_parameter("k_merging_threshold", 0.1); + z_merging_threshold_ = this->declare_parameter("z_merging_threshold", 0.0); + radius_min_ = this->declare_parameter("radius_min", 0.4); + radius_max_ = this->declare_parameter("radius_max", 120.0); + generate_bounding_boxes_ = this->declare_parameter("generate_bounding_boxes", true); + + /*** Subscribers ***/ + point_cloud_sub_ = this->create_subscription( + "/perception/points_nonground", rclcpp::SensorDataQoS(), + std::bind(&AdaptiveClustering::pointCloudCallback, this, std::placeholders::_1)); + + /*** Publishers ***/ + if (do_ground_ceiling_rm_) { + cloud_filtered_pub_ = + this->create_publisher("cloud_filtered", rclcpp::SystemDefaultsQoS()); + } + cluster_array_pub_ = + this->create_publisher("clusters", rclcpp::SystemDefaultsQoS()); + pose_array_pub_ = this->create_publisher("poses", rclcpp::SystemDefaultsQoS()); + marker_array_pub_ = + this->create_publisher("clustering_markers", rclcpp::SystemDefaultsQoS()); + +#ifdef AUTOWARE_AUTO_PERCEPTION_MSGS__MSG__BOUNDING_BOX_HPP_ + bounding_boxes_pub_ = this->create_publisher("/perception/lidar_clusters", + rclcpp::SystemDefaultsQoS()); + wall_boxes_pub_ = this->create_publisher("/perception/lidar_clusters_wall", + rclcpp::SystemDefaultsQoS()); + obstacle_boxes_pub_ = this->create_publisher( + "/perception/lidar_clusters_obstacle", rclcpp::SystemDefaultsQoS()); +#endif + obstacle_marker_array_pub_ = this->create_publisher( + "/perception/lidar_clusters_obstacle_marker", rclcpp::SystemDefaultsQoS()); + + // fps initialization + reset = true; + frames = 0; + start_time = this->now(); + } - // 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; + private: + void pointCloudCallback(PointCloud2::UniquePtr ros_pc2_in); + + bool print_fps_; + // If a pre-processed point cloud is used, we do't need the internal heuristics-based ground + // removal stage. + bool do_ground_ceiling_rm_; + std::string sensor_model_; + int leaf_; + float z_axis_min_; + float z_axis_max_; + unsigned int cluster_size_min_; + unsigned int cluster_size_max_; + + const int region_max_ = 10; // Change this value to match how far you want to detect. + std::vector regions_; + + // post-processing of clustering results + float k_merging_threshold_; + float z_merging_threshold_; + float radius_min_; + float radius_max_; + bool generate_bounding_boxes_; + + rclcpp::Publisher::SharedPtr cluster_array_pub_; + rclcpp::Publisher::SharedPtr cloud_filtered_pub_; + rclcpp::Publisher::SharedPtr pose_array_pub_; + rclcpp::Publisher::SharedPtr marker_array_pub_, obstacle_marker_array_pub_; + rclcpp::Subscription::SharedPtr point_cloud_sub_; + +#ifdef AUTOWARE_AUTO_PERCEPTION_MSGS__MSG__BOUNDING_BOX_HPP_ + rclcpp::Publisher::SharedPtr bounding_boxes_pub_, wall_boxes_pub_, + obstacle_boxes_pub_; +#endif + + // fps calculation + int frames; + rclcpp::Time start_time; + bool reset = true; +}; - point_cloud_sub = this->create_subscription( - "ransac_non_ground", 10, std::bind(&AdaptiveClustering::pointCloudCallback, this, std::placeholders::_1) - ); +void AdaptiveClustering::pointCloudCallback(PointCloud2::UniquePtr ros_pc2_in) { + if (print_fps_ && reset) { + frames = 0; + start_time = this->now(); + reset = false; + } // fps + + /*** Convert ROS message to PCL ***/ + pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); + pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in); + + /*** Remove ground and ceiling ***/ + pcl::IndicesPtr pc_indices(new std::vector); + if (do_ground_ceiling_rm_) { + for (size_t i = 0; i < pcl_pc_in->size(); ++i) { + if (i % leaf_) continue; + if (pcl_pc_in->points[i].z < z_axis_min_ || pcl_pc_in->points[i].z > z_axis_max_) continue; + pc_indices->push_back(i); + } + } else { + pc_indices->resize(pcl_pc_in->size()); + // sequentially increment from 1 to size of cloud + std::iota(pc_indices->begin(), pc_indices->end(), 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); - + /*** Divide the point cloud into nested circular regions ***/ + std::vector> indices_array{}; + indices_array.resize(region_max_); + for (size_t i = 0; i < pc_indices->size(); i++) { + float range = 0.0; + for (int j = 0; j < region_max_; j++) { + float d2 = pcl_pc_in->points[(*pc_indices)[i]].x * pcl_pc_in->points[(*pc_indices)[i]].x + + pcl_pc_in->points[(*pc_indices)[i]].y * pcl_pc_in->points[(*pc_indices)[i]].y + + pcl_pc_in->points[(*pc_indices)[i]].z * pcl_pc_in->points[(*pc_indices)[i]].z; + if (d2 > radius_min_ * radius_min_ && d2 < radius_max_ * radius_max_ && d2 > range * range && + d2 <= (range + regions_[j]) * (range + regions_[j])) { + indices_array[j].push_back((*pc_indices)[i]); + break; + } + range += regions_[j]; + } + } + RCLCPP_INFO(this->get_logger(), "PC indices: '%i'", pc_indices->size()); + + /*** Euclidean clustering ***/ + float tolerance = 0.0; + std::vector::Ptr, + Eigen::aligned_allocator::Ptr>> + clusters; + int last_clusters_begin = 0; + int last_clusters_end = 0; + + for (int i = 0; i < region_max_; i++) { + tolerance += 0.1; + if (indices_array[i].size() > cluster_size_min_) { + pcl::IndicesPtr indices_array_ptr(new std::vector(indices_array[i])); + 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); + + for (std::vector::const_iterator it = cluster_indices.begin(); + it != cluster_indices.end(); it++) { + pcl::PointCloud::Ptr cluster(new pcl::PointCloud); + for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); + ++pit) { + cluster->points.push_back(pcl_pc_in->points[*pit]); + } - //regions_[0] = 5; regions_[1] = 20; regions_[2] = 30; regions_[3] = 30; regions_[4] = 30; // FIXME: Add these to parameter files + /*** Merge clusters separated by nested regions ***/ + for (int j = last_clusters_begin; j < last_clusters_end; j++) { + pcl::KdTreeFLANN kdtree; + int K = 1; // the number of neighbors to search for + std::vector k_indices(K); + std::vector k_sqr_distances(K); + kdtree.setInputCloud(cluster); + if (clusters[j]->points.size() >= 1) { + if (kdtree.nearestKSearch(*clusters[j], clusters[j]->points.size() - 1, K, k_indices, + k_sqr_distances) > 0) { + if (k_sqr_distances[0] < k_merging_threshold_) { + *cluster += *clusters[j].get(); + clusters.erase(clusters.begin() + j); + last_clusters_end--; + // std::cerr << "k-merging: clusters " << j << " is merged" << std::endl; + } + } + } + } + /**************************************************/ - //reset = true;//fps - //frames = 0; - //start_time = clock(); + cluster->width = cluster->size(); + cluster->height = 1; + cluster->is_dense = true; + clusters.push_back(cluster); + } - } - - 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; + /*** Merge z-axis clusters ***/ + for (size_t j = last_clusters_end; j < clusters.size(); j++) { + Eigen::Vector4f j_min, j_max; + pcl::getMinMax3D(*clusters[j], j_min, j_max); + for (size_t k = j + 1; k < clusters.size(); k++) { + Eigen::Vector4f k_min, k_max; + pcl::getMinMax3D(*clusters[k], k_min, k_max); + if (std::max(std::min((double)j_max[0], (double)k_max[0]) - + std::max((double)j_min[0], (double)k_min[0]), + 0.0) * + std::max(std::min((double)j_max[1], (double)k_max[1]) - + std::max((double)j_min[1], (double)k_min[1]), + 0.0) > + z_merging_threshold_) { + *clusters[j] += *clusters[k]; + clusters.erase(clusters.begin() + k); + // std::cerr << "z-merging: clusters " << k << " is merged into " << j << std::endl; } - } - } - return true; + last_clusters_begin = last_clusters_end; + last_clusters_end = clusters.size(); + /*****************************/ } + } - - - - 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; + /* Post-process the clusters -- distinguish between obstacles and walls */ + std::vector isWall(clusters.size()); + std::vector> min_max_coords(clusters.size()); + std::vector> centroid_coords(clusters.size()); + std::vector> box_sizes(clusters.size()); + // TODO: use std::transform instead. + for (size_t i = 0; i < clusters.size(); i++) { + Eigen::Vector4f centroid; + pcl::compute3DCentroid(*clusters[i], centroid); + // FIXME: Add a method to compute the orientation of the bounding box + // FIXME: the quaternion (orientation of the box) should replace [0., 0., 0., 1.]. + centroid_coords[i] = {centroid[0], centroid[1], centroid[2], 0., 0., 0., 1.}; + + Eigen::Vector4f min_coords, max_coords; + pcl::getMinMax3D(*clusters[i], min_coords, max_coords); + min_max_coords[i] = {min_coords[0], min_coords[1], min_coords[2], + max_coords[0], max_coords[1], max_coords[2]}; + box_sizes[i] = {(max_coords[0] - min_coords[0]), (max_coords[1] - min_coords[1]), + (max_coords[2] - min_coords[2])}; +#ifdef LOG + std::cerr << ros_pc2_in->header.seq << " " << ros_pc2_in->header.stamp << " " << min_coords[0] + << " " << min_coords[1] << " " << min_coords[2] << " " << max_coords[0] << " " + << max_coords[1] << " " << max_coords[2] << " " << std::endl; +#endif + float max_box_dim = box_sizes[i][0]; + float min_box_dim = box_sizes[i][0]; + for (size_t j = 0; j < 3; j++) { + max_box_dim = std::fmax(box_sizes[i][j], max_box_dim); + min_box_dim = std::fmin(box_sizes[i][j], min_box_dim); } + isWall[i] = (box_sizes[i][0] * box_sizes[i][1] * box_sizes[i][2] >= 30.0) || + (max_box_dim > 10.0 * min_box_dim); + } - // 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; - } + /*** Output: use lazy publishers ***/ + if (do_ground_ceiling_rm_) { + if (cloud_filtered_pub_->get_subscription_count()) { + pcl::PointCloud::Ptr pcl_pc_out(new pcl::PointCloud); + std::unique_ptr ros_pc2_out(new PointCloud2); + pcl::copyPointCloud(*pcl_pc_in, *pc_indices, *pcl_pc_out); + pcl::toROSMsg(*pcl_pc_out, *ros_pc2_out.get()); + cloud_filtered_pub_->publish(std::move(ros_pc2_out)); + } + } - // 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; - } + if (cluster_array_pub_->get_subscription_count()) { + std::unique_ptr cluster_array(new ClusterArray); + for (size_t i = 0; i < clusters.size(); i++) { + PointCloud2 ros_pc2_out; + pcl::toROSMsg(*clusters[i], ros_pc2_out); + cluster_array->clusters.push_back(ros_pc2_out); } - return nullptr; -} + if (cluster_array->clusters.size()) { + cluster_array->header = ros_pc2_in->header; + cluster_array_pub_->publish(std::move(cluster_array)); + } + } - 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); - } + if (pose_array_pub_->get_subscription_count() > 0) { + std::unique_ptr pose_array(new PoseArray); + pose_array->poses.resize(clusters.size()); + std::transform(centroid_coords.begin(), centroid_coords.end(), pose_array->poses.begin(), + [](const std::vector &c) { + geometry_msgs::msg::Pose pose; + pose.position.x = c[0]; + pose.position.y = c[1]; + pose.position.z = c[2]; + pose.orientation.x = c[3]; + pose.orientation.y = c[4]; + pose.orientation.z = c[5]; + pose.orientation.w = c[6]; + return pose; + }); + pose_array->header = ros_pc2_in->header; + pose_array_pub_->publish(std::move(pose_array)); + } + std::unique_ptr marker_array = std::make_unique(); + marker_array->markers.reserve(clusters.size()); + std::unique_ptr obstacle_marker_array = std::make_unique(); + Marker marker; + marker.header = ros_pc2_in->header; + marker.ns = "adaptive_clustering"; + marker.type = Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.color.a = 0.3; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.5; + marker.lifetime = rclcpp::Duration(0.1s); + for (size_t i = 0; i < clusters.size(); i++) { + marker.id = static_cast(i); + marker.pose.position.x = (min_max_coords[i][0] + min_max_coords[i][3]) * 0.5; + marker.pose.position.y = (min_max_coords[i][1] + min_max_coords[i][4]) * 0.5; + marker.pose.position.z = (min_max_coords[i][2] + min_max_coords[i][5]) * 0.5; + marker.pose.orientation.x = centroid_coords[i][3]; + marker.pose.orientation.y = centroid_coords[i][4]; + marker.pose.orientation.z = centroid_coords[i][5]; + marker.pose.orientation.w = centroid_coords[i][6]; + marker.scale.x = box_sizes[i][0]; + marker.scale.y = box_sizes[i][1]; + marker.scale.z = box_sizes[i][2]; + marker_array->markers.push_back(marker); + if (!isWall[i]) { + obstacle_marker_array->markers.push_back(marker); + // marker color tweaks + obstacle_marker_array->markers.back().color.r = 1.0; + obstacle_marker_array->markers.back().color.g = 0.0; + obstacle_marker_array->markers.back().color.b = 0.0; + obstacle_marker_array->markers.back().color.a = 0.75; } + } - 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]; +#ifdef AUTOWARE_AUTO_PERCEPTION_MSGS__MSG__BOUNDING_BOX_HPP_ + std::unique_ptr bounding_boxes(new BoundingBoxArray); + std::unique_ptr wall_bounding_boxes(new BoundingBoxArray); + std::unique_ptr obstacle_bounding_boxes(new BoundingBoxArray); + bounding_boxes->header = ros_pc2_in->header; + wall_bounding_boxes->header = ros_pc2_in->header; + obstacle_bounding_boxes->header = ros_pc2_in->header; + autoware_auto_perception_msgs::msg::BoundingBox box; + for (size_t i = 0; i < clusters.size(); i++) { + box.centroid.x = marker_array->markers[i].pose.position.x; + box.centroid.y = marker_array->markers[i].pose.position.y; + box.centroid.z = marker_array->markers[i].pose.position.z; + box.orientation.x = marker_array->markers[i].pose.orientation.x; + box.orientation.y = marker_array->markers[i].pose.orientation.y; + box.orientation.z = marker_array->markers[i].pose.orientation.z; + box.orientation.w = marker_array->markers[i].pose.orientation.w; + box.size.x = marker_array->markers[i].scale.x; + box.size.y = marker_array->markers[i].scale.y; + box.size.z = marker_array->markers[i].scale.z; + bounding_boxes->boxes.push_back(box); + // figure out geometrically if it is a wall + if (isWall[i]) { + wall_bounding_boxes->boxes.push_back(box); + } else { + obstacle_bounding_boxes->boxes.push_back(box); } - - - - - - - - - - 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); - - - - - } -}; - - + bounding_boxes_pub_->publish(std::move(bounding_boxes)); + wall_boxes_pub_->publish(std::move(wall_bounding_boxes)); + obstacle_boxes_pub_->publish(std::move(obstacle_bounding_boxes)); +#endif + + marker_array_pub_->publish(std::move(marker_array)); + obstacle_marker_array_pub_->publish(std::move(obstacle_marker_array)); + + if (print_fps_) { + if (++frames > 10) { + std::cerr << "[adaptive_clustering] fps = " + << float(frames) / (this->now() - start_time).seconds() + << ", timestamp = " << this->now().seconds() << std::endl; + reset = true; + } // fps + } +} -int main(int argc, char * argv[]) { +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown();