From db909eaf9674c6daf9dfe8688c8c117667bbebf5 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Tue, 20 Feb 2024 20:06:10 +0000 Subject: [PATCH 01/13] initial commit --- .../perception/tracking/tracking.Dockerfile | 1 + modules/docker-compose.perception.yaml | 4 +- src/perception/tracking/CMakeLists.txt | 14 - .../tracking/dets_2d_3d/CMakeLists.txt | 64 ++++ .../tracking/dets_2d_3d/cmake/Eigen.cmake | 7 + .../tracking/dets_2d_3d/cmake/PCL.cmake | 8 + .../tracking/dets_2d_3d/include/cluster.hpp | 37 +++ .../tracking/dets_2d_3d/include/det_utils.hpp | 66 ++++ .../dets_2d_3d/include/dets_2d_3d_node.hpp | 62 ++++ .../dets_2d_3d/launch/dets_2d_3d.launch.py | 14 + .../tracking/{ => dets_2d_3d}/package.xml | 16 +- .../tracking/dets_2d_3d/src/cluster.cpp | 94 ++++++ .../tracking/dets_2d_3d/src/det_utils.cpp | 296 ++++++++++++++++++ .../dets_2d_3d/src/dets_2d_3d_node.cpp | 227 ++++++++++++++ 14 files changed, 893 insertions(+), 17 deletions(-) delete mode 100644 src/perception/tracking/CMakeLists.txt create mode 100644 src/perception/tracking/dets_2d_3d/CMakeLists.txt create mode 100644 src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake create mode 100644 src/perception/tracking/dets_2d_3d/cmake/PCL.cmake create mode 100644 src/perception/tracking/dets_2d_3d/include/cluster.hpp create mode 100644 src/perception/tracking/dets_2d_3d/include/det_utils.hpp create mode 100644 src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp create mode 100644 src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py rename src/perception/tracking/{ => dets_2d_3d}/package.xml (51%) create mode 100644 src/perception/tracking/dets_2d_3d/src/cluster.cpp create mode 100644 src/perception/tracking/dets_2d_3d/src/det_utils.cpp create mode 100644 src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp diff --git a/docker/perception/tracking/tracking.Dockerfile b/docker/perception/tracking/tracking.Dockerfile index 368c9ac6..42ce2be4 100644 --- a/docker/perception/tracking/tracking.Dockerfile +++ b/docker/perception/tracking/tracking.Dockerfile @@ -21,6 +21,7 @@ FROM ${BASE_IMAGE} as dependencies # Install Rosdep requirements COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list +RUN apt-get update RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) # Copy in source code from source stage diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index fd6ce9b3..c534a6e0 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -28,7 +28,7 @@ services: - driver: nvidia count: 1 capabilities: [ gpu ] - command: /bin/bash -c "ros2 launch camera_object_detection nuscenes_launch.py" + command: /bin/bash -c "ros2 launch camera_object_detection sim_launch.py" volumes: - /mnt/wato-drive2/perception_models/yolov8s.pt:/perception_models/yolov8s.pt @@ -95,4 +95,4 @@ services: - "${PERCEPTION_TRACKING_IMAGE}:main" target: deploy image: "${PERCEPTION_TRACKING_IMAGE}:${TAG}" - command: /bin/bash -c "ros2 launch tracking tracking.launch.py" + command: /bin/bash -c "ros2 launch dets_2d_3d dets_2d_3d.launch.py" diff --git a/src/perception/tracking/CMakeLists.txt b/src/perception/tracking/CMakeLists.txt deleted file mode 100644 index ffb74e71..00000000 --- a/src/perception/tracking/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tracking) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -ament_package() diff --git a/src/perception/tracking/dets_2d_3d/CMakeLists.txt b/src/perception/tracking/dets_2d_3d/CMakeLists.txt new file mode 100644 index 00000000..1af820e6 --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.10) +project(dets_2d_3d) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages +find_package(sensor_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +add_library(det_utils + src/det_utils.cpp src/cluster.cpp) # add more non-ros node files here + +target_include_directories(det_utils + PUBLIC include) + +ament_target_dependencies(det_utils + rclcpp + sample_msgs + sensor_msgs + vision_msgs + visualization_msgs + pcl_conversions + pcl_ros + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs) + +add_executable(dets_2d_3d_node src/dets_2d_3d_node.cpp) +target_include_directories(dets_2d_3d_node + PUBLIC include) +include(cmake/PCL.cmake) +# include(cmake/Eigen.cmake) +# include(cmake/OpenCV.cmake) + +ament_target_dependencies(dets_2d_3d_node) +target_link_libraries(dets_2d_3d_node det_utils) + +# Copy executable to installation location +install(TARGETS + dets_2d_3d_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch files to installation location +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake b/src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake new file mode 100644 index 00000000..8dcf251c --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake @@ -0,0 +1,7 @@ +find_package (Eigen3 3.3 REQUIRED NO_MODULE) + +include_directories(${Eigen_INCLUDE_DIRS}) +list(APPEND ALL_TARGET_LIBRARIES ${Eigen_LIBRARIES}) + +link_directories(${Eigen_LIBRARY_DIRS}) +add_definitions(${Eigen_DEFINITIONS}) diff --git a/src/perception/tracking/dets_2d_3d/cmake/PCL.cmake b/src/perception/tracking/dets_2d_3d/cmake/PCL.cmake new file mode 100644 index 00000000..330f3189 --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/cmake/PCL.cmake @@ -0,0 +1,8 @@ +find_package(PCL 1.10 REQUIRED) +list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") + +include_directories(${PCL_INCLUDE_DIRS}) +list(APPEND ALL_TARGET_LIBRARIES ${PCL_LIBRARIES}) + +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) diff --git a/src/perception/tracking/dets_2d_3d/include/cluster.hpp b/src/perception/tracking/dets_2d_3d/include/cluster.hpp new file mode 100644 index 00000000..ac80b627 --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/include/cluster.hpp @@ -0,0 +1,37 @@ +#include +#include + +#include + +#include + +class Cluster +{ +public: + // make cloud from only pts that have in "in_cluster_indices" + Cluster( + const pcl::PointCloud::Ptr& in_cloud_ptr, + const std::vector& in_cluster_indices); + + Cluster( + const pcl::PointCloud::Ptr& in_cloud_ptr, + const std::vector& in_cluster_indices); + + pcl::PointXYZ getCentroid() { return centroid_; } + pcl::PointCloud::Ptr getCloud() { return cloud_; } + vision_msgs::msg::BoundingBox3D getBoundingBox(); + + bool isValid(const vision_msgs::msg::BoundingBox3D& b); + + +private: + pcl::PointCloud::Ptr cloud_; + pcl::PointXYZ centroid_; + + // stores the min_x, min_y, min_z + pcl::PointXYZ max_point_; + pcl::PointXYZ min_point_; + + static int color_index; + static std::vector> colors; +}; diff --git a/src/perception/tracking/dets_2d_3d/include/det_utils.hpp b/src/perception/tracking/dets_2d_3d/include/det_utils.hpp new file mode 100644 index 00000000..0c0a5c3f --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/include/det_utils.hpp @@ -0,0 +1,66 @@ +#include "cluster.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +// main helper functions used by the node -- should all be static +class DetUtils +{ +public: + + static void pointsInBbox( + const pcl::PointCloud::Ptr& inlierCloud, + const pcl::PointCloud::Ptr& lidarCloud, + const std::vector& projs2d, + const vision_msgs::msg::BoundingBox2D& bbox); + + // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to camera frame + static geometry_msgs::msg::Point projectLidarToCamera( + const geometry_msgs::msg::TransformStamped& transform, + const std::array& P, + const pcl::PointXYZ& pt); + + static bool isPointInBbox( + const geometry_msgs::msg::Point& pt, + const vision_msgs::msg::BoundingBox2D& bbox); + + static void removeFloor( + const pcl::PointCloud::Ptr& lidarCloud, + const pcl::PointCloud::Ptr& cloud_filtered); + + static std::pair>, std::vector> getClusteredBBoxes( + const pcl::PointCloud::Ptr& lidarCloud); + +private: + + static std::vector> clusterAndColor( + const pcl::PointCloud::Ptr& in_cloud_ptr, + double in_max_cluster_distance); + + static void checkClusterMerge( + size_t in_cluster_id, const std::vector> &in_clusters, + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, double in_merge_threshold); + + static std::shared_ptr mergeClusters( + const std::vector>& in_clusters, + const std::vector& in_merge_indices, + std::vector& in_out_merged_clusters); + + static std::vector> checkAllForMerge( + const std::vector>& in_clusters, + float in_merge_threshold); + + static std::vector _clustering_distances; + static std::vector _clustering_thresholds; + + static double cluster_size_min_; + static double cluster_size_max_; + static double cluster_merge_threshold_; +}; diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp new file mode 100644 index 00000000..4924f988 --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +class TrackingNode : public rclcpp::Node +{ +public: + TrackingNode(); + +private: + + // synchronization utils + std::mutex lidarCloud_mutex_; + + // camera information cached + bool transformInited_; + geometry_msgs::msg::TransformStamped transform_; + sensor_msgs::msg::CameraInfo::SharedPtr camInfo_; + pcl::PointCloud::Ptr lidarCloud_; + + // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection boxes + rclcpp::Subscription::SharedPtr camInfo_subscriber_; + rclcpp::Subscription::SharedPtr lidar_subscriber_; + rclcpp::Subscription::SharedPtr det_subscriber_; + + // publish the 3d detections + rclcpp::Publisher::SharedPtr det3d_publisher_; + rclcpp::Publisher::SharedPtr marker_publisher_; + rclcpp::Publisher::SharedPtr pc_publisher_; + + // temp + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); + + vision_msgs::msg::BoundingBox3D highestIOUScoredBBox( + const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D& detBBox); + double overlapBoundingBox( + const vision_msgs::msg::BoundingBox2D& bboxA, + const vision_msgs::msg::BoundingBox2D& bboxB); + double iouScore( + const vision_msgs::msg::BoundingBox2D& bboxA, + const vision_msgs::msg::BoundingBox2D& bboxB); + +}; diff --git a/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py b/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py new file mode 100644 index 00000000..5b167eb7 --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch aggregator node.""" + tracking_node = Node( + package='dets_2d_3d', + executable='dets_2d_3d_node', + ) + + return LaunchDescription([ + tracking_node + ]) diff --git a/src/perception/tracking/package.xml b/src/perception/tracking/dets_2d_3d/package.xml similarity index 51% rename from src/perception/tracking/package.xml rename to src/perception/tracking/dets_2d_3d/package.xml index bff8967c..55527356 100644 --- a/src/perception/tracking/package.xml +++ b/src/perception/tracking/dets_2d_3d/package.xml @@ -1,7 +1,7 @@ - tracking + dets_2d_3d 0.0.0 TODO: Package description bolty @@ -9,6 +9,20 @@ ament_cmake + ament_lint_auto + ament_lint_common + + rclcpp + sample_msgs + sensor_msgs + vision_msgs + pcl_conversions + pcl_ros + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + visualization_msgs ament_cmake diff --git a/src/perception/tracking/dets_2d_3d/src/cluster.cpp b/src/perception/tracking/dets_2d_3d/src/cluster.cpp new file mode 100644 index 00000000..b4005cd5 --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/src/cluster.cpp @@ -0,0 +1,94 @@ +#include "cluster.hpp" + +#include +#include +#include + +// #include "opencv2/core/core.hpp" +// #include "opencv2/imgproc/imgproc.hpp" + + +typedef pcl::PointCloud ClusterCloud; +typedef vision_msgs::msg::BoundingBox3D BBox3D; + +int Cluster::color_index = 0; +std::vector> Cluster::colors = {{0,0,255}, {0,255,0}, {255,0,0}, {255,255,0}, {0,255,255}, {255,0,255}}; + +Cluster::Cluster( + const ClusterCloud::Ptr& in_cloud_ptr, + const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} +{ + bool indexesGiven = in_cluster_indices.size() > 0; + + int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + + for (int i=0; ipoints[idx].x; + pt.y = in_cloud_ptr->points[idx].y; + pt.z = in_cloud_ptr->points[idx].z; + pt.r = in_cloud_ptr->points[idx].r; + pt.g = in_cloud_ptr->points[idx].g; + pt.b = in_cloud_ptr->points[idx].b; + + cloud_->emplace_back(pt); + } + + ++color_index; +} + +Cluster::Cluster( + const pcl::PointCloud::Ptr& in_cloud_ptr, + const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} +{ + bool indexesGiven = in_cluster_indices.size() > 0; + + int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + + for (int i=0; ipoints[idx].x; + pt.y = in_cloud_ptr->points[idx].y; + pt.z = in_cloud_ptr->points[idx].z; + pt.r = colors[color_index % colors.size()][0]; + pt.g = colors[color_index % colors.size()][1]; + pt.b = colors[color_index % colors.size()][2]; + + + cloud_->emplace_back(pt); + } + + ++color_index; +} + +// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html +BBox3D Cluster::getBoundingBox() +{ + // AXIS ALIGNED BBOX + pcl::PointXYZRGB minPoint, maxPoint; + pcl::getMinMax3D(*cloud_, minPoint, maxPoint); + + + BBox3D bbox3d; + bbox3d.center.position.x = (maxPoint.x + minPoint.x)/2; + bbox3d.center.position.y = (maxPoint.y + minPoint.y)/2; + bbox3d.center.position.z = (maxPoint.z + minPoint.z)/2; + + bbox3d.size.x = maxPoint.x - minPoint.x; + bbox3d.size.y = maxPoint.y - minPoint.y; + bbox3d.size.z = maxPoint.z - minPoint.z; + + return bbox3d; +} + +bool Cluster::isValid(const BBox3D& b) +{ + return cloud_->size() > 0; + // return (b.size.x > 0 && b.size.y > 0 && b.size.z > 0); +} diff --git a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp new file mode 100644 index 00000000..badfc9ea --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp @@ -0,0 +1,296 @@ +#include "det_utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +typedef pcl::PointCloud Cloud; +typedef pcl::PointCloud ColorCloud; +typedef std::shared_ptr ClusterPtr; +typedef vision_msgs::msg::BoundingBox3D BBox3D; + +// segment by distance (5 sections) +// 0 => 0-15m d=0.5, 1 => 15-30 d=1, 2 => 30-45 d=1.6, 3 => 45-60 d=2.1, 4 => >60 d=2.6 +std::vector DetUtils::_clustering_distances = {15, 30, 45, 60}; + +//Nearest neighbor distance threshold for each segment +std::vector DetUtils::_clustering_thresholds = {0.5, 1.1, 1.6, 2.1, 2.6}; + +double DetUtils::cluster_size_min_ = 20; +double DetUtils::cluster_size_max_ = 100000; +double DetUtils::cluster_merge_threshold_ = 1.5; + +void DetUtils::pointsInBbox( + const Cloud::Ptr& inlierCloud, + const Cloud::Ptr& lidarCloud, + const std::vector& projs2d, + const vision_msgs::msg::BoundingBox2D& bbox) +{ + for (int i=0; ipush_back(lidarCloud->points[i]); + } + + } +} + +bool DetUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) +{ + if (bbox.center.position.x - bbox.size_x/2 < pt.x && pt.x > bbox.center.position.x + bbox.size_x/2 + && bbox.center.position.y - bbox.size_y/2 < pt.y && pt.y > bbox.center.position.y + bbox.size_y/2) + { + return true; + } + return false; +} + +geometry_msgs::msg::Point DetUtils::projectLidarToCamera( + const geometry_msgs::msg::TransformStamped& transform, + const std::array& p, + const pcl::PointXYZ& pt) +{ + // lidar to camera frame + auto trans_pt = geometry_msgs::msg::Point(); + auto orig_pt = geometry_msgs::msg::Point(); + orig_pt.x = pt.x; + orig_pt.y = pt.y; + orig_pt.z = pt.z; + + tf2::doTransform(orig_pt, trans_pt, transform); + + // camera frame to camera 2D projection + double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; + double v = p[4] * trans_pt.x + p[5] * trans_pt.y + p[6] * trans_pt.z + p[7]; + double w = p[8] * trans_pt.x + p[9] * trans_pt.y + p[10] * trans_pt.z + p[11]; + + auto proj_pt = geometry_msgs::msg::Point(); + proj_pt.x = u/w; + proj_pt.y = v/w; + proj_pt.z = w; // needed to check if front/behind camera + + return proj_pt; +} + +// https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html +void DetUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud_filtered) +{ + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + + // Create the filtering object + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(100); + seg.setAxis(Eigen::Vector3f(0, 0, 1)); + seg.setEpsAngle(0.5); + seg.setDistanceThreshold(0.4); // floor distance + seg.setOptimizeCoefficients(true); + seg.setInputCloud(lidarCloud); + seg.segment(*inliers, *coefficients); + + // Create the filtering object + pcl::ExtractIndices extract; + extract.setInputCloud(lidarCloud); + extract.setIndices(inliers); + + // Extract non-ground returns + extract.setNegative (true); + extract.filter(*cloud_filtered); +} + +// https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html +std::pair, std::vector> DetUtils::getClusteredBBoxes(const Cloud::Ptr& lidarCloud) +{ + std::vector::Ptr> cloud_segments_array(5); + for (int i=0; i::Ptr temp(new pcl::PointCloud()); + cloud_segments_array[i] = temp; + } + + // segment points into spherical shells from point cloud origin + for (const pcl::PointXYZ& pt : lidarCloud->points) + { + float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); + if (origin_distance < _clustering_distances[0]) + cloud_segments_array[0]->points.push_back(pt); + else if (origin_distance < _clustering_distances[1]) + cloud_segments_array[1]->points.push_back(pt); + else if (origin_distance < _clustering_distances[2]) + cloud_segments_array[2]->points.push_back(pt); + else if (origin_distance < _clustering_distances[3]) + cloud_segments_array[3]->points.push_back(pt); + else + cloud_segments_array[4]->points.push_back(pt); + } + + // get largest cluster in each shell + std::vector all_clusters; + for (unsigned int i = 0; i < cloud_segments_array.size(); i++) + { + // add clusters from each shell + std::vector local_clusters = clusterAndColor(cloud_segments_array[i], _clustering_thresholds[i]); + all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end()); + } + + // merge clusters if possible, do this twice? + std::vector mid_clusters = (all_clusters.size() > 0) + ? DetUtils::checkAllForMerge(all_clusters, cluster_merge_threshold_) + : all_clusters; + std::vector final_clusters = (mid_clusters.size() > 0) + ? DetUtils::checkAllForMerge(mid_clusters, cluster_merge_threshold_) + : mid_clusters; + + // std::vector final_clusters = all_clusters; + + // get boundingboxes for each & return all possible 3d bboxes (if valid) + std::vector bboxes; + for (const ClusterPtr& cluster : final_clusters) + { + BBox3D b = cluster->getBoundingBox(); + if (cluster->isValid(b)) + bboxes.emplace_back(b); + } + + + // TEMP - RETURN RGB CLOUD WITH ALL CLUSTERS (not just the same merged c) + // Cloud merged_cloud; + // for (const ClusterPtr& clusterPtr : final_clusters) + // { + // merged_cloud += *(clusterPtr->getCloud()); + // } + + return std::pair, std::vector>{final_clusters, bboxes}; +} + +std::vector DetUtils::clusterAndColor( + const Cloud::Ptr& in_cloud_ptr, double in_max_cluster_distance) +{ + std::vector clusters; + if(in_cloud_ptr->size() == 0) return clusters; + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); + + // create 2d pc, by copying & making it flat + Cloud::Ptr cloud_2d(new pcl::PointCloud()); + pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d); + for (size_t i = 0; i < cloud_2d->points.size(); i++) + cloud_2d->points[i].z = 0; + + tree->setInputCloud(cloud_2d); + + std::vector cluster_indices; + + // perform clustering on 2d cloud + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(in_max_cluster_distance); + ec.setMinClusterSize(cluster_size_min_); + ec.setMaxClusterSize(cluster_size_max_); + ec.setSearchMethod(tree); + ec.setInputCloud(cloud_2d); + ec.extract(cluster_indices); + + + // add pts at clustered indexes to cluster + for (const auto& cluster : cluster_indices) + { + ClusterPtr cloud_cluster = std::make_shared(in_cloud_ptr, cluster.indices); + clusters.emplace_back(cloud_cluster); + } + + return clusters; +} + + +void DetUtils::checkClusterMerge( + size_t in_cluster_id, const std::vector &in_clusters, + std::vector& in_out_visited_clusters, + std::vector& out_merge_indices, double in_merge_threshold) +{ + + pcl::PointXYZ point_a = in_clusters[in_cluster_id]->getCentroid(); + + for (size_t i = 0; i < in_clusters.size(); i++) { + if (i != in_cluster_id && !in_out_visited_clusters[i]) + { + pcl::PointXYZ point_b = in_clusters[i]->getCentroid(); + double distance = sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); + + if (distance <= in_merge_threshold) + { + in_out_visited_clusters[i] = true; + out_merge_indices.push_back(i); + // look for all other clusters that can be merged with this merge-able cluster + checkClusterMerge(i, in_clusters, in_out_visited_clusters, out_merge_indices, in_merge_threshold); + } + } + } +} + + +ClusterPtr DetUtils::mergeClusters( + const std::vector& in_clusters, + const std::vector& in_merge_indices, + std::vector &in_out_merged_clusters) +{ + ColorCloud merged_cloud; + + // for each cluster in merge cloud indices, merge into larger cloud + for (size_t i = 0; i < in_merge_indices.size(); i++) + { + merged_cloud += *(in_clusters[in_merge_indices[i]]->getCloud()); + in_out_merged_clusters[in_merge_indices[i]] = true; + } + + ColorCloud::Ptr merged_cloud_ptr(new ColorCloud(merged_cloud)); + + std::vector temp; + ClusterPtr merged_cluster = std::make_shared(merged_cloud_ptr, temp); + return merged_cluster; +} + +std::vector DetUtils::checkAllForMerge( + const std::vector& in_clusters, float in_merge_threshold) +{ + std::vector out_clusters; + + std::vector visited_clusters(in_clusters.size(), false); + std::vector merged_clusters(in_clusters.size(), false); + + for (size_t i = 0; i < in_clusters.size(); i++) { + if (!visited_clusters[i]) { + visited_clusters[i] = true; + + std::vector merge_indices; + checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold); + ClusterPtr mergedCluster = mergeClusters(in_clusters, merge_indices, merged_clusters); + + out_clusters.emplace_back(mergedCluster); + } + } + for (size_t i = 0; i < in_clusters.size(); i++) { + // check for clusters not merged, add them to the output + if (!merged_clusters[i]) { + out_clusters.push_back(in_clusters[i]); + } + } + + return out_clusters; +} diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp new file mode 100644 index 00000000..5de9b46e --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -0,0 +1,227 @@ +#include "dets_2d_3d_node.hpp" +#include "det_utils.hpp" + +#include +#include +#include +#include + +#include +#include + +#include +#include + +TrackingNode::TrackingNode() : Node("dets_2d_3d"), lidarCloud_{new pcl::PointCloud()}, transformInited_{false} +{ + camInfo_subscriber_ = this->create_subscription( + "/CAM_FRONT/camera_info", 10, + std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); + + lidar_subscriber_ = this->create_subscription( + "/LIDAR_TOP", 10, + std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); + + det_subscriber_ = this->create_subscription( + "/detections", 10, + std::bind(&TrackingNode::receiveDetections, this, std::placeholders::_1)); + + det3d_publisher_ = this->create_publisher("/detections_3d", 10); + marker_publisher_ = this->create_publisher("/markers_3d", 10); + pc_publisher_ = this->create_publisher("/trans_pc", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) +{ + // save latest lidar info + std::lock_guard guard_lidar(lidarCloud_mutex_); + + sensor_msgs::msg::PointCloud2 pointCloud = *pointCloud_msg; + pcl::fromROSMsg(pointCloud, *lidarCloud_); +} + +void TrackingNode::readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) +{ + std::cout << msg->distortion_model << std::endl; + camInfo_ = msg; + camInfo_subscriber_.reset(); +} + +void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg) +{ + if (!transformInited_) + { + try { + transform_ = tf_buffer_ ->lookupTransform("LIDAR_TOP", "CAM_FRONT", tf2::TimePointZero); + transformInited_ = true; + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); + return; + } + } + + // temporarily also publish viz markers + visualization_msgs::msg::MarkerArray markerArray3d; + vision_msgs::msg::Detection3DArray detArray3d; + + // remove floor from lidar cloud + pcl::PointCloud::Ptr lidarCloudNoFloor(new pcl::PointCloud()); + { + std::lock_guard guard_lidar(lidarCloud_mutex_); + DetUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); + + if (lidarCloudNoFloor->size() == 0) return; // end if there is no floor + RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", lidarCloud_->size(), lidarCloudNoFloor->size()); + } + + // transform all lidar pts to 2d + std::vector lidar2dProjs; + for (const pcl::PointXYZ& pt : lidarCloudNoFloor->points) + { + geometry_msgs::msg::Point proj = DetUtils::projectLidarToCamera(transform_, camInfo_->p, pt); + if (proj.z >= 0) + { + proj.x /= proj.z; + proj.y /= proj.z; + lidar2dProjs.emplace_back(proj); + } + } + + pcl::PointCloud mergedClusters; + + // process each detection in det array + for (const vision_msgs::msg::Detection2D& det : msg->detections) + { + vision_msgs::msg::BoundingBox2D bbox = det.bbox; + + // process lidarcloud with extrinsic trans from lidar to cam frame + pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); + DetUtils::pointsInBbox(inlierPoints, lidarCloudNoFloor, lidar2dProjs, bbox); + if (inlierPoints->size() == 0) return; + + RCLCPP_INFO(this->get_logger(), "restrict to 2d %ld vs %ld", inlierPoints->size(), lidarCloudNoFloor->size()); + + // clustering + auto clusterAndBBoxes = DetUtils::getClusteredBBoxes(inlierPoints); + + RCLCPP_INFO(this->get_logger(), "get merged cloud %ld clustered bboxes %ld", clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); + + std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only + + for (const std::shared_ptr& cl : clusters) + { + RCLCPP_INFO(this->get_logger(), "got cluster %ld", cl->getCloud()->size()); + mergedClusters += *(cl->getCloud()); + } + + std::vector allBBoxes = clusterAndBBoxes.second; + + // [TODO] score bboxes by projecting & iou + vision_msgs::msg::BoundingBox3D bestBBox = highestIOUScoredBBox(allBBoxes, bbox); + + RCLCPP_INFO(this->get_logger(), "score bboxes"); + + // find 3d box that encloses clustered pointcloud & add to det array + vision_msgs::msg::Detection3D det3d; + det3d.bbox = bestBBox; + det3d.results = det.results; + detArray3d.detections.emplace_back(det3d); + + // make marker & add to array (TEMP) + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale = bestBBox.size; + marker.pose = bestBBox.center; + + marker.header.frame_id = "/LIDAR_TOP"; + markerArray3d.markers.emplace_back(marker); + + break; + } + + det3d_publisher_->publish(detArray3d); + marker_publisher_->publish(markerArray3d); + + sensor_msgs::msg::PointCloud2 pubCloud; + pcl::toROSMsg(mergedClusters, pubCloud); + pubCloud.header.frame_id = "LIDAR_TOP"; + pc_publisher_->publish(pubCloud); + + RCLCPP_INFO(this->get_logger(), "published 3d detection"); +} + +vision_msgs::msg::BoundingBox3D TrackingNode::highestIOUScoredBBox( + const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D& detBBox) +{ + int bestScore = 0; + int bestBBox = 0; + + for (int i=0; ip, top_left); + geometry_msgs::msg::Point bottom_right2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); + + vision_msgs::msg::BoundingBox2D bbox2d; + bbox2d.center.position.x = ((top_left2d.x/top_left2d.z) + (bottom_right2d.x/bottom_right2d.z))/2; + bbox2d.center.position.y = ((top_left2d.y/top_left2d.z) + (bottom_right2d.y/bottom_right2d.z))/2; + bbox2d.size_x = abs(top_left2d.x - bottom_right2d.x); + bbox2d.size_y = abs(top_left2d.y - bottom_right2d.y); + + double iou = iouScore(bbox2d, detBBox); + RCLCPP_INFO(this->get_logger(), "pos: %f, %f, %f, size: %f, %f, %f : iou %f", + b.center.position.x, b.center.position.y, b.center.position.z, + b.size.x, b.size.y, b.size.z, iou); + + if (iou > bestScore) + { + bestScore = iou; + bestBBox = i; + } + } + return bboxes[bestBBox]; +} + +double TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D& boxA, const vision_msgs::msg::BoundingBox2D& boxB) +{ + double overlapHeight = + std::min(boxA.center.position.y + boxA.size_y/2, boxB.center.position.y + boxB.size_y) - + std::max(boxA.center.position.y, boxB.center.position.y); + if (overlapHeight <= 0) return 0; + double overlapWidth = std::min(boxA.center.position.x + boxA.size_x, boxB.center.position.x + boxB.size_x) - + std::max(boxA.center.position.x, boxB.center.position.x); + if (overlapWidth <= 0) return 0; + return overlapHeight * overlapWidth; +} + +double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB) +{ + double overlap = overlapBoundingBox(bboxA, bboxB); + // If overlap is 0, it returns 0 + return (overlap / + (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - overlap)); + +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From f8b772b7afd003c00016dbe124feb8ddb45f6aa4 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Wed, 21 Feb 2024 19:38:16 +0000 Subject: [PATCH 02/13] add bounding boxes for clusters --- .../perception/tracking/tracking.Dockerfile | 3 + .../tracking/dets_2d_3d/CMakeLists.txt | 7 +- .../tracking/dets_2d_3d/include/cluster.hpp | 6 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 3 +- .../tracking/dets_2d_3d/src/cluster.cpp | 118 ++++++++++++------ .../tracking/dets_2d_3d/src/det_utils.cpp | 9 +- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 45 +++---- 7 files changed, 126 insertions(+), 65 deletions(-) diff --git a/docker/perception/tracking/tracking.Dockerfile b/docker/perception/tracking/tracking.Dockerfile index 42ce2be4..c9a3669e 100644 --- a/docker/perception/tracking/tracking.Dockerfile +++ b/docker/perception/tracking/tracking.Dockerfile @@ -24,6 +24,9 @@ COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list RUN apt-get update RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) +# install opencv +RUN apt-get install -y libopencv-dev + # Copy in source code from source stage WORKDIR ${AMENT_WS} COPY --from=source ${AMENT_WS}/src src diff --git a/src/perception/tracking/dets_2d_3d/CMakeLists.txt b/src/perception/tracking/dets_2d_3d/CMakeLists.txt index 1af820e6..2a415e2e 100644 --- a/src/perception/tracking/dets_2d_3d/CMakeLists.txt +++ b/src/perception/tracking/dets_2d_3d/CMakeLists.txt @@ -45,8 +45,11 @@ add_executable(dets_2d_3d_node src/dets_2d_3d_node.cpp) target_include_directories(dets_2d_3d_node PUBLIC include) include(cmake/PCL.cmake) -# include(cmake/Eigen.cmake) -# include(cmake/OpenCV.cmake) + +cmake_minimum_required(VERSION 2.8) +find_package( OpenCV REQUIRED ) +include_directories( ${OpenCV_INCLUDE_DIRS} ) +target_link_libraries (dets_2d_3d_node ${OpenCV_LIBS}) ament_target_dependencies(dets_2d_3d_node) target_link_libraries(dets_2d_3d_node det_utils) diff --git a/src/perception/tracking/dets_2d_3d/include/cluster.hpp b/src/perception/tracking/dets_2d_3d/include/cluster.hpp index ac80b627..e8627e24 100644 --- a/src/perception/tracking/dets_2d_3d/include/cluster.hpp +++ b/src/perception/tracking/dets_2d_3d/include/cluster.hpp @@ -13,9 +13,9 @@ class Cluster const pcl::PointCloud::Ptr& in_cloud_ptr, const std::vector& in_cluster_indices); - Cluster( - const pcl::PointCloud::Ptr& in_cloud_ptr, - const std::vector& in_cluster_indices); + // Cluster( + // const pcl::PointCloud::Ptr& in_cloud_ptr, + // const std::vector& in_cluster_indices); pcl::PointXYZ getCentroid() { return centroid_; } pcl::PointCloud::Ptr getCloud() { return cloud_; } diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index 4924f988..492791e2 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -40,6 +40,7 @@ class TrackingNode : public rclcpp::Node rclcpp::Publisher::SharedPtr det3d_publisher_; rclcpp::Publisher::SharedPtr marker_publisher_; rclcpp::Publisher::SharedPtr pc_publisher_; + rclcpp::Publisher::SharedPtr pc_publisher2_; // temp std::unique_ptr tf_buffer_; @@ -49,7 +50,7 @@ class TrackingNode : public rclcpp::Node void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); void receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); - vision_msgs::msg::BoundingBox3D highestIOUScoredBBox( + int highestIOUScoredBBox( const std::vector bboxes, const vision_msgs::msg::BoundingBox2D& detBBox); double overlapBoundingBox( diff --git a/src/perception/tracking/dets_2d_3d/src/cluster.cpp b/src/perception/tracking/dets_2d_3d/src/cluster.cpp index b4005cd5..bfd3444c 100644 --- a/src/perception/tracking/dets_2d_3d/src/cluster.cpp +++ b/src/perception/tracking/dets_2d_3d/src/cluster.cpp @@ -3,9 +3,16 @@ #include #include #include +#include +#include +#include -// #include "opencv2/core/core.hpp" -// #include "opencv2/imgproc/imgproc.hpp" +#include +#include +#include +#include + +#include typedef pcl::PointCloud ClusterCloud; @@ -14,31 +21,25 @@ typedef vision_msgs::msg::BoundingBox3D BBox3D; int Cluster::color_index = 0; std::vector> Cluster::colors = {{0,0,255}, {0,255,0}, {255,0,0}, {255,255,0}, {0,255,255}, {255,0,255}}; -Cluster::Cluster( - const ClusterCloud::Ptr& in_cloud_ptr, - const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} -{ - bool indexesGiven = in_cluster_indices.size() > 0; +// Cluster::Cluster( +// const ClusterCloud::Ptr& in_cloud_ptr, +// const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} +// { +// bool indexesGiven = in_cluster_indices.size() > 0; - int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); +// int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); - for (int i=0; ipoints[idx].x; - pt.y = in_cloud_ptr->points[idx].y; - pt.z = in_cloud_ptr->points[idx].z; - pt.r = in_cloud_ptr->points[idx].r; - pt.g = in_cloud_ptr->points[idx].g; - pt.b = in_cloud_ptr->points[idx].b; +// pcl::PointXYZRGB pt; +// pt = in_cloud_ptr->points[idx]; +// cloud_->emplace_back(pt); +// } - cloud_->emplace_back(pt); - } - - ++color_index; -} +// ++color_index; +// } Cluster::Cluster( const pcl::PointCloud::Ptr& in_cloud_ptr, @@ -48,6 +49,14 @@ Cluster::Cluster( int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + min_point_.x = std::numeric_limits::max(); + min_point_.y = std::numeric_limits::max(); + min_point_.z = std::numeric_limits::max(); + + max_point_.x = -std::numeric_limits::max(); + max_point_.y = -std::numeric_limits::max(); + max_point_.z = -std::numeric_limits::max(); + for (int i=0; i max_point_.x) max_point_.x = pt.x; + if (pt.y > max_point_.y) max_point_.y = pt.y; + if (pt.z > max_point_.z) max_point_.z = pt.z; + cloud_->emplace_back(pt); } @@ -67,24 +84,55 @@ Cluster::Cluster( ++color_index; } -// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html BBox3D Cluster::getBoundingBox() { - // AXIS ALIGNED BBOX - pcl::PointXYZRGB minPoint, maxPoint; - pcl::getMinMax3D(*cloud_, minPoint, maxPoint); + BBox3D bounding_box_; + + if (cloud_->size() == 0) return bounding_box_; + + double length_ = max_point_.x - min_point_.x; + double width_ = max_point_.y - min_point_.y; + double height_ = max_point_.z - min_point_.z; + + bounding_box_.center.position.x = min_point_.x + length_ / 2; + bounding_box_.center.position.y = min_point_.y + width_ / 2; + bounding_box_.center.position.z = min_point_.z + height_ / 2; + + bounding_box_.size.x = ((length_ < 0) ? -1 * length_ : length_); + bounding_box_.size.y = ((width_ < 0) ? -1 * width_ : width_); + bounding_box_.size.z = ((height_ < 0) ? -1 * height_ : height_); + + // pose estimation + double rz = 0; + + { + std::vector points; + for (unsigned int i = 0; i < cloud_->points.size(); i++) + { + cv::Point2f pt; + pt.x = cloud_->points[i].x; + pt.y = cloud_->points[i].y; + points.push_back(pt); + } + + cv::RotatedRect box = cv::minAreaRect(points); + rz = box.angle * 3.14 / 180; + bounding_box_.center.position.x = box.center.x; + bounding_box_.center.position.y = box.center.y; + bounding_box_.size.x = box.size.width; + bounding_box_.size.y = box.size.height; + } + // set bounding box direction + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, rz); - BBox3D bbox3d; - bbox3d.center.position.x = (maxPoint.x + minPoint.x)/2; - bbox3d.center.position.y = (maxPoint.y + minPoint.y)/2; - bbox3d.center.position.z = (maxPoint.z + minPoint.z)/2; + geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(quat); + bounding_box_.center.orientation = msg_quat; - bbox3d.size.x = maxPoint.x - minPoint.x; - bbox3d.size.y = maxPoint.y - minPoint.y; - bbox3d.size.z = maxPoint.z - minPoint.z; + std::cout << cv::getBuildInformation() << std::endl; - return bbox3d; + return bounding_box_; } bool Cluster::isValid(const BBox3D& b) diff --git a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp index badfc9ea..f5925168 100644 --- a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp @@ -99,8 +99,8 @@ void DetUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100); seg.setAxis(Eigen::Vector3f(0, 0, 1)); - seg.setEpsAngle(0.5); - seg.setDistanceThreshold(0.4); // floor distance + seg.setEpsAngle(0.1); + seg.setDistanceThreshold(0.2); // floor distance seg.setOptimizeCoefficients(true); seg.setInputCloud(lidarCloud); seg.segment(*inliers, *coefficients); @@ -259,7 +259,10 @@ ClusterPtr DetUtils::mergeClusters( in_out_merged_clusters[in_merge_indices[i]] = true; } - ColorCloud::Ptr merged_cloud_ptr(new ColorCloud(merged_cloud)); + Cloud merged_cloud_uncoloured; + pcl::copyPointCloud(merged_cloud, merged_cloud_uncoloured); + + Cloud::Ptr merged_cloud_ptr(new Cloud(merged_cloud_uncoloured)); std::vector temp; ClusterPtr merged_cluster = std::make_shared(merged_cloud_ptr, temp); diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index 5de9b46e..2584ae0e 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -29,6 +29,7 @@ TrackingNode::TrackingNode() : Node("dets_2d_3d"), lidarCloud_{new pcl::PointClo det3d_publisher_ = this->create_publisher("/detections_3d", 10); marker_publisher_ = this->create_publisher("/markers_3d", 10); pc_publisher_ = this->create_publisher("/trans_pc", 10); + pc_publisher2_ = this->create_publisher("/trans_pc2", 10); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -90,6 +91,11 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S } } + sensor_msgs::msg::PointCloud2 pubCloud; + pcl::toROSMsg(*lidarCloudNoFloor, pubCloud); + pubCloud.header.frame_id = "LIDAR_TOP"; + pc_publisher_->publish(pubCloud); + pcl::PointCloud mergedClusters; // process each detection in det array @@ -100,7 +106,9 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // process lidarcloud with extrinsic trans from lidar to cam frame pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); DetUtils::pointsInBbox(inlierPoints, lidarCloudNoFloor, lidar2dProjs, bbox); - if (inlierPoints->size() == 0) return; + if (inlierPoints->size() == 0) continue; + + // mergedClusters += *inlierPoints; RCLCPP_INFO(this->get_logger(), "restrict to 2d %ld vs %ld", inlierPoints->size(), lidarCloudNoFloor->size()); @@ -110,19 +118,16 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S RCLCPP_INFO(this->get_logger(), "get merged cloud %ld clustered bboxes %ld", clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only - - for (const std::shared_ptr& cl : clusters) - { - RCLCPP_INFO(this->get_logger(), "got cluster %ld", cl->getCloud()->size()); - mergedClusters += *(cl->getCloud()); - } - std::vector allBBoxes = clusterAndBBoxes.second; - // [TODO] score bboxes by projecting & iou - vision_msgs::msg::BoundingBox3D bestBBox = highestIOUScoredBBox(allBBoxes, bbox); + if (clusters.size() == 0 || allBBoxes.size() == 0) continue; - RCLCPP_INFO(this->get_logger(), "score bboxes"); + // [TODO] fix scoring bboxes by projecting & iou + int bestIndex = highestIOUScoredBBox(allBBoxes, bbox); + vision_msgs::msg::BoundingBox3D bestBBox = allBBoxes[bestIndex]; + mergedClusters += *(clusters[bestIndex]->getCloud()); + + RCLCPP_INFO(this->get_logger(), "scored bboxes"); // find 3d box that encloses clustered pointcloud & add to det array vision_msgs::msg::Detection3D det3d; @@ -136,24 +141,22 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S marker.scale = bestBBox.size; marker.pose = bestBBox.center; - marker.header.frame_id = "/LIDAR_TOP"; + marker.header = pubCloud.header; markerArray3d.markers.emplace_back(marker); - - break; } det3d_publisher_->publish(detArray3d); marker_publisher_->publish(markerArray3d); - sensor_msgs::msg::PointCloud2 pubCloud; - pcl::toROSMsg(mergedClusters, pubCloud); - pubCloud.header.frame_id = "LIDAR_TOP"; - pc_publisher_->publish(pubCloud); + sensor_msgs::msg::PointCloud2 pubCloud2; + pcl::toROSMsg(mergedClusters, pubCloud2); + pubCloud2.header = pubCloud.header; + pc_publisher2_->publish(pubCloud2); - RCLCPP_INFO(this->get_logger(), "published 3d detection"); + RCLCPP_INFO(this->get_logger(), "published 3d detection %ld", markerArray3d.markers.size()); } -vision_msgs::msg::BoundingBox3D TrackingNode::highestIOUScoredBBox( +int TrackingNode::highestIOUScoredBBox( const std::vector bboxes, const vision_msgs::msg::BoundingBox2D& detBBox) { @@ -194,7 +197,7 @@ vision_msgs::msg::BoundingBox3D TrackingNode::highestIOUScoredBBox( bestBBox = i; } } - return bboxes[bestBBox]; + return bestBBox; } double TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D& boxA, const vision_msgs::msg::BoundingBox2D& boxB) From 3066df05415610e990fb39fc7f6c1500d3b8fe64 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Thu, 22 Feb 2024 19:12:05 +0000 Subject: [PATCH 03/13] edit clustering parameters --- .../tracking/dets_2d_3d/src/det_utils.cpp | 28 ++++++++++++++----- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 26 ++++++++--------- 2 files changed, 33 insertions(+), 21 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp index f5925168..b6747bfb 100644 --- a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp @@ -22,7 +22,7 @@ typedef vision_msgs::msg::BoundingBox3D BBox3D; // segment by distance (5 sections) // 0 => 0-15m d=0.5, 1 => 15-30 d=1, 2 => 30-45 d=1.6, 3 => 45-60 d=2.1, 4 => >60 d=2.6 -std::vector DetUtils::_clustering_distances = {15, 30, 45, 60}; +std::vector DetUtils::_clustering_distances = {5, 30, 45, 60}; //Nearest neighbor distance threshold for each segment std::vector DetUtils::_clustering_thresholds = {0.5, 1.1, 1.6, 2.1, 2.6}; @@ -51,14 +51,28 @@ void DetUtils::pointsInBbox( bool DetUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) { - if (bbox.center.position.x - bbox.size_x/2 < pt.x && pt.x > bbox.center.position.x + bbox.size_x/2 - && bbox.center.position.y - bbox.size_y/2 < pt.y && pt.y > bbox.center.position.y + bbox.size_y/2) - { + double padding = 10; + + // if (bbox.center.position.x - bbox.size_x/2 - padding < pt.x && pt.x < bbox.center.position.x + bbox.size_x/2 + padding + // && bbox.center.position.y - bbox.size_y/2 - padding < pt.y && pt.y < bbox.center.position.y + bbox.size_y/2 + padding) + // { + // return true; + // } + + if (pt.x > 0 && pt.x < 1600 && pt.y > 0 && pt.y < 900) return true; - } return false; } +/* + +1266.417203046554 0.0 816.2670197447984 0.0 +0.0 1266.417203046554 491.50706579294757 0.0 +0.0 0.0 1.0 0.0 + + +*/ + geometry_msgs::msg::Point DetUtils::projectLidarToCamera( const geometry_msgs::msg::TransformStamped& transform, const std::array& p, @@ -100,7 +114,7 @@ void DetUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud seg.setMaxIterations(100); seg.setAxis(Eigen::Vector3f(0, 0, 1)); seg.setEpsAngle(0.1); - seg.setDistanceThreshold(0.2); // floor distance + seg.setDistanceThreshold(0.5); // floor distance seg.setOptimizeCoefficients(true); seg.setInputCloud(lidarCloud); seg.segment(*inliers, *coefficients); @@ -143,7 +157,7 @@ std::pair, std::vector> DetUtils::getClusteredBB // get largest cluster in each shell std::vector all_clusters; - for (unsigned int i = 0; i < cloud_segments_array.size(); i++) + for (unsigned int i = 1; i < cloud_segments_array.size(); i++) { // add clusters from each shell std::vector local_clusters = clusterAndColor(cloud_segments_array[i], _clustering_thresholds[i]); diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index 2584ae0e..483c2dcd 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -56,7 +56,7 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S if (!transformInited_) { try { - transform_ = tf_buffer_ ->lookupTransform("LIDAR_TOP", "CAM_FRONT", tf2::TimePointZero); + transform_ = tf_buffer_ ->lookupTransform("CAM_FRONT", "LIDAR_TOP", msg->header.stamp); transformInited_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); @@ -64,17 +64,13 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S } } - // temporarily also publish viz markers - visualization_msgs::msg::MarkerArray markerArray3d; - vision_msgs::msg::Detection3DArray detArray3d; - // remove floor from lidar cloud pcl::PointCloud::Ptr lidarCloudNoFloor(new pcl::PointCloud()); { std::lock_guard guard_lidar(lidarCloud_mutex_); DetUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); - if (lidarCloudNoFloor->size() == 0) return; // end if there is no floor + if (lidarCloudNoFloor->size() == 0) return; // end if there is only a floor RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", lidarCloud_->size(), lidarCloudNoFloor->size()); } @@ -85,18 +81,21 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S geometry_msgs::msg::Point proj = DetUtils::projectLidarToCamera(transform_, camInfo_->p, pt); if (proj.z >= 0) { - proj.x /= proj.z; - proj.y /= proj.z; lidar2dProjs.emplace_back(proj); + // RCLCPP_INFO(this->get_logger(), "transform to 2d %f , %f, %f", proj.x, proj.y, proj.z); } } + + // temporarily also publish viz markers + visualization_msgs::msg::MarkerArray markerArray3d; + vision_msgs::msg::Detection3DArray detArray3d; sensor_msgs::msg::PointCloud2 pubCloud; pcl::toROSMsg(*lidarCloudNoFloor, pubCloud); pubCloud.header.frame_id = "LIDAR_TOP"; pc_publisher_->publish(pubCloud); - pcl::PointCloud mergedClusters; + pcl::PointCloud mergedClusters; // process each detection in det array for (const vision_msgs::msg::Detection2D& det : msg->detections) @@ -108,14 +107,14 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S DetUtils::pointsInBbox(inlierPoints, lidarCloudNoFloor, lidar2dProjs, bbox); if (inlierPoints->size() == 0) continue; - // mergedClusters += *inlierPoints; + mergedClusters += *inlierPoints; RCLCPP_INFO(this->get_logger(), "restrict to 2d %ld vs %ld", inlierPoints->size(), lidarCloudNoFloor->size()); // clustering auto clusterAndBBoxes = DetUtils::getClusteredBBoxes(inlierPoints); - RCLCPP_INFO(this->get_logger(), "get merged cloud %ld clustered bboxes %ld", clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); + RCLCPP_INFO(this->get_logger(), "%f, %f : get merged cloud %ld clustered bboxes %ld", bbox.center.position.x, bbox.center.position.y, clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only std::vector allBBoxes = clusterAndBBoxes.second; @@ -125,7 +124,7 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // [TODO] fix scoring bboxes by projecting & iou int bestIndex = highestIOUScoredBBox(allBBoxes, bbox); vision_msgs::msg::BoundingBox3D bestBBox = allBBoxes[bestIndex]; - mergedClusters += *(clusters[bestIndex]->getCloud()); + // mergedClusters += *(clusters[bestIndex]->getCloud()); RCLCPP_INFO(this->get_logger(), "scored bboxes"); @@ -140,9 +139,8 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale = bestBBox.size; marker.pose = bestBBox.center; - marker.header = pubCloud.header; - markerArray3d.markers.emplace_back(marker); + markerArray3d.markers.push_back(marker); } det3d_publisher_->publish(detArray3d); From 579bbb3107b317fd9bf15def517623f1bbeb23b3 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Thu, 22 Feb 2024 20:20:14 +0000 Subject: [PATCH 04/13] add padding to bboxes --- .../tracking/dets_2d_3d/src/det_utils.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp index b6747bfb..874136b9 100644 --- a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp @@ -51,16 +51,16 @@ void DetUtils::pointsInBbox( bool DetUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) { - double padding = 10; + double padding = 20; - // if (bbox.center.position.x - bbox.size_x/2 - padding < pt.x && pt.x < bbox.center.position.x + bbox.size_x/2 + padding - // && bbox.center.position.y - bbox.size_y/2 - padding < pt.y && pt.y < bbox.center.position.y + bbox.size_y/2 + padding) - // { - // return true; - // } - - if (pt.x > 0 && pt.x < 1600 && pt.y > 0 && pt.y < 900) + if (bbox.center.position.x - bbox.size_x/2 - padding < pt.x && pt.x < bbox.center.position.x + bbox.size_x/2 + padding + && bbox.center.position.y - bbox.size_y/2 - padding < pt.y && pt.y < bbox.center.position.y + bbox.size_y/2 + padding) + { return true; + } + + // if (pt.x > 0 && pt.x < 1600 && pt.y > 0 && pt.y < 900) + // return true; return false; } From f62ba04eb79672c7af08186b93b86ff22bd07b76 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Fri, 1 Mar 2024 02:20:13 +0000 Subject: [PATCH 05/13] show --- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 41 ++++++++++++------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index 483c2dcd..04093769 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -89,15 +89,11 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // temporarily also publish viz markers visualization_msgs::msg::MarkerArray markerArray3d; vision_msgs::msg::Detection3DArray detArray3d; - - sensor_msgs::msg::PointCloud2 pubCloud; - pcl::toROSMsg(*lidarCloudNoFloor, pubCloud); - pubCloud.header.frame_id = "LIDAR_TOP"; - pc_publisher_->publish(pubCloud); pcl::PointCloud mergedClusters; // process each detection in det array + int bboxId = 0; for (const vision_msgs::msg::Detection2D& det : msg->detections) { vision_msgs::msg::BoundingBox2D bbox = det.bbox; @@ -114,7 +110,9 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // clustering auto clusterAndBBoxes = DetUtils::getClusteredBBoxes(inlierPoints); - RCLCPP_INFO(this->get_logger(), "%f, %f : get merged cloud %ld clustered bboxes %ld", bbox.center.position.x, bbox.center.position.y, clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); + RCLCPP_INFO(this->get_logger(), "(x,y) : (%f, %f) size:(%f, %f) : get merged cloud %ld clustered bboxes %ld", + bbox.center.position.x, bbox.center.position.y, bbox.size_x, bbox.size_y, + clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only std::vector allBBoxes = clusterAndBBoxes.second; @@ -134,13 +132,20 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S det3d.results = det.results; detArray3d.detections.emplace_back(det3d); - // make marker & add to array (TEMP) - visualization_msgs::msg::Marker marker; - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.scale = bestBBox.size; - marker.pose = bestBBox.center; - marker.header = pubCloud.header; - markerArray3d.markers.push_back(marker); + // make marker & add to array (TEMP PUBLISH ALL BBOXES, NOT JUST "BEST") + for (const auto& maybeBbox : allBBoxes) + { + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale = maybeBbox.size; + marker.pose = maybeBbox.center; + marker.header.frame_id = "CAM_FRONT_RIGHT"; + marker.header.stamp = msg->header.stamp; + marker.id = bboxId; + + markerArray3d.markers.push_back(marker); + ++bboxId; + } } det3d_publisher_->publish(detArray3d); @@ -148,8 +153,16 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S sensor_msgs::msg::PointCloud2 pubCloud2; pcl::toROSMsg(mergedClusters, pubCloud2); - pubCloud2.header = pubCloud.header; + pubCloud2.header.frame_id = "LIDAR_TOP"; + pubCloud2.header.stamp = msg->header.stamp; + + sensor_msgs::msg::PointCloud2 pubCloud; + pcl::toROSMsg(*lidarCloudNoFloor, pubCloud); + pubCloud.header.frame_id = "LIDAR_TOP"; + pubCloud.header.stamp = msg->header.stamp; + pc_publisher2_->publish(pubCloud2); + pc_publisher_->publish(pubCloud); RCLCPP_INFO(this->get_logger(), "published 3d detection %ld", markerArray3d.markers.size()); } From a9ca40f2cee884150892a24be3c966655bb44e93 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Wed, 13 Mar 2024 20:25:46 +0000 Subject: [PATCH 06/13] fix projection --- .../tracking/dets_2d_3d/include/det_utils.hpp | 3 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 6 +- .../tracking/dets_2d_3d/src/det_utils.cpp | 29 +++++----- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 58 ++++++++++++++----- 4 files changed, 65 insertions(+), 31 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/include/det_utils.hpp b/src/perception/tracking/dets_2d_3d/include/det_utils.hpp index 0c0a5c3f..f8f49d06 100644 --- a/src/perception/tracking/dets_2d_3d/include/det_utils.hpp +++ b/src/perception/tracking/dets_2d_3d/include/det_utils.hpp @@ -8,6 +8,7 @@ #include #include +#include // main helper functions used by the node -- should all be static class DetUtils @@ -21,7 +22,7 @@ class DetUtils const vision_msgs::msg::BoundingBox2D& bbox); // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to camera frame - static geometry_msgs::msg::Point projectLidarToCamera( + static std::optional projectLidarToCamera( const geometry_msgs::msg::TransformStamped& transform, const std::array& P, const pcl::PointXYZ& pt); diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index 492791e2..bad30704 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -11,6 +11,8 @@ #include #include +#include "tf2_ros/static_transform_broadcaster.h" + #include #include @@ -42,7 +44,6 @@ class TrackingNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pc_publisher_; rclcpp::Publisher::SharedPtr pc_publisher2_; - // temp std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -60,4 +61,7 @@ class TrackingNode : public rclcpp::Node const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB); + // temp add colours to markers, corresponding to + std::map detColors; + std::shared_ptr tf_static_broadcaster_; }; diff --git a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp index 874136b9..7b72aa66 100644 --- a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/det_utils.cpp @@ -39,7 +39,7 @@ void DetUtils::pointsInBbox( { for (int i=0; i 0 && pt.x < 1600 && pt.y > 0 && pt.y < 900) - // return true; - return false; } -/* - +/* intrin 1266.417203046554 0.0 816.2670197447984 0.0 0.0 1266.417203046554 491.50706579294757 0.0 0.0 0.0 1.0 0.0 - - */ -geometry_msgs::msg::Point DetUtils::projectLidarToCamera( +std::optional DetUtils::projectLidarToCamera( const geometry_msgs::msg::TransformStamped& transform, const std::array& p, const pcl::PointXYZ& pt) { // lidar to camera frame auto trans_pt = geometry_msgs::msg::Point(); + // rotate points extra 90 degs [TODO] 90 roll, 90 yaw + geometry_msgs::msg::TransformStamped t; + t.header = transform.header; + t.transform.translation = transform.transform.translation; + auto orig_pt = geometry_msgs::msg::Point(); orig_pt.x = pt.x; orig_pt.y = pt.y; @@ -87,6 +87,8 @@ geometry_msgs::msg::Point DetUtils::projectLidarToCamera( tf2::doTransform(orig_pt, trans_pt, transform); + if (trans_pt.z < 1) return std::nullopt; + // camera frame to camera 2D projection double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; double v = p[4] * trans_pt.x + p[5] * trans_pt.y + p[6] * trans_pt.z + p[7]; @@ -95,9 +97,10 @@ geometry_msgs::msg::Point DetUtils::projectLidarToCamera( auto proj_pt = geometry_msgs::msg::Point(); proj_pt.x = u/w; proj_pt.y = v/w; - proj_pt.z = w; // needed to check if front/behind camera - return proj_pt; + // check if inside camera frame bounds -- earlier filtering + if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) return proj_pt; + return std::nullopt; } // https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index 04093769..94e93f92 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -12,8 +12,16 @@ #include #include + TrackingNode::TrackingNode() : Node("dets_2d_3d"), lidarCloud_{new pcl::PointCloud()}, transformInited_{false} { + { + std_msgs::msg::ColorRGBA r; r.r = 255; detColors["car"] = r; + std_msgs::msg::ColorRGBA b; b.b = 255; detColors["truck"] = b; + std_msgs::msg::ColorRGBA g; g.g = 255; detColors["person"] = g; + std_msgs::msg::ColorRGBA blk; detColors["default"] = blk; + } + camInfo_subscriber_ = this->create_subscription( "/CAM_FRONT/camera_info", 10, std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); @@ -58,6 +66,9 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S try { transform_ = tf_buffer_ ->lookupTransform("CAM_FRONT", "LIDAR_TOP", msg->header.stamp); transformInited_ = true; + RCLCPP_INFO(this->get_logger(), "got transform: \nquaternion: (%f, %f, %f, %f), \ntranslation: (%f, %f, %f)", + transform_.transform.rotation.x, transform_.transform.rotation.y, transform_.transform.rotation.z, transform_.transform.rotation.w, + transform_.transform.translation.x, transform_.transform.translation.y, transform_.transform.translation.z); } catch (const tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); return; @@ -76,16 +87,17 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // transform all lidar pts to 2d std::vector lidar2dProjs; + pcl::PointCloud::Ptr inCameraPoints(new pcl::PointCloud()); for (const pcl::PointXYZ& pt : lidarCloudNoFloor->points) - { - geometry_msgs::msg::Point proj = DetUtils::projectLidarToCamera(transform_, camInfo_->p, pt); - if (proj.z >= 0) + { + std::optional proj = DetUtils::projectLidarToCamera(transform_, camInfo_->p, pt); + if (proj) { - lidar2dProjs.emplace_back(proj); - // RCLCPP_INFO(this->get_logger(), "transform to 2d %f , %f, %f", proj.x, proj.y, proj.z); + inCameraPoints->emplace_back(pt); + lidar2dProjs.emplace_back(*proj); } } - + // temporarily also publish viz markers visualization_msgs::msg::MarkerArray markerArray3d; vision_msgs::msg::Detection3DArray detArray3d; @@ -100,9 +112,12 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // process lidarcloud with extrinsic trans from lidar to cam frame pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); - DetUtils::pointsInBbox(inlierPoints, lidarCloudNoFloor, lidar2dProjs, bbox); - if (inlierPoints->size() == 0) continue; - + DetUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, bbox); + if (inlierPoints->size() == 0) { + RCLCPP_INFO(this->get_logger(), "no inliers"); + continue; + } + mergedClusters += *inlierPoints; RCLCPP_INFO(this->get_logger(), "restrict to 2d %ld vs %ld", inlierPoints->size(), lidarCloudNoFloor->size()); @@ -139,13 +154,21 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale = maybeBbox.size; marker.pose = maybeBbox.center; - marker.header.frame_id = "CAM_FRONT_RIGHT"; + marker.header.frame_id = "LIDAR_TOP"; marker.header.stamp = msg->header.stamp; marker.id = bboxId; + // marker.color.r = 1.0; + + // if (const auto& it = detColors.find(det.results[0].hypothesis.class_id); it != detColors.end()) + // marker.color = it->second; + // else + // marker.color = detColors["default"]; + markerArray3d.markers.push_back(marker); ++bboxId; } + break; // look at one det for debugging } det3d_publisher_->publish(detArray3d); @@ -165,6 +188,7 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S pc_publisher_->publish(pubCloud); RCLCPP_INFO(this->get_logger(), "published 3d detection %ld", markerArray3d.markers.size()); + } int TrackingNode::highestIOUScoredBBox( @@ -188,14 +212,16 @@ int TrackingNode::highestIOUScoredBBox( bottom_right.y = b.center.position.y + b.size.y/2; bottom_right.z = b.center.position.z + b.size.z/2; - geometry_msgs::msg::Point top_left2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); - geometry_msgs::msg::Point bottom_right2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); + std::optional top_left2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); + std::optional bottom_right2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); + + if (!top_left2d || !bottom_right2d) return 0; vision_msgs::msg::BoundingBox2D bbox2d; - bbox2d.center.position.x = ((top_left2d.x/top_left2d.z) + (bottom_right2d.x/bottom_right2d.z))/2; - bbox2d.center.position.y = ((top_left2d.y/top_left2d.z) + (bottom_right2d.y/bottom_right2d.z))/2; - bbox2d.size_x = abs(top_left2d.x - bottom_right2d.x); - bbox2d.size_y = abs(top_left2d.y - bottom_right2d.y); + bbox2d.center.position.x = ((top_left2d->x/top_left2d->z) + (bottom_right2d->x/bottom_right2d->z))/2; + bbox2d.center.position.y = ((top_left2d->y/top_left2d->z) + (bottom_right2d->y/bottom_right2d->z))/2; + bbox2d.size_x = abs(top_left2d->x - bottom_right2d->x); + bbox2d.size_y = abs(top_left2d->y - bottom_right2d->y); double iou = iouScore(bbox2d, detBBox); RCLCPP_INFO(this->get_logger(), "pos: %f, %f, %f, size: %f, %f, %f : iou %f", From 7220b398f2f8435a534937cdb21f42e11d0c8730 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Mon, 18 Mar 2024 03:54:27 +0000 Subject: [PATCH 07/13] cleanup and add node params --- .../tracking/dets_2d_3d/CMakeLists.txt | 13 +- .../tracking/dets_2d_3d/cmake/Eigen.cmake | 7 - .../dets_2d_3d/config/nuscenes_config.yaml | 18 ++ .../tracking/dets_2d_3d/include/cluster.hpp | 13 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 13 +- .../{det_utils.hpp => projection_utils.hpp} | 19 +- .../dets_2d_3d/launch/dets_2d_3d.launch.py | 23 ++- .../tracking/dets_2d_3d/package.xml | 1 + .../tracking/dets_2d_3d/src/cluster.cpp | 30 +-- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 188 +++++++++--------- .../{det_utils.cpp => projection_utils.cpp} | 90 +++------ 11 files changed, 194 insertions(+), 221 deletions(-) delete mode 100644 src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake create mode 100644 src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml rename src/perception/tracking/dets_2d_3d/include/{det_utils.hpp => projection_utils.hpp} (91%) rename src/perception/tracking/dets_2d_3d/src/{det_utils.cpp => projection_utils.cpp} (76%) diff --git a/src/perception/tracking/dets_2d_3d/CMakeLists.txt b/src/perception/tracking/dets_2d_3d/CMakeLists.txt index 2a415e2e..1e7b3c78 100644 --- a/src/perception/tracking/dets_2d_3d/CMakeLists.txt +++ b/src/perception/tracking/dets_2d_3d/CMakeLists.txt @@ -21,14 +21,15 @@ find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(std_msgs REQUIRED) -add_library(det_utils - src/det_utils.cpp src/cluster.cpp) # add more non-ros node files here +add_library(projection_utils + src/projection_utils.cpp src/cluster.cpp) # add more non-ros node files here -target_include_directories(det_utils +target_include_directories(projection_utils PUBLIC include) -ament_target_dependencies(det_utils +ament_target_dependencies(projection_utils rclcpp sample_msgs sensor_msgs @@ -39,6 +40,7 @@ ament_target_dependencies(det_utils tf2 tf2_ros tf2_geometry_msgs + std_msgs geometry_msgs) add_executable(dets_2d_3d_node src/dets_2d_3d_node.cpp) @@ -52,7 +54,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) target_link_libraries (dets_2d_3d_node ${OpenCV_LIBS}) ament_target_dependencies(dets_2d_3d_node) -target_link_libraries(dets_2d_3d_node det_utils) +target_link_libraries(dets_2d_3d_node projection_utils) # Copy executable to installation location install(TARGETS @@ -61,6 +63,7 @@ install(TARGETS # Copy launch files to installation location install(DIRECTORY + config launch DESTINATION share/${PROJECT_NAME}) diff --git a/src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake b/src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake deleted file mode 100644 index 8dcf251c..00000000 --- a/src/perception/tracking/dets_2d_3d/cmake/Eigen.cmake +++ /dev/null @@ -1,7 +0,0 @@ -find_package (Eigen3 3.3 REQUIRED NO_MODULE) - -include_directories(${Eigen_INCLUDE_DIRS}) -list(APPEND ALL_TARGET_LIBRARIES ${Eigen_LIBRARIES}) - -link_directories(${Eigen_LIBRARY_DIRS}) -add_definitions(${Eigen_DEFINITIONS}) diff --git a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml new file mode 100644 index 00000000..ca9e3dae --- /dev/null +++ b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml @@ -0,0 +1,18 @@ +camera_object_detection_node: + ros__parameters: + camera_info_topic: /CAM_FRONT/camera_info + lidar_topic: /LIDAR_TOP + detections_topic: /detections + + publish_detections_topic: /detections_3d + publish_markers_topic: /markers_3d + publish_clusters_topic: /clustered_pc + + camera_frame: CAM_FRONT + lidar_frame: LIDAR_TOP + + clustering_distances: [5., 30., 45., 60.] + clustering_thresholds: [0.5, 1.1, 1.6, 2.1, 2.6] + cluster_size_min: 20. + cluster_size_max: 100000. + cluster_merge_threshold: 1.5 diff --git a/src/perception/tracking/dets_2d_3d/include/cluster.hpp b/src/perception/tracking/dets_2d_3d/include/cluster.hpp index e8627e24..13bad5d4 100644 --- a/src/perception/tracking/dets_2d_3d/include/cluster.hpp +++ b/src/perception/tracking/dets_2d_3d/include/cluster.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include @@ -8,27 +10,22 @@ class Cluster { public: - // make cloud from only pts that have in "in_cluster_indices" + // makes a cloud from all points `in_cloud_ptr` that are indexed by `in_cluster_indices` Cluster( const pcl::PointCloud::Ptr& in_cloud_ptr, const std::vector& in_cluster_indices); - // Cluster( - // const pcl::PointCloud::Ptr& in_cloud_ptr, - // const std::vector& in_cluster_indices); - pcl::PointXYZ getCentroid() { return centroid_; } pcl::PointCloud::Ptr getCloud() { return cloud_; } vision_msgs::msg::BoundingBox3D getBoundingBox(); - bool isValid(const vision_msgs::msg::BoundingBox3D& b); - + bool isValid(); + int size() { return cloud_->size(); } private: pcl::PointCloud::Ptr cloud_; pcl::PointXYZ centroid_; - // stores the min_x, min_y, min_z pcl::PointXYZ max_point_; pcl::PointXYZ min_point_; diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index bad30704..a03ce6eb 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -11,8 +11,9 @@ #include #include -#include "tf2_ros/static_transform_broadcaster.h" +#include +#include "cluster.hpp" #include #include @@ -23,6 +24,9 @@ class TrackingNode : public rclcpp::Node TrackingNode(); private: + // lidar and camera frame names + std::string lidarFrame_; + std::string cameraFrame_; // synchronization utils std::mutex lidarCloud_mutex_; @@ -53,15 +57,12 @@ class TrackingNode : public rclcpp::Node int highestIOUScoredBBox( const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D& detBBox); + const vision_msgs::msg::BoundingBox2D& detBBox, + const std::vector>& clusters); double overlapBoundingBox( const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB); double iouScore( const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB); - - // temp add colours to markers, corresponding to - std::map detColors; - std::shared_ptr tf_static_broadcaster_; }; diff --git a/src/perception/tracking/dets_2d_3d/include/det_utils.hpp b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp similarity index 91% rename from src/perception/tracking/dets_2d_3d/include/det_utils.hpp rename to src/perception/tracking/dets_2d_3d/include/projection_utils.hpp index f8f49d06..941c6399 100644 --- a/src/perception/tracking/dets_2d_3d/include/det_utils.hpp +++ b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp @@ -1,3 +1,5 @@ +#pragma once + #include "cluster.hpp" #include @@ -11,9 +13,17 @@ #include // main helper functions used by the node -- should all be static -class DetUtils +class ProjectionUtils { public: + // segment by distance + static std::vector clustering_distances_; + // Nearest neighbor distance threshold for each segment + static std::vector clustering_thresholds_; + + static double cluster_size_min_; + static double cluster_size_max_; + static double cluster_merge_threshold_; static void pointsInBbox( const pcl::PointCloud::Ptr& inlierCloud, @@ -57,11 +67,4 @@ class DetUtils static std::vector> checkAllForMerge( const std::vector>& in_clusters, float in_merge_threshold); - - static std::vector _clustering_distances; - static std::vector _clustering_thresholds; - - static double cluster_size_min_; - static double cluster_size_max_; - static double cluster_merge_threshold_; }; diff --git a/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py b/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py index 5b167eb7..ccf46579 100644 --- a/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py +++ b/src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py @@ -1,14 +1,27 @@ from launch import LaunchDescription from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): - """Launch aggregator node.""" - tracking_node = Node( + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('dets_2d_3d'), + 'config', + 'nuscenes_config.yaml' + ) + + # nodes + detections_2d_3d_node = Node( package='dets_2d_3d', executable='dets_2d_3d_node', + name='dets_2d_3d_node', + parameters=[config], + arguments=['--ros-args', '--log-level', 'info'] ) - return LaunchDescription([ - tracking_node - ]) + # finalize + ld.add_action(detections_2d_3d_node) + + return ld diff --git a/src/perception/tracking/dets_2d_3d/package.xml b/src/perception/tracking/dets_2d_3d/package.xml index 55527356..dbb6686d 100644 --- a/src/perception/tracking/dets_2d_3d/package.xml +++ b/src/perception/tracking/dets_2d_3d/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs geometry_msgs visualization_msgs + std_msgs ament_cmake diff --git a/src/perception/tracking/dets_2d_3d/src/cluster.cpp b/src/perception/tracking/dets_2d_3d/src/cluster.cpp index bfd3444c..e2b16717 100644 --- a/src/perception/tracking/dets_2d_3d/src/cluster.cpp +++ b/src/perception/tracking/dets_2d_3d/src/cluster.cpp @@ -14,40 +14,19 @@ #include - typedef pcl::PointCloud ClusterCloud; typedef vision_msgs::msg::BoundingBox3D BBox3D; int Cluster::color_index = 0; std::vector> Cluster::colors = {{0,0,255}, {0,255,0}, {255,0,0}, {255,255,0}, {0,255,255}, {255,0,255}}; -// Cluster::Cluster( -// const ClusterCloud::Ptr& in_cloud_ptr, -// const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} -// { -// bool indexesGiven = in_cluster_indices.size() > 0; - -// int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); - -// for (int i=0; ipoints[idx]; -// cloud_->emplace_back(pt); -// } - -// ++color_index; -// } - Cluster::Cluster( const pcl::PointCloud::Ptr& in_cloud_ptr, const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} { bool indexesGiven = in_cluster_indices.size() > 0; - int max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + size_t max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); min_point_.x = std::numeric_limits::max(); min_point_.y = std::numeric_limits::max(); @@ -57,9 +36,9 @@ Cluster::Cluster( max_point_.y = -std::numeric_limits::max(); max_point_.z = -std::numeric_limits::max(); - for (int i=0; ipoints[idx].x; @@ -135,8 +114,7 @@ BBox3D Cluster::getBoundingBox() return bounding_box_; } -bool Cluster::isValid(const BBox3D& b) +bool Cluster::isValid() { return cloud_->size() > 0; - // return (b.size.x > 0 && b.size.y > 0 && b.size.z > 0); } diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index 94e93f92..e2296893 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -1,10 +1,9 @@ #include "dets_2d_3d_node.hpp" -#include "det_utils.hpp" +#include "projection_utils.hpp" #include #include #include -#include #include #include @@ -13,40 +12,69 @@ #include -TrackingNode::TrackingNode() : Node("dets_2d_3d"), lidarCloud_{new pcl::PointCloud()}, transformInited_{false} +TrackingNode::TrackingNode() : Node("dets_2d_3d"), transformInited_{false} , lidarCloud_{new pcl::PointCloud()} { - { - std_msgs::msg::ColorRGBA r; r.r = 255; detColors["car"] = r; - std_msgs::msg::ColorRGBA b; b.b = 255; detColors["truck"] = b; - std_msgs::msg::ColorRGBA g; g.g = 255; detColors["person"] = g; - std_msgs::msg::ColorRGBA blk; detColors["default"] = blk; - } + // setup paramaters + this->declare_parameter("camera_info_topic", "/CAM_FRONT/camera_info"); + this->declare_parameter("lidar_topic", "/LIDAR_TOP"); + this->declare_parameter("detections_topic", "/detections"); + + this->declare_parameter("publish_detections_topic", "/detections_3d"); + this->declare_parameter("publish_markers_topic", "/markers_3d"); + this->declare_parameter("publish_clusters_topic", "/clustered_pc"); + + this->declare_parameter("camera_frame", "CAM_FRONT"); + this->declare_parameter("lidar_frame", "LIDAR_TOP"); + + this->declare_parameter("clustering_distances", std::vector{5, 30, 45, 60}); + this->declare_parameter("clustering_thresholds", std::vector{0.5, 1.1, 1.6, 2.1, 2.6}); + this->declare_parameter("cluster_size_min", 20.); + this->declare_parameter("cluster_size_max", 100000.); + this->declare_parameter("cluster_merge_threshold", 1.5); + lidarFrame_ = this->get_parameter("camera_frame").as_string(); + cameraFrame_ = this->get_parameter("lidar_frame").as_string(); + + // setup pub subs camInfo_subscriber_ = this->create_subscription( - "/CAM_FRONT/camera_info", 10, + this->get_parameter("camera_info_topic").as_string(), 10, std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); lidar_subscriber_ = this->create_subscription( - "/LIDAR_TOP", 10, + this->get_parameter("lidar_topic").as_string(), 10, std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); det_subscriber_ = this->create_subscription( - "/detections", 10, + this->get_parameter("detections_topic").as_string(), 10, std::bind(&TrackingNode::receiveDetections, this, std::placeholders::_1)); - det3d_publisher_ = this->create_publisher("/detections_3d", 10); - marker_publisher_ = this->create_publisher("/markers_3d", 10); - pc_publisher_ = this->create_publisher("/trans_pc", 10); - pc_publisher2_ = this->create_publisher("/trans_pc2", 10); + det3d_publisher_ = this->create_publisher( + this->get_parameter("publish_detections_topic").as_string(), 10); + marker_publisher_ = this->create_publisher( + this->get_parameter("publish_markers_topic").as_string(), 10); + pc_publisher_ = this->create_publisher( + this->get_parameter("publish_clusters_topic").as_string(), 10); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + + // initialize common params used by utils + + // 0 => 0-15m d=0.5, 1 => 15-30 d=1, 2 => 30-45 d=1.6, 3 => 45-60 d=2.1, 4 => >60 d=2.6 + ProjectionUtils::clustering_distances_ = this->get_parameter("clustering_distances").as_double_array(); + ProjectionUtils::clustering_thresholds_ = this->get_parameter("clustering_thresholds").as_double_array(); + + ProjectionUtils::cluster_size_min_ = this->get_parameter("cluster_size_min").as_double(); + ProjectionUtils::cluster_size_max_ = this->get_parameter("cluster_size_max").as_double(); + ProjectionUtils::cluster_merge_threshold_ = this->get_parameter("cluster_merge_threshold").as_double();; + } void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) { // save latest lidar info std::lock_guard guard_lidar(lidarCloud_mutex_); + sensor_msgs::msg::PointCloud2 pointCloud = *pointCloud_msg; pcl::fromROSMsg(pointCloud, *lidarCloud_); @@ -64,14 +92,11 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S if (!transformInited_) { try { - transform_ = tf_buffer_ ->lookupTransform("CAM_FRONT", "LIDAR_TOP", msg->header.stamp); - transformInited_ = true; - RCLCPP_INFO(this->get_logger(), "got transform: \nquaternion: (%f, %f, %f, %f), \ntranslation: (%f, %f, %f)", - transform_.transform.rotation.x, transform_.transform.rotation.y, transform_.transform.rotation.z, transform_.transform.rotation.w, - transform_.transform.translation.x, transform_.transform.translation.y, transform_.transform.translation.z); + transform_ = tf_buffer_ ->lookupTransform(cameraFrame_, lidarFrame_, msg->header.stamp); + transformInited_ = true; } catch (const tf2::TransformException & ex) { - RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); - return; + RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); + return; } } @@ -79,18 +104,18 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S pcl::PointCloud::Ptr lidarCloudNoFloor(new pcl::PointCloud()); { std::lock_guard guard_lidar(lidarCloud_mutex_); - DetUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); + ProjectionUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); - if (lidarCloudNoFloor->size() == 0) return; // end if there is only a floor + if (lidarCloudNoFloor->size() == 0) return; // do not process more, if all the points are floor points RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", lidarCloud_->size(), lidarCloudNoFloor->size()); } - // transform all lidar pts to 2d + // transform all lidar pts to 2d points in the camera std::vector lidar2dProjs; pcl::PointCloud::Ptr inCameraPoints(new pcl::PointCloud()); for (const pcl::PointXYZ& pt : lidarCloudNoFloor->points) { - std::optional proj = DetUtils::projectLidarToCamera(transform_, camInfo_->p, pt); + std::optional proj = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, pt); if (proj) { inCameraPoints->emplace_back(pt); @@ -98,11 +123,10 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S } } - // temporarily also publish viz markers + // publish both rviz markers (for visualization) and a detection array visualization_msgs::msg::MarkerArray markerArray3d; vision_msgs::msg::Detection3DArray detArray3d; - - pcl::PointCloud mergedClusters; + pcl::PointCloud mergedClusters; // coloured pc to visualize points in each cluster // process each detection in det array int bboxId = 0; @@ -112,96 +136,68 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S // process lidarcloud with extrinsic trans from lidar to cam frame pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); - DetUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, bbox); + ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, bbox); if (inlierPoints->size() == 0) { - RCLCPP_INFO(this->get_logger(), "no inliers"); + RCLCPP_INFO(this->get_logger(), "no inliers found for detection %s", det.results[0].hypothesis.class_id.c_str()); continue; } - - mergedClusters += *inlierPoints; - - RCLCPP_INFO(this->get_logger(), "restrict to 2d %ld vs %ld", inlierPoints->size(), lidarCloudNoFloor->size()); // clustering - auto clusterAndBBoxes = DetUtils::getClusteredBBoxes(inlierPoints); - - RCLCPP_INFO(this->get_logger(), "(x,y) : (%f, %f) size:(%f, %f) : get merged cloud %ld clustered bboxes %ld", - bbox.center.position.x, bbox.center.position.y, bbox.size_x, bbox.size_y, - clusterAndBBoxes.first.size(), clusterAndBBoxes.second.size()); - + auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes(inlierPoints); std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only std::vector allBBoxes = clusterAndBBoxes.second; if (clusters.size() == 0 || allBBoxes.size() == 0) continue; - // [TODO] fix scoring bboxes by projecting & iou - int bestIndex = highestIOUScoredBBox(allBBoxes, bbox); + // score each 3d bbox based on iou with teh original 2d bbox & the density of points in the cluster + int bestIndex = highestIOUScoredBBox(allBBoxes, bbox, clusters); vision_msgs::msg::BoundingBox3D bestBBox = allBBoxes[bestIndex]; - // mergedClusters += *(clusters[bestIndex]->getCloud()); - RCLCPP_INFO(this->get_logger(), "scored bboxes"); + // for visauzliation : adds detection to cluster pc visualization & the marker array & the 3d detection array + mergedClusters += *(clusters[bestIndex]->getCloud()); - // find 3d box that encloses clustered pointcloud & add to det array vision_msgs::msg::Detection3D det3d; det3d.bbox = bestBBox; det3d.results = det.results; detArray3d.detections.emplace_back(det3d); - // make marker & add to array (TEMP PUBLISH ALL BBOXES, NOT JUST "BEST") - for (const auto& maybeBbox : allBBoxes) - { - visualization_msgs::msg::Marker marker; - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.scale = maybeBbox.size; - marker.pose = maybeBbox.center; - marker.header.frame_id = "LIDAR_TOP"; - marker.header.stamp = msg->header.stamp; - marker.id = bboxId; - - // marker.color.r = 1.0; - - // if (const auto& it = detColors.find(det.results[0].hypothesis.class_id); it != detColors.end()) - // marker.color = it->second; - // else - // marker.color = detColors["default"]; - - markerArray3d.markers.push_back(marker); - ++bboxId; - } - break; // look at one det for debugging + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale = bestBBox.size; + marker.pose = bestBBox.center; + marker.header.frame_id = lidarFrame_; + marker.header.stamp = msg->header.stamp; + marker.id = bboxId; + markerArray3d.markers.push_back(marker); + ++bboxId; } det3d_publisher_->publish(detArray3d); marker_publisher_->publish(markerArray3d); - sensor_msgs::msg::PointCloud2 pubCloud2; - pcl::toROSMsg(mergedClusters, pubCloud2); - pubCloud2.header.frame_id = "LIDAR_TOP"; - pubCloud2.header.stamp = msg->header.stamp; - sensor_msgs::msg::PointCloud2 pubCloud; - pcl::toROSMsg(*lidarCloudNoFloor, pubCloud); - pubCloud.header.frame_id = "LIDAR_TOP"; + pcl::toROSMsg(mergedClusters, pubCloud); + pubCloud.header.frame_id = lidarFrame_; pubCloud.header.stamp = msg->header.stamp; - pc_publisher2_->publish(pubCloud2); pc_publisher_->publish(pubCloud); - RCLCPP_INFO(this->get_logger(), "published 3d detection %ld", markerArray3d.markers.size()); + RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", markerArray3d.markers.size()); } int TrackingNode::highestIOUScoredBBox( const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D& detBBox) + const vision_msgs::msg::BoundingBox2D& detBBox, + const std::vector>& clusters) { int bestScore = 0; int bestBBox = 0; - for (int i=0; i top_left2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); - std::optional bottom_right2d = DetUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); + std::optional top_left2d = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); + std::optional bottom_right2d = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); if (!top_left2d || !bottom_right2d) return 0; vision_msgs::msg::BoundingBox2D bbox2d; - bbox2d.center.position.x = ((top_left2d->x/top_left2d->z) + (bottom_right2d->x/bottom_right2d->z))/2; - bbox2d.center.position.y = ((top_left2d->y/top_left2d->z) + (bottom_right2d->y/bottom_right2d->z))/2; + bbox2d.center.position.x = ((top_left2d->x) + (bottom_right2d->x))/2; + bbox2d.center.position.y = ((top_left2d->y) + (bottom_right2d->y))/2; bbox2d.size_x = abs(top_left2d->x - bottom_right2d->x); bbox2d.size_y = abs(top_left2d->y - bottom_right2d->y); double iou = iouScore(bbox2d, detBBox); - RCLCPP_INFO(this->get_logger(), "pos: %f, %f, %f, size: %f, %f, %f : iou %f", - b.center.position.x, b.center.position.y, b.center.position.z, - b.size.x, b.size.y, b.size.z, iou); - if (iou > bestScore) + // score also includes density of points (num points/ volume) in the 3d bbox + double density = clusters[i]->size() / (b.size.x * b.size.y * b.size.z); + double score = iou + density; + + if (score > bestScore) { - bestScore = iou; + bestScore = score; bestBBox = i; } } @@ -240,19 +237,20 @@ int TrackingNode::highestIOUScoredBBox( double TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D& boxA, const vision_msgs::msg::BoundingBox2D& boxB) { double overlapHeight = - std::min(boxA.center.position.y + boxA.size_y/2, boxB.center.position.y + boxB.size_y) - - std::max(boxA.center.position.y, boxB.center.position.y); + std::min(boxA.center.position.y + boxA.size_y/2, boxB.center.position.y + boxB.size_y/2) - + std::max(boxA.center.position.y - boxA.size_y/2, boxB.center.position.y - boxB.size_y/2); if (overlapHeight <= 0) return 0; - double overlapWidth = std::min(boxA.center.position.x + boxA.size_x, boxB.center.position.x + boxB.size_x) - - std::max(boxA.center.position.x, boxB.center.position.x); + + double overlapWidth = std::min(boxA.center.position.x + boxA.size_x/2, boxB.center.position.x + boxB.size_x/2) - + std::max(boxA.center.position.x - boxA.size_x/2, boxB.center.position.x - boxB.size_x/2); if (overlapWidth <= 0) return 0; + return overlapHeight * overlapWidth; } double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB) { double overlap = overlapBoundingBox(bboxA, bboxB); - // If overlap is 0, it returns 0 return (overlap / (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - overlap)); diff --git a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp similarity index 76% rename from src/perception/tracking/dets_2d_3d/src/det_utils.cpp rename to src/perception/tracking/dets_2d_3d/src/projection_utils.cpp index 7b72aa66..2ad65754 100644 --- a/src/perception/tracking/dets_2d_3d/src/det_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp @@ -1,8 +1,8 @@ -#include "det_utils.hpp" +#include "projection_utils.hpp" #include #include -#include +#include #include #include @@ -20,38 +20,28 @@ typedef pcl::PointCloud ColorCloud; typedef std::shared_ptr ClusterPtr; typedef vision_msgs::msg::BoundingBox3D BBox3D; -// segment by distance (5 sections) -// 0 => 0-15m d=0.5, 1 => 15-30 d=1, 2 => 30-45 d=1.6, 3 => 45-60 d=2.1, 4 => >60 d=2.6 -std::vector DetUtils::_clustering_distances = {5, 30, 45, 60}; +std::vector ProjectionUtils::clustering_distances_; +std::vector ProjectionUtils::clustering_thresholds_; +double ProjectionUtils::cluster_size_min_; +double ProjectionUtils::cluster_size_max_; +double ProjectionUtils::cluster_merge_threshold_; -//Nearest neighbor distance threshold for each segment -std::vector DetUtils::_clustering_thresholds = {0.5, 1.1, 1.6, 2.1, 2.6}; - -double DetUtils::cluster_size_min_ = 20; -double DetUtils::cluster_size_max_ = 100000; -double DetUtils::cluster_merge_threshold_ = 1.5; - -void DetUtils::pointsInBbox( +void ProjectionUtils::pointsInBbox( const Cloud::Ptr& inlierCloud, const Cloud::Ptr& lidarCloud, const std::vector& projs2d, const vision_msgs::msg::BoundingBox2D& bbox) { - for (int i=0; ipush_back(lidarCloud->points[i]); - } - } } -bool DetUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) +bool ProjectionUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) { - return true; double padding = 0; if (bbox.center.position.x - bbox.size_x/2 - padding < pt.x && pt.x < bbox.center.position.x + bbox.size_x/2 + padding @@ -59,27 +49,17 @@ bool DetUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_m { return true; } + return false; } -/* intrin -1266.417203046554 0.0 816.2670197447984 0.0 -0.0 1266.417203046554 491.50706579294757 0.0 -0.0 0.0 1.0 0.0 -*/ - -std::optional DetUtils::projectLidarToCamera( +std::optional ProjectionUtils::projectLidarToCamera( const geometry_msgs::msg::TransformStamped& transform, const std::array& p, const pcl::PointXYZ& pt) { // lidar to camera frame auto trans_pt = geometry_msgs::msg::Point(); - // rotate points extra 90 degs [TODO] 90 roll, 90 yaw - geometry_msgs::msg::TransformStamped t; - t.header = transform.header; - t.transform.translation = transform.transform.translation; - auto orig_pt = geometry_msgs::msg::Point(); orig_pt.x = pt.x; orig_pt.y = pt.y; @@ -98,13 +78,13 @@ std::optional DetUtils::projectLidarToCamera( proj_pt.x = u/w; proj_pt.y = v/w; - // check if inside camera frame bounds -- earlier filtering + // check if inside camera frame bounds if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) return proj_pt; return std::nullopt; } // https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html -void DetUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud_filtered) +void ProjectionUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud_filtered) { pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); @@ -132,11 +112,10 @@ void DetUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud extract.filter(*cloud_filtered); } -// https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html -std::pair, std::vector> DetUtils::getClusteredBBoxes(const Cloud::Ptr& lidarCloud) +std::pair, std::vector> ProjectionUtils::getClusteredBBoxes(const Cloud::Ptr& lidarCloud) { std::vector::Ptr> cloud_segments_array(5); - for (int i=0; i::Ptr temp(new pcl::PointCloud()); cloud_segments_array[i] = temp; @@ -146,13 +125,13 @@ std::pair, std::vector> DetUtils::getClusteredBB for (const pcl::PointXYZ& pt : lidarCloud->points) { float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); - if (origin_distance < _clustering_distances[0]) + if (origin_distance < clustering_distances_[0]) cloud_segments_array[0]->points.push_back(pt); - else if (origin_distance < _clustering_distances[1]) + else if (origin_distance < clustering_distances_[1]) cloud_segments_array[1]->points.push_back(pt); - else if (origin_distance < _clustering_distances[2]) + else if (origin_distance < clustering_distances_[2]) cloud_segments_array[2]->points.push_back(pt); - else if (origin_distance < _clustering_distances[3]) + else if (origin_distance < clustering_distances_[3]) cloud_segments_array[3]->points.push_back(pt); else cloud_segments_array[4]->points.push_back(pt); @@ -163,41 +142,30 @@ std::pair, std::vector> DetUtils::getClusteredBB for (unsigned int i = 1; i < cloud_segments_array.size(); i++) { // add clusters from each shell - std::vector local_clusters = clusterAndColor(cloud_segments_array[i], _clustering_thresholds[i]); + std::vector local_clusters = clusterAndColor(cloud_segments_array[i], clustering_thresholds_[i]); all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end()); } // merge clusters if possible, do this twice? std::vector mid_clusters = (all_clusters.size() > 0) - ? DetUtils::checkAllForMerge(all_clusters, cluster_merge_threshold_) + ? ProjectionUtils::checkAllForMerge(all_clusters, cluster_merge_threshold_) : all_clusters; std::vector final_clusters = (mid_clusters.size() > 0) - ? DetUtils::checkAllForMerge(mid_clusters, cluster_merge_threshold_) + ? ProjectionUtils::checkAllForMerge(mid_clusters, cluster_merge_threshold_) : mid_clusters; - // std::vector final_clusters = all_clusters; - // get boundingboxes for each & return all possible 3d bboxes (if valid) std::vector bboxes; for (const ClusterPtr& cluster : final_clusters) { BBox3D b = cluster->getBoundingBox(); - if (cluster->isValid(b)) + if (cluster->isValid()) bboxes.emplace_back(b); } - - - // TEMP - RETURN RGB CLOUD WITH ALL CLUSTERS (not just the same merged c) - // Cloud merged_cloud; - // for (const ClusterPtr& clusterPtr : final_clusters) - // { - // merged_cloud += *(clusterPtr->getCloud()); - // } - return std::pair, std::vector>{final_clusters, bboxes}; } -std::vector DetUtils::clusterAndColor( +std::vector ProjectionUtils::clusterAndColor( const Cloud::Ptr& in_cloud_ptr, double in_max_cluster_distance) { std::vector clusters; @@ -215,7 +183,7 @@ std::vector DetUtils::clusterAndColor( std::vector cluster_indices; - // perform clustering on 2d cloud + // perform clustering on 2d cloud : https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(in_max_cluster_distance); ec.setMinClusterSize(cluster_size_min_); @@ -236,7 +204,7 @@ std::vector DetUtils::clusterAndColor( } -void DetUtils::checkClusterMerge( +void ProjectionUtils::checkClusterMerge( size_t in_cluster_id, const std::vector &in_clusters, std::vector& in_out_visited_clusters, std::vector& out_merge_indices, double in_merge_threshold) @@ -262,7 +230,7 @@ void DetUtils::checkClusterMerge( } -ClusterPtr DetUtils::mergeClusters( +ClusterPtr ProjectionUtils::mergeClusters( const std::vector& in_clusters, const std::vector& in_merge_indices, std::vector &in_out_merged_clusters) @@ -286,7 +254,7 @@ ClusterPtr DetUtils::mergeClusters( return merged_cluster; } -std::vector DetUtils::checkAllForMerge( +std::vector ProjectionUtils::checkAllForMerge( const std::vector& in_clusters, float in_merge_threshold) { std::vector out_clusters; From 3d51a6d43e40ccb6dec70ccb11dfa6981afebf27 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Thu, 28 Mar 2024 23:17:31 +0000 Subject: [PATCH 08/13] update parameter parsing --- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 24 ++++++- .../dets_2d_3d/include/projection_utils.hpp | 25 ++++--- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 72 +++++++++++-------- .../dets_2d_3d/src/projection_utils.cpp | 32 ++++----- 4 files changed, 94 insertions(+), 59 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index a03ce6eb..349516e1 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -13,17 +13,24 @@ #include #include -#include "cluster.hpp" - #include #include +#include "cluster.hpp" +#include "projection_utils.hpp" + +#include + +struct ClusteringParams; + class TrackingNode : public rclcpp::Node { public: TrackingNode(); private: + std::map clusteringParams; + // lidar and camera frame names std::string lidarFrame_; std::string cameraFrame_; @@ -65,4 +72,17 @@ class TrackingNode : public rclcpp::Node double iouScore( const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB); + + std::map initializeClusteringParams( + const std::map& clustering_params_map); + + // should put somewhere else? + template + T getDefaultOrValue(std::map m, std::string key) + { + if (m.find(key) == m.end()) + return m[key]; + return m["default"]; + } + }; diff --git a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp index 941c6399..9410382c 100644 --- a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp +++ b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp @@ -12,19 +12,22 @@ #include #include +struct ClusteringParams +{ + // segment by distance + std::vector clustering_distances; + // Map of the nearest neighbor distance threshold for each segment, for each detection + std::vector clustering_thresholds; + + double cluster_size_min; + double cluster_size_max; + double cluster_merge_threshold; +}; + // main helper functions used by the node -- should all be static class ProjectionUtils { public: - // segment by distance - static std::vector clustering_distances_; - // Nearest neighbor distance threshold for each segment - static std::vector clustering_thresholds_; - - static double cluster_size_min_; - static double cluster_size_max_; - static double cluster_merge_threshold_; - static void pointsInBbox( const pcl::PointCloud::Ptr& inlierCloud, const pcl::PointCloud::Ptr& lidarCloud, @@ -46,13 +49,13 @@ class ProjectionUtils const pcl::PointCloud::Ptr& cloud_filtered); static std::pair>, std::vector> getClusteredBBoxes( - const pcl::PointCloud::Ptr& lidarCloud); + const pcl::PointCloud::Ptr& lidarCloud, const ClusteringParams& clusteringParams); private: static std::vector> clusterAndColor( const pcl::PointCloud::Ptr& in_cloud_ptr, - double in_max_cluster_distance); + double in_max_cluster_distance, double cluster_size_min, double cluster_size_max); static void checkClusterMerge( size_t in_cluster_id, const std::vector> &in_clusters, diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index e2296893..0b823e33 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -1,5 +1,4 @@ #include "dets_2d_3d_node.hpp" -#include "projection_utils.hpp" #include #include @@ -12,28 +11,23 @@ #include -TrackingNode::TrackingNode() : Node("dets_2d_3d"), transformInited_{false} , lidarCloud_{new pcl::PointCloud()} +TrackingNode::TrackingNode() : Node("dets_2d_3d", rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)), + transformInited_{false} , lidarCloud_{new pcl::PointCloud()} { // setup paramaters - this->declare_parameter("camera_info_topic", "/CAM_FRONT/camera_info"); - this->declare_parameter("lidar_topic", "/LIDAR_TOP"); - this->declare_parameter("detections_topic", "/detections"); + // this->declare_parameter("camera_info_topic", "/CAM_FRONT/camera_info"); + // this->declare_parameter("lidar_topic", "/LIDAR_TOP"); + // this->declare_parameter("detections_topic", "/detections"); - this->declare_parameter("publish_detections_topic", "/detections_3d"); - this->declare_parameter("publish_markers_topic", "/markers_3d"); - this->declare_parameter("publish_clusters_topic", "/clustered_pc"); + // this->declare_parameter("publish_detections_topic", "/detections_3d"); + // this->declare_parameter("publish_markers_topic", "/markers_3d"); + // this->declare_parameter("publish_clusters_topic", "/clustered_pc"); - this->declare_parameter("camera_frame", "CAM_FRONT"); - this->declare_parameter("lidar_frame", "LIDAR_TOP"); + // this->declare_parameter("camera_frame", ""); + // this->declare_parameter("lidar_frame", ""); - this->declare_parameter("clustering_distances", std::vector{5, 30, 45, 60}); - this->declare_parameter("clustering_thresholds", std::vector{0.5, 1.1, 1.6, 2.1, 2.6}); - this->declare_parameter("cluster_size_min", 20.); - this->declare_parameter("cluster_size_max", 100000.); - this->declare_parameter("cluster_merge_threshold", 1.5); - - lidarFrame_ = this->get_parameter("camera_frame").as_string(); - cameraFrame_ = this->get_parameter("lidar_frame").as_string(); + lidarFrame_ = this->get_parameter("lidar_frame").as_string(); + cameraFrame_ = this->get_parameter("camera_frame").as_string(); // setup pub subs camInfo_subscriber_ = this->create_subscription( @@ -58,23 +52,43 @@ TrackingNode::TrackingNode() : Node("dets_2d_3d"), transformInited_{false} , lid tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - // initialize common params used by utils - - // 0 => 0-15m d=0.5, 1 => 15-30 d=1, 2 => 30-45 d=1.6, 3 => 45-60 d=2.1, 4 => >60 d=2.6 - ProjectionUtils::clustering_distances_ = this->get_parameter("clustering_distances").as_double_array(); - ProjectionUtils::clustering_thresholds_ = this->get_parameter("clustering_thresholds").as_double_array(); - - ProjectionUtils::cluster_size_min_ = this->get_parameter("cluster_size_min").as_double(); - ProjectionUtils::cluster_size_max_ = this->get_parameter("cluster_size_max").as_double(); - ProjectionUtils::cluster_merge_threshold_ = this->get_parameter("cluster_merge_threshold").as_double();; + std::map clustering_params_map; + this->get_parameters("clustering_params", clustering_params_map); + clusteringParams = initializeClusteringParams(clustering_params_map); +} +std::map TrackingNode::initializeClusteringParams( + const std::map& clustering_params_map) +{ + std::map clusteringParams; + for(auto iter = clustering_params_map.begin(); iter != clustering_params_map.end(); ++iter) + { + std::string key = iter->first; + size_t split_loc = key.find_first_of('.'); + std::string det_type = key.substr(0, split_loc); + std::string field_type = key.substr(split_loc + 1); + + ClusteringParams params; + if (field_type == "clustering_distances") + params.clustering_distances = iter->second.as_double_array(); + else if (field_type == "clustering_thresholds") + params.clustering_thresholds = iter->second.as_double_array(); + else if (field_type == "cluster_size_min") + params.cluster_size_min = iter->second.as_double(); + else if (field_type == "cluster_size_max") + params.cluster_size_max = iter->second.as_double(); + else if (field_type == "cluster_merge_threshold") + params.cluster_merge_threshold = iter->second.as_double(); + + clusteringParams[det_type] = params; + } + return clusteringParams; } void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) { // save latest lidar info std::lock_guard guard_lidar(lidarCloud_mutex_); - sensor_msgs::msg::PointCloud2 pointCloud = *pointCloud_msg; pcl::fromROSMsg(pointCloud, *lidarCloud_); @@ -143,7 +157,7 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S } // clustering - auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes(inlierPoints); + auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes(inlierPoints, getDefaultOrValue(clusteringParams, det.results[0].hypothesis.class_id.c_str())); std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only std::vector allBBoxes = clusterAndBBoxes.second; diff --git a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp index 2ad65754..1ff2768c 100644 --- a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp @@ -20,12 +20,6 @@ typedef pcl::PointCloud ColorCloud; typedef std::shared_ptr ClusterPtr; typedef vision_msgs::msg::BoundingBox3D BBox3D; -std::vector ProjectionUtils::clustering_distances_; -std::vector ProjectionUtils::clustering_thresholds_; -double ProjectionUtils::cluster_size_min_; -double ProjectionUtils::cluster_size_max_; -double ProjectionUtils::cluster_merge_threshold_; - void ProjectionUtils::pointsInBbox( const Cloud::Ptr& inlierCloud, const Cloud::Ptr& lidarCloud, @@ -112,7 +106,9 @@ void ProjectionUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr extract.filter(*cloud_filtered); } -std::pair, std::vector> ProjectionUtils::getClusteredBBoxes(const Cloud::Ptr& lidarCloud) +std::pair, std::vector> ProjectionUtils::getClusteredBBoxes( + const Cloud::Ptr& lidarCloud, + const ClusteringParams& clusteringParams) { std::vector::Ptr> cloud_segments_array(5); for (size_t i=0; i, std::vector> ProjectionUtils::getClus for (const pcl::PointXYZ& pt : lidarCloud->points) { float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); - if (origin_distance < clustering_distances_[0]) + if (origin_distance < clusteringParams.clustering_distances[0]) cloud_segments_array[0]->points.push_back(pt); - else if (origin_distance < clustering_distances_[1]) + else if (origin_distance < clusteringParams.clustering_distances[1]) cloud_segments_array[1]->points.push_back(pt); - else if (origin_distance < clustering_distances_[2]) + else if (origin_distance < clusteringParams.clustering_distances[2]) cloud_segments_array[2]->points.push_back(pt); - else if (origin_distance < clustering_distances_[3]) + else if (origin_distance < clusteringParams.clustering_distances[3]) cloud_segments_array[3]->points.push_back(pt); else cloud_segments_array[4]->points.push_back(pt); @@ -142,16 +138,18 @@ std::pair, std::vector> ProjectionUtils::getClus for (unsigned int i = 1; i < cloud_segments_array.size(); i++) { // add clusters from each shell - std::vector local_clusters = clusterAndColor(cloud_segments_array[i], clustering_thresholds_[i]); + std::vector local_clusters = clusterAndColor(cloud_segments_array[i], + clusteringParams.clustering_thresholds[i], clusteringParams.cluster_size_min, + clusteringParams.cluster_size_max); all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end()); } // merge clusters if possible, do this twice? std::vector mid_clusters = (all_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge(all_clusters, cluster_merge_threshold_) + ? ProjectionUtils::checkAllForMerge(all_clusters, clusteringParams.cluster_merge_threshold) : all_clusters; std::vector final_clusters = (mid_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge(mid_clusters, cluster_merge_threshold_) + ? ProjectionUtils::checkAllForMerge(mid_clusters, clusteringParams.cluster_merge_threshold) : mid_clusters; // get boundingboxes for each & return all possible 3d bboxes (if valid) @@ -166,7 +164,7 @@ std::pair, std::vector> ProjectionUtils::getClus } std::vector ProjectionUtils::clusterAndColor( - const Cloud::Ptr& in_cloud_ptr, double in_max_cluster_distance) + const Cloud::Ptr& in_cloud_ptr, double in_max_cluster_distance, double cluster_size_min, double cluster_size_max) { std::vector clusters; if(in_cloud_ptr->size() == 0) return clusters; @@ -186,8 +184,8 @@ std::vector ProjectionUtils::clusterAndColor( // perform clustering on 2d cloud : https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(in_max_cluster_distance); - ec.setMinClusterSize(cluster_size_min_); - ec.setMaxClusterSize(cluster_size_max_); + ec.setMinClusterSize(cluster_size_min); + ec.setMaxClusterSize(cluster_size_max); ec.setSearchMethod(tree); ec.setInputCloud(cloud_2d); ec.extract(cluster_indices); From 88e575f5b1b7b1008e4f3ef61e6837ff53958f0a Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Tue, 4 Jun 2024 05:07:03 +0000 Subject: [PATCH 09/13] cleanup --- .../tracking/dets_2d_3d/config/nuscenes_config.yaml | 12 +++++++----- .../tracking/dets_2d_3d/include/dets_2d_3d_node.hpp | 1 - .../tracking/dets_2d_3d/src/dets_2d_3d_node.cpp | 12 +----------- 3 files changed, 8 insertions(+), 17 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml index ca9e3dae..04756127 100644 --- a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml +++ b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml @@ -11,8 +11,10 @@ camera_object_detection_node: camera_frame: CAM_FRONT lidar_frame: LIDAR_TOP - clustering_distances: [5., 30., 45., 60.] - clustering_thresholds: [0.5, 1.1, 1.6, 2.1, 2.6] - cluster_size_min: 20. - cluster_size_max: 100000. - cluster_merge_threshold: 1.5 + clustering_params: + car: + clustering_distances: [5., 30., 45., 60.] + clustering_thresholds: [0.5, 1.1, 1.6, 2.1, 2.6] + cluster_size_min: 20. + cluster_size_max: 100000. + cluster_merge_threshold: 1.5 diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index 349516e1..fa9dfe65 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -76,7 +76,6 @@ class TrackingNode : public rclcpp::Node std::map initializeClusteringParams( const std::map& clustering_params_map); - // should put somewhere else? template T getDefaultOrValue(std::map m, std::string key) { diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index 0b823e33..f23dcec8 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -14,17 +14,7 @@ TrackingNode::TrackingNode() : Node("dets_2d_3d", rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)), transformInited_{false} , lidarCloud_{new pcl::PointCloud()} { - // setup paramaters - // this->declare_parameter("camera_info_topic", "/CAM_FRONT/camera_info"); - // this->declare_parameter("lidar_topic", "/LIDAR_TOP"); - // this->declare_parameter("detections_topic", "/detections"); - - // this->declare_parameter("publish_detections_topic", "/detections_3d"); - // this->declare_parameter("publish_markers_topic", "/markers_3d"); - // this->declare_parameter("publish_clusters_topic", "/clustered_pc"); - - // this->declare_parameter("camera_frame", ""); - // this->declare_parameter("lidar_frame", ""); + rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); lidarFrame_ = this->get_parameter("lidar_frame").as_string(); cameraFrame_ = this->get_parameter("camera_frame").as_string(); From 417ae6ce2c0bda58860c31235c08c9c0987d21d1 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Sun, 9 Jun 2024 23:01:29 +0000 Subject: [PATCH 10/13] linter --- .../tracking/dets_2d_3d/include/cluster.hpp | 35 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 122 ++--- .../dets_2d_3d/include/projection_utils.hpp | 99 ++-- .../tracking/dets_2d_3d/src/cluster.cpp | 178 +++---- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 222 +++++---- .../dets_2d_3d/src/projection_utils.cpp | 452 +++++++++--------- 6 files changed, 571 insertions(+), 537 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/include/cluster.hpp b/src/perception/tracking/dets_2d_3d/include/cluster.hpp index 13bad5d4..b19c4241 100644 --- a/src/perception/tracking/dets_2d_3d/include/cluster.hpp +++ b/src/perception/tracking/dets_2d_3d/include/cluster.hpp @@ -1,34 +1,33 @@ #pragma once -#include #include +#include #include #include -class Cluster -{ +class Cluster { public: - // makes a cloud from all points `in_cloud_ptr` that are indexed by `in_cluster_indices` - Cluster( - const pcl::PointCloud::Ptr& in_cloud_ptr, - const std::vector& in_cluster_indices); + // makes a cloud from all points `in_cloud_ptr` that are indexed by + // `in_cluster_indices` + Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, + const std::vector &in_cluster_indices); - pcl::PointXYZ getCentroid() { return centroid_; } - pcl::PointCloud::Ptr getCloud() { return cloud_; } - vision_msgs::msg::BoundingBox3D getBoundingBox(); + pcl::PointXYZ getCentroid() { return centroid_; } + pcl::PointCloud::Ptr getCloud() { return cloud_; } + vision_msgs::msg::BoundingBox3D getBoundingBox(); - bool isValid(); - int size() { return cloud_->size(); } + bool isValid(); + int size() { return cloud_->size(); } private: - pcl::PointCloud::Ptr cloud_; - pcl::PointXYZ centroid_; + pcl::PointCloud::Ptr cloud_; + pcl::PointXYZ centroid_; - pcl::PointXYZ max_point_; - pcl::PointXYZ min_point_; + pcl::PointXYZ max_point_; + pcl::PointXYZ min_point_; - static int color_index; - static std::vector> colors; + static int color_index; + static std::vector> colors; }; diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index fa9dfe65..0f40db83 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -10,11 +10,11 @@ #include #include -#include #include +#include -#include #include +#include #include "cluster.hpp" #include "projection_utils.hpp" @@ -23,65 +23,67 @@ struct ClusteringParams; -class TrackingNode : public rclcpp::Node -{ +class TrackingNode : public rclcpp::Node { public: - TrackingNode(); + TrackingNode(); private: - std::map clusteringParams; - - // lidar and camera frame names - std::string lidarFrame_; - std::string cameraFrame_; - - // synchronization utils - std::mutex lidarCloud_mutex_; - - // camera information cached - bool transformInited_; - geometry_msgs::msg::TransformStamped transform_; - sensor_msgs::msg::CameraInfo::SharedPtr camInfo_; - pcl::PointCloud::Ptr lidarCloud_; - - // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection boxes - rclcpp::Subscription::SharedPtr camInfo_subscriber_; - rclcpp::Subscription::SharedPtr lidar_subscriber_; - rclcpp::Subscription::SharedPtr det_subscriber_; - - // publish the 3d detections - rclcpp::Publisher::SharedPtr det3d_publisher_; - rclcpp::Publisher::SharedPtr marker_publisher_; - rclcpp::Publisher::SharedPtr pc_publisher_; - rclcpp::Publisher::SharedPtr pc_publisher2_; - - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); - - int highestIOUScoredBBox( - const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D& detBBox, - const std::vector>& clusters); - double overlapBoundingBox( - const vision_msgs::msg::BoundingBox2D& bboxA, - const vision_msgs::msg::BoundingBox2D& bboxB); - double iouScore( - const vision_msgs::msg::BoundingBox2D& bboxA, - const vision_msgs::msg::BoundingBox2D& bboxB); - - std::map initializeClusteringParams( - const std::map& clustering_params_map); - - template - T getDefaultOrValue(std::map m, std::string key) - { - if (m.find(key) == m.end()) - return m[key]; - return m["default"]; - } - + std::map clusteringParams; + + // lidar and camera frame names + std::string lidarFrame_; + std::string cameraFrame_; + + // synchronization utils + std::mutex lidarCloud_mutex_; + + // camera information cached + bool transformInited_; + geometry_msgs::msg::TransformStamped transform_; + sensor_msgs::msg::CameraInfo::SharedPtr camInfo_; + pcl::PointCloud::Ptr lidarCloud_; + + // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection + // boxes + rclcpp::Subscription::SharedPtr + camInfo_subscriber_; + rclcpp::Subscription::SharedPtr + lidar_subscriber_; + rclcpp::Subscription::SharedPtr + det_subscriber_; + + // publish the 3d detections + rclcpp::Publisher::SharedPtr + det3d_publisher_; + rclcpp::Publisher::SharedPtr + marker_publisher_; + rclcpp::Publisher::SharedPtr pc_publisher_; + rclcpp::Publisher::SharedPtr pc_publisher2_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void + receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); + + int highestIOUScoredBBox( + const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D &detBBox, + const std::vector> &clusters); + double overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &bboxA, + const vision_msgs::msg::BoundingBox2D &bboxB); + double iouScore(const vision_msgs::msg::BoundingBox2D &bboxA, + const vision_msgs::msg::BoundingBox2D &bboxB); + + std::map initializeClusteringParams( + const std::map &clustering_params_map); + + template + T getDefaultOrValue(std::map m, std::string key) { + if (m.find(key) == m.end()) + return m[key]; + return m["default"]; + } }; diff --git a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp index 9410382c..c5e2f169 100644 --- a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp +++ b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp @@ -2,72 +2,75 @@ #include "cluster.hpp" -#include +#include +#include #include +#include #include #include -#include -#include -#include #include +#include -struct ClusteringParams -{ - // segment by distance - std::vector clustering_distances; - // Map of the nearest neighbor distance threshold for each segment, for each detection - std::vector clustering_thresholds; +struct ClusteringParams { + // segment by distance + std::vector clustering_distances; + // Map of the nearest neighbor distance threshold for each segment, for each + // detection + std::vector clustering_thresholds; - double cluster_size_min; - double cluster_size_max; - double cluster_merge_threshold; + double cluster_size_min; + double cluster_size_max; + double cluster_merge_threshold; }; // main helper functions used by the node -- should all be static -class ProjectionUtils -{ +class ProjectionUtils { public: - static void pointsInBbox( - const pcl::PointCloud::Ptr& inlierCloud, - const pcl::PointCloud::Ptr& lidarCloud, - const std::vector& projs2d, - const vision_msgs::msg::BoundingBox2D& bbox); + static void + pointsInBbox(const pcl::PointCloud::Ptr &inlierCloud, + const pcl::PointCloud::Ptr &lidarCloud, + const std::vector &projs2d, + const vision_msgs::msg::BoundingBox2D &bbox); - // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to camera frame - static std::optional projectLidarToCamera( - const geometry_msgs::msg::TransformStamped& transform, - const std::array& P, - const pcl::PointXYZ& pt); + // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to + // camera frame + static std::optional + projectLidarToCamera(const geometry_msgs::msg::TransformStamped &transform, + const std::array &P, + const pcl::PointXYZ &pt); - static bool isPointInBbox( - const geometry_msgs::msg::Point& pt, - const vision_msgs::msg::BoundingBox2D& bbox); + static bool isPointInBbox(const geometry_msgs::msg::Point &pt, + const vision_msgs::msg::BoundingBox2D &bbox); - static void removeFloor( - const pcl::PointCloud::Ptr& lidarCloud, - const pcl::PointCloud::Ptr& cloud_filtered); + static void + removeFloor(const pcl::PointCloud::Ptr &lidarCloud, + const pcl::PointCloud::Ptr &cloud_filtered); - static std::pair>, std::vector> getClusteredBBoxes( - const pcl::PointCloud::Ptr& lidarCloud, const ClusteringParams& clusteringParams); + static std::pair>, + std::vector> + getClusteredBBoxes(const pcl::PointCloud::Ptr &lidarCloud, + const ClusteringParams &clusteringParams); private: + static std::vector> + clusterAndColor(const pcl::PointCloud::Ptr &in_cloud_ptr, + double in_max_cluster_distance, double cluster_size_min, + double cluster_size_max); - static std::vector> clusterAndColor( - const pcl::PointCloud::Ptr& in_cloud_ptr, - double in_max_cluster_distance, double cluster_size_min, double cluster_size_max); - - static void checkClusterMerge( - size_t in_cluster_id, const std::vector> &in_clusters, - std::vector &in_out_visited_clusters, - std::vector &out_merge_indices, double in_merge_threshold); + static void + checkClusterMerge(size_t in_cluster_id, + const std::vector> &in_clusters, + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, + double in_merge_threshold); - static std::shared_ptr mergeClusters( - const std::vector>& in_clusters, - const std::vector& in_merge_indices, - std::vector& in_out_merged_clusters); + static std::shared_ptr + mergeClusters(const std::vector> &in_clusters, + const std::vector &in_merge_indices, + std::vector &in_out_merged_clusters); - static std::vector> checkAllForMerge( - const std::vector>& in_clusters, - float in_merge_threshold); + static std::vector> + checkAllForMerge(const std::vector> &in_clusters, + float in_merge_threshold); }; diff --git a/src/perception/tracking/dets_2d_3d/src/cluster.cpp b/src/perception/tracking/dets_2d_3d/src/cluster.cpp index e2b16717..69d3a9f1 100644 --- a/src/perception/tracking/dets_2d_3d/src/cluster.cpp +++ b/src/perception/tracking/dets_2d_3d/src/cluster.cpp @@ -1,15 +1,15 @@ #include "cluster.hpp" -#include +#include #include #include -#include +#include #include #include #include -#include #include +#include #include #include @@ -18,103 +18,105 @@ typedef pcl::PointCloud ClusterCloud; typedef vision_msgs::msg::BoundingBox3D BBox3D; int Cluster::color_index = 0; -std::vector> Cluster::colors = {{0,0,255}, {0,255,0}, {255,0,0}, {255,255,0}, {0,255,255}, {255,0,255}}; - -Cluster::Cluster( - const pcl::PointCloud::Ptr& in_cloud_ptr, - const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} -{ - bool indexesGiven = in_cluster_indices.size() > 0; - - size_t max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); - - min_point_.x = std::numeric_limits::max(); - min_point_.y = std::numeric_limits::max(); - min_point_.z = std::numeric_limits::max(); +std::vector> Cluster::colors = {{0, 0, 255}, {0, 255, 0}, + {255, 0, 0}, {255, 255, 0}, + {0, 255, 255}, {255, 0, 255}}; + +Cluster::Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, + const std::vector &in_cluster_indices) + : cloud_{new ClusterCloud()} { + bool indexesGiven = in_cluster_indices.size() > 0; + + size_t max_index = + (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + + min_point_.x = std::numeric_limits::max(); + min_point_.y = std::numeric_limits::max(); + min_point_.z = std::numeric_limits::max(); + + max_point_.x = -std::numeric_limits::max(); + max_point_.y = -std::numeric_limits::max(); + max_point_.z = -std::numeric_limits::max(); + + for (size_t i = 0; i < max_index; ++i) { + size_t idx = (indexesGiven) ? in_cluster_indices[i] : i; + + pcl::PointXYZRGB pt; + pt.x = in_cloud_ptr->points[idx].x; + pt.y = in_cloud_ptr->points[idx].y; + pt.z = in_cloud_ptr->points[idx].z; + pt.r = colors[color_index % colors.size()][0]; + pt.g = colors[color_index % colors.size()][1]; + pt.b = colors[color_index % colors.size()][2]; + + if (pt.x < min_point_.x) + min_point_.x = pt.x; + if (pt.y < min_point_.y) + min_point_.y = pt.y; + if (pt.z < min_point_.z) + min_point_.z = pt.z; + + if (pt.x > max_point_.x) + max_point_.x = pt.x; + if (pt.y > max_point_.y) + max_point_.y = pt.y; + if (pt.z > max_point_.z) + max_point_.z = pt.z; + + cloud_->emplace_back(pt); + } + + ++color_index; +} - max_point_.x = -std::numeric_limits::max(); - max_point_.y = -std::numeric_limits::max(); - max_point_.z = -std::numeric_limits::max(); +BBox3D Cluster::getBoundingBox() { + BBox3D bounding_box_; - for (size_t i=0; isize() == 0) + return bounding_box_; - pcl::PointXYZRGB pt; - pt.x = in_cloud_ptr->points[idx].x; - pt.y = in_cloud_ptr->points[idx].y; - pt.z = in_cloud_ptr->points[idx].z; - pt.r = colors[color_index % colors.size()][0]; - pt.g = colors[color_index % colors.size()][1]; - pt.b = colors[color_index % colors.size()][2]; + double length_ = max_point_.x - min_point_.x; + double width_ = max_point_.y - min_point_.y; + double height_ = max_point_.z - min_point_.z; - if (pt.x < min_point_.x) min_point_.x = pt.x; - if (pt.y < min_point_.y) min_point_.y = pt.y; - if (pt.z < min_point_.z) min_point_.z = pt.z; + bounding_box_.center.position.x = min_point_.x + length_ / 2; + bounding_box_.center.position.y = min_point_.y + width_ / 2; + bounding_box_.center.position.z = min_point_.z + height_ / 2; - if (pt.x > max_point_.x) max_point_.x = pt.x; - if (pt.y > max_point_.y) max_point_.y = pt.y; - if (pt.z > max_point_.z) max_point_.z = pt.z; + bounding_box_.size.x = ((length_ < 0) ? -1 * length_ : length_); + bounding_box_.size.y = ((width_ < 0) ? -1 * width_ : width_); + bounding_box_.size.z = ((height_ < 0) ? -1 * height_ : height_); + // pose estimation + double rz = 0; - cloud_->emplace_back(pt); + { + std::vector points; + for (unsigned int i = 0; i < cloud_->points.size(); i++) { + cv::Point2f pt; + pt.x = cloud_->points[i].x; + pt.y = cloud_->points[i].y; + points.push_back(pt); } - ++color_index; -} - -BBox3D Cluster::getBoundingBox() -{ - BBox3D bounding_box_; - - if (cloud_->size() == 0) return bounding_box_; - - double length_ = max_point_.x - min_point_.x; - double width_ = max_point_.y - min_point_.y; - double height_ = max_point_.z - min_point_.z; - - bounding_box_.center.position.x = min_point_.x + length_ / 2; - bounding_box_.center.position.y = min_point_.y + width_ / 2; - bounding_box_.center.position.z = min_point_.z + height_ / 2; - - bounding_box_.size.x = ((length_ < 0) ? -1 * length_ : length_); - bounding_box_.size.y = ((width_ < 0) ? -1 * width_ : width_); - bounding_box_.size.z = ((height_ < 0) ? -1 * height_ : height_); - - // pose estimation - double rz = 0; - - { - std::vector points; - for (unsigned int i = 0; i < cloud_->points.size(); i++) - { - cv::Point2f pt; - pt.x = cloud_->points[i].x; - pt.y = cloud_->points[i].y; - points.push_back(pt); - } - - cv::RotatedRect box = cv::minAreaRect(points); - rz = box.angle * 3.14 / 180; - bounding_box_.center.position.x = box.center.x; - bounding_box_.center.position.y = box.center.y; - bounding_box_.size.x = box.size.width; - bounding_box_.size.y = box.size.height; - } + cv::RotatedRect box = cv::minAreaRect(points); + rz = box.angle * 3.14 / 180; + bounding_box_.center.position.x = box.center.x; + bounding_box_.center.position.y = box.center.y; + bounding_box_.size.x = box.size.width; + bounding_box_.size.y = box.size.height; + } - // set bounding box direction - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, rz); + // set bounding box direction + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, rz); - geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(quat); - bounding_box_.center.orientation = msg_quat; + geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(quat); + bounding_box_.center.orientation = msg_quat; - std::cout << cv::getBuildInformation() << std::endl; + std::cout << cv::getBuildInformation() << std::endl; - return bounding_box_; + return bounding_box_; } -bool Cluster::isValid() -{ - return cloud_->size() > 0; -} +bool Cluster::isValid() { return cloud_->size() > 0; } diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index f23dcec8..d5ef416a 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -1,8 +1,8 @@ #include "dets_2d_3d_node.hpp" +#include #include #include -#include #include #include @@ -10,35 +10,43 @@ #include #include - -TrackingNode::TrackingNode() : Node("dets_2d_3d", rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)), - transformInited_{false} , lidarCloud_{new pcl::PointCloud()} -{ - rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); +TrackingNode::TrackingNode() + : Node("dets_2d_3d", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)), + transformInited_{false}, lidarCloud_{ + new pcl::PointCloud()} { + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); lidarFrame_ = this->get_parameter("lidar_frame").as_string(); cameraFrame_ = this->get_parameter("camera_frame").as_string(); // setup pub subs camInfo_subscriber_ = this->create_subscription( - this->get_parameter("camera_info_topic").as_string(), 10, - std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); + this->get_parameter("camera_info_topic").as_string(), 10, + std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); lidar_subscriber_ = this->create_subscription( - this->get_parameter("lidar_topic").as_string(), 10, - std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); + this->get_parameter("lidar_topic").as_string(), 10, + std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); - det_subscriber_ = this->create_subscription( - this->get_parameter("detections_topic").as_string(), 10, - std::bind(&TrackingNode::receiveDetections, this, std::placeholders::_1)); + det_subscriber_ = + this->create_subscription( + this->get_parameter("detections_topic").as_string(), 10, + std::bind(&TrackingNode::receiveDetections, this, + std::placeholders::_1)); det3d_publisher_ = this->create_publisher( - this->get_parameter("publish_detections_topic").as_string(), 10); - marker_publisher_ = this->create_publisher( - this->get_parameter("publish_markers_topic").as_string(), 10); + this->get_parameter("publish_detections_topic").as_string(), 10); + marker_publisher_ = + this->create_publisher( + this->get_parameter("publish_markers_topic").as_string(), 10); pc_publisher_ = this->create_publisher( - this->get_parameter("publish_clusters_topic").as_string(), 10); - + this->get_parameter("publish_clusters_topic").as_string(), 10); + tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -47,12 +55,12 @@ TrackingNode::TrackingNode() : Node("dets_2d_3d", rclcpp::NodeOptions().allow_un clusteringParams = initializeClusteringParams(clustering_params_map); } -std::map TrackingNode::initializeClusteringParams( - const std::map& clustering_params_map) -{ +std::map +TrackingNode::initializeClusteringParams( + const std::map &clustering_params_map) { std::map clusteringParams; - for(auto iter = clustering_params_map.begin(); iter != clustering_params_map.end(); ++iter) - { + for (auto iter = clustering_params_map.begin(); + iter != clustering_params_map.end(); ++iter) { std::string key = iter->first; size_t split_loc = key.find_first_of('.'); std::string det_type = key.substr(0, split_loc); @@ -75,8 +83,8 @@ std::map TrackingNode::initializeClusteringParams return clusteringParams; } -void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) -{ +void TrackingNode::receiveLidar( + const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) { // save latest lidar info std::lock_guard guard_lidar(lidarCloud_mutex_); @@ -84,89 +92,103 @@ void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr p pcl::fromROSMsg(pointCloud, *lidarCloud_); } -void TrackingNode::readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) -{ +void TrackingNode::readCameraInfo( + const sensor_msgs::msg::CameraInfo::SharedPtr msg) { std::cout << msg->distortion_model << std::endl; camInfo_ = msg; camInfo_subscriber_.reset(); } -void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg) -{ - if (!transformInited_) - { +void TrackingNode::receiveDetections( + const vision_msgs::msg::Detection2DArray::SharedPtr msg) { + if (!transformInited_) { try { - transform_ = tf_buffer_ ->lookupTransform(cameraFrame_, lidarFrame_, msg->header.stamp); + transform_ = tf_buffer_->lookupTransform(cameraFrame_, lidarFrame_, + msg->header.stamp); transformInited_ = true; - } catch (const tf2::TransformException & ex) { + } catch (const tf2::TransformException &ex) { RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); return; } } // remove floor from lidar cloud - pcl::PointCloud::Ptr lidarCloudNoFloor(new pcl::PointCloud()); + pcl::PointCloud::Ptr lidarCloudNoFloor( + new pcl::PointCloud()); { std::lock_guard guard_lidar(lidarCloud_mutex_); ProjectionUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); - if (lidarCloudNoFloor->size() == 0) return; // do not process more, if all the points are floor points - RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", lidarCloud_->size(), lidarCloudNoFloor->size()); + if (lidarCloudNoFloor->size() == 0) + return; // do not process more, if all the points are floor points + RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", + lidarCloud_->size(), lidarCloudNoFloor->size()); } // transform all lidar pts to 2d points in the camera std::vector lidar2dProjs; - pcl::PointCloud::Ptr inCameraPoints(new pcl::PointCloud()); - for (const pcl::PointXYZ& pt : lidarCloudNoFloor->points) - { - std::optional proj = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, pt); - if (proj) - { + pcl::PointCloud::Ptr inCameraPoints( + new pcl::PointCloud()); + for (const pcl::PointXYZ &pt : lidarCloudNoFloor->points) { + std::optional proj = + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, pt); + if (proj) { inCameraPoints->emplace_back(pt); lidar2dProjs.emplace_back(*proj); } } - + // publish both rviz markers (for visualization) and a detection array visualization_msgs::msg::MarkerArray markerArray3d; vision_msgs::msg::Detection3DArray detArray3d; - pcl::PointCloud mergedClusters; // coloured pc to visualize points in each cluster + pcl::PointCloud + mergedClusters; // coloured pc to visualize points in each cluster // process each detection in det array int bboxId = 0; - for (const vision_msgs::msg::Detection2D& det : msg->detections) - { + for (const vision_msgs::msg::Detection2D &det : msg->detections) { vision_msgs::msg::BoundingBox2D bbox = det.bbox; // process lidarcloud with extrinsic trans from lidar to cam frame - pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); - ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, bbox); + pcl::PointCloud::Ptr inlierPoints( + new pcl::PointCloud()); + ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, + bbox); if (inlierPoints->size() == 0) { - RCLCPP_INFO(this->get_logger(), "no inliers found for detection %s", det.results[0].hypothesis.class_id.c_str()); + RCLCPP_INFO(this->get_logger(), "no inliers found for detection %s", + det.results[0].hypothesis.class_id.c_str()); continue; } // clustering - auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes(inlierPoints, getDefaultOrValue(clusteringParams, det.results[0].hypothesis.class_id.c_str())); - std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only - std::vector allBBoxes = clusterAndBBoxes.second; - - if (clusters.size() == 0 || allBBoxes.size() == 0) continue; + auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes( + inlierPoints, + getDefaultOrValue( + clusteringParams, det.results[0].hypothesis.class_id.c_str())); + std::vector> clusters = + clusterAndBBoxes.first; // needed? for viz purposes only + std::vector allBBoxes = + clusterAndBBoxes.second; + + if (clusters.size() == 0 || allBBoxes.size() == 0) + continue; - // score each 3d bbox based on iou with teh original 2d bbox & the density of points in the cluster + // score each 3d bbox based on iou with teh original 2d bbox & the density + // of points in the cluster int bestIndex = highestIOUScoredBBox(allBBoxes, bbox, clusters); vision_msgs::msg::BoundingBox3D bestBBox = allBBoxes[bestIndex]; - // for visauzliation : adds detection to cluster pc visualization & the marker array & the 3d detection array + // for visauzliation : adds detection to cluster pc visualization & the + // marker array & the 3d detection array mergedClusters += *(clusters[bestIndex]->getCloud()); vision_msgs::msg::Detection3D det3d; - det3d.bbox = bestBBox; + det3d.bbox = bestBBox; det3d.results = det.results; detArray3d.detections.emplace_back(det3d); visualization_msgs::msg::Marker marker; - marker.type = visualization_msgs::msg::Marker::CUBE; + marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale = bestBBox.size; marker.pose = bestBBox.center; marker.header.frame_id = lidarFrame_; @@ -186,40 +208,43 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S pc_publisher_->publish(pubCloud); - RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", markerArray3d.markers.size()); - + RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", + markerArray3d.markers.size()); } int TrackingNode::highestIOUScoredBBox( - const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D& detBBox, - const std::vector>& clusters) -{ + const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D &detBBox, + const std::vector> &clusters) { int bestScore = 0; int bestBBox = 0; - for (size_t i=0; i top_left2d = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); - std::optional bottom_right2d = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); + std::optional top_left2d = + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, + top_left); + std::optional bottom_right2d = + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, + bottom_right); - if (!top_left2d || !bottom_right2d) return 0; + if (!top_left2d || !bottom_right2d) + return 0; vision_msgs::msg::BoundingBox2D bbox2d; - bbox2d.center.position.x = ((top_left2d->x) + (bottom_right2d->x))/2; - bbox2d.center.position.y = ((top_left2d->y) + (bottom_right2d->y))/2; + bbox2d.center.position.x = ((top_left2d->x) + (bottom_right2d->x)) / 2; + bbox2d.center.position.y = ((top_left2d->y) + (bottom_right2d->y)) / 2; bbox2d.size_x = abs(top_left2d->x - bottom_right2d->x); bbox2d.size_y = abs(top_left2d->y - bottom_right2d->y); @@ -229,8 +254,7 @@ int TrackingNode::highestIOUScoredBBox( double density = clusters[i]->size() / (b.size.x * b.size.y * b.size.z); double score = iou + density; - if (score > bestScore) - { + if (score > bestScore) { bestScore = score; bestBBox = i; } @@ -238,30 +262,34 @@ int TrackingNode::highestIOUScoredBBox( return bestBBox; } -double TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D& boxA, const vision_msgs::msg::BoundingBox2D& boxB) -{ - double overlapHeight = - std::min(boxA.center.position.y + boxA.size_y/2, boxB.center.position.y + boxB.size_y/2) - - std::max(boxA.center.position.y - boxA.size_y/2, boxB.center.position.y - boxB.size_y/2); - if (overlapHeight <= 0) return 0; - - double overlapWidth = std::min(boxA.center.position.x + boxA.size_x/2, boxB.center.position.x + boxB.size_x/2) - - std::max(boxA.center.position.x - boxA.size_x/2, boxB.center.position.x - boxB.size_x/2); - if (overlapWidth <= 0) return 0; - +double +TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &boxA, + const vision_msgs::msg::BoundingBox2D &boxB) { + double overlapHeight = std::min(boxA.center.position.y + boxA.size_y / 2, + boxB.center.position.y + boxB.size_y / 2) - + std::max(boxA.center.position.y - boxA.size_y / 2, + boxB.center.position.y - boxB.size_y / 2); + if (overlapHeight <= 0) + return 0; + + double overlapWidth = std::min(boxA.center.position.x + boxA.size_x / 2, + boxB.center.position.x + boxB.size_x / 2) - + std::max(boxA.center.position.x - boxA.size_x / 2, + boxB.center.position.x - boxB.size_x / 2); + if (overlapWidth <= 0) + return 0; + return overlapHeight * overlapWidth; } -double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB) -{ +double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D &bboxA, + const vision_msgs::msg::BoundingBox2D &bboxB) { double overlap = overlapBoundingBox(bboxA, bboxB); - return (overlap / - (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - overlap)); - + return (overlap / (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - + overlap)); } -int main(int argc, char ** argv) -{ +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp index 1ff2768c..fa1f29d3 100644 --- a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp @@ -1,14 +1,14 @@ #include "projection_utils.hpp" -#include -#include #include +#include +#include +#include #include #include #include #include -#include #include #include @@ -21,262 +21,262 @@ typedef std::shared_ptr ClusterPtr; typedef vision_msgs::msg::BoundingBox3D BBox3D; void ProjectionUtils::pointsInBbox( - const Cloud::Ptr& inlierCloud, - const Cloud::Ptr& lidarCloud, - const std::vector& projs2d, - const vision_msgs::msg::BoundingBox2D& bbox) -{ - for (size_t i=0; ipush_back(lidarCloud->points[i]); - } + const Cloud::Ptr &inlierCloud, const Cloud::Ptr &lidarCloud, + const std::vector &projs2d, + const vision_msgs::msg::BoundingBox2D &bbox) { + for (size_t i = 0; i < projs2d.size(); ++i) { + // P * [x y z 1]^T, P is row major + if (isPointInBbox(projs2d[i], bbox)) + inlierCloud->push_back(lidarCloud->points[i]); + } } - -bool ProjectionUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) -{ - double padding = 0; - - if (bbox.center.position.x - bbox.size_x/2 - padding < pt.x && pt.x < bbox.center.position.x + bbox.size_x/2 + padding - && bbox.center.position.y - bbox.size_y/2 - padding < pt.y && pt.y < bbox.center.position.y + bbox.size_y/2 + padding) - { - return true; - } - return false; +bool ProjectionUtils::isPointInBbox( + const geometry_msgs::msg::Point &pt, + const vision_msgs::msg::BoundingBox2D &bbox) { + double padding = 0; + + if (bbox.center.position.x - bbox.size_x / 2 - padding < pt.x && + pt.x < bbox.center.position.x + bbox.size_x / 2 + padding && + bbox.center.position.y - bbox.size_y / 2 - padding < pt.y && + pt.y < bbox.center.position.y + bbox.size_y / 2 + padding) { + return true; + } + return false; } std::optional ProjectionUtils::projectLidarToCamera( - const geometry_msgs::msg::TransformStamped& transform, - const std::array& p, - const pcl::PointXYZ& pt) -{ - // lidar to camera frame - auto trans_pt = geometry_msgs::msg::Point(); - auto orig_pt = geometry_msgs::msg::Point(); - orig_pt.x = pt.x; - orig_pt.y = pt.y; - orig_pt.z = pt.z; - - tf2::doTransform(orig_pt, trans_pt, transform); - - if (trans_pt.z < 1) return std::nullopt; - - // camera frame to camera 2D projection - double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; - double v = p[4] * trans_pt.x + p[5] * trans_pt.y + p[6] * trans_pt.z + p[7]; - double w = p[8] * trans_pt.x + p[9] * trans_pt.y + p[10] * trans_pt.z + p[11]; - - auto proj_pt = geometry_msgs::msg::Point(); - proj_pt.x = u/w; - proj_pt.y = v/w; - - // check if inside camera frame bounds - if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) return proj_pt; + const geometry_msgs::msg::TransformStamped &transform, + const std::array &p, const pcl::PointXYZ &pt) { + // lidar to camera frame + auto trans_pt = geometry_msgs::msg::Point(); + auto orig_pt = geometry_msgs::msg::Point(); + orig_pt.x = pt.x; + orig_pt.y = pt.y; + orig_pt.z = pt.z; + + tf2::doTransform(orig_pt, trans_pt, transform); + + if (trans_pt.z < 1) return std::nullopt; -} -// https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html -void ProjectionUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud_filtered) -{ - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - - // Create the filtering object - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setMaxIterations(100); - seg.setAxis(Eigen::Vector3f(0, 0, 1)); - seg.setEpsAngle(0.1); - seg.setDistanceThreshold(0.5); // floor distance - seg.setOptimizeCoefficients(true); - seg.setInputCloud(lidarCloud); - seg.segment(*inliers, *coefficients); - - // Create the filtering object - pcl::ExtractIndices extract; - extract.setInputCloud(lidarCloud); - extract.setIndices(inliers); - - // Extract non-ground returns - extract.setNegative (true); - extract.filter(*cloud_filtered); -} + // camera frame to camera 2D projection + double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; + double v = p[4] * trans_pt.x + p[5] * trans_pt.y + p[6] * trans_pt.z + p[7]; + double w = p[8] * trans_pt.x + p[9] * trans_pt.y + p[10] * trans_pt.z + p[11]; -std::pair, std::vector> ProjectionUtils::getClusteredBBoxes( - const Cloud::Ptr& lidarCloud, - const ClusteringParams& clusteringParams) -{ - std::vector::Ptr> cloud_segments_array(5); - for (size_t i=0; i::Ptr temp(new pcl::PointCloud()); - cloud_segments_array[i] = temp; - } + auto proj_pt = geometry_msgs::msg::Point(); + proj_pt.x = u / w; + proj_pt.y = v / w; - // segment points into spherical shells from point cloud origin - for (const pcl::PointXYZ& pt : lidarCloud->points) - { - float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); - if (origin_distance < clusteringParams.clustering_distances[0]) - cloud_segments_array[0]->points.push_back(pt); - else if (origin_distance < clusteringParams.clustering_distances[1]) - cloud_segments_array[1]->points.push_back(pt); - else if (origin_distance < clusteringParams.clustering_distances[2]) - cloud_segments_array[2]->points.push_back(pt); - else if (origin_distance < clusteringParams.clustering_distances[3]) - cloud_segments_array[3]->points.push_back(pt); - else - cloud_segments_array[4]->points.push_back(pt); - } + // check if inside camera frame bounds + if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) + return proj_pt; + return std::nullopt; +} - // get largest cluster in each shell - std::vector all_clusters; - for (unsigned int i = 1; i < cloud_segments_array.size(); i++) - { - // add clusters from each shell - std::vector local_clusters = clusterAndColor(cloud_segments_array[i], - clusteringParams.clustering_thresholds[i], clusteringParams.cluster_size_min, - clusteringParams.cluster_size_max); - all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end()); - } +// https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html +void ProjectionUtils::removeFloor(const Cloud::Ptr &lidarCloud, + const Cloud::Ptr &cloud_filtered) { + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + + // Create the filtering object + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(100); + seg.setAxis(Eigen::Vector3f(0, 0, 1)); + seg.setEpsAngle(0.1); + seg.setDistanceThreshold(0.5); // floor distance + seg.setOptimizeCoefficients(true); + seg.setInputCloud(lidarCloud); + seg.segment(*inliers, *coefficients); + + // Create the filtering object + pcl::ExtractIndices extract; + extract.setInputCloud(lidarCloud); + extract.setIndices(inliers); + + // Extract non-ground returns + extract.setNegative(true); + extract.filter(*cloud_filtered); +} - // merge clusters if possible, do this twice? - std::vector mid_clusters = (all_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge(all_clusters, clusteringParams.cluster_merge_threshold) - : all_clusters; - std::vector final_clusters = (mid_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge(mid_clusters, clusteringParams.cluster_merge_threshold) - : mid_clusters; - - // get boundingboxes for each & return all possible 3d bboxes (if valid) - std::vector bboxes; - for (const ClusterPtr& cluster : final_clusters) - { - BBox3D b = cluster->getBoundingBox(); - if (cluster->isValid()) - bboxes.emplace_back(b); - } - return std::pair, std::vector>{final_clusters, bboxes}; +std::pair, std::vector> +ProjectionUtils::getClusteredBBoxes(const Cloud::Ptr &lidarCloud, + const ClusteringParams &clusteringParams) { + std::vector::Ptr> cloud_segments_array(5); + for (size_t i = 0; i < cloud_segments_array.size(); ++i) { + pcl::PointCloud::Ptr temp( + new pcl::PointCloud()); + cloud_segments_array[i] = temp; + } + + // segment points into spherical shells from point cloud origin + for (const pcl::PointXYZ &pt : lidarCloud->points) { + float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); + if (origin_distance < clusteringParams.clustering_distances[0]) + cloud_segments_array[0]->points.push_back(pt); + else if (origin_distance < clusteringParams.clustering_distances[1]) + cloud_segments_array[1]->points.push_back(pt); + else if (origin_distance < clusteringParams.clustering_distances[2]) + cloud_segments_array[2]->points.push_back(pt); + else if (origin_distance < clusteringParams.clustering_distances[3]) + cloud_segments_array[3]->points.push_back(pt); + else + cloud_segments_array[4]->points.push_back(pt); + } + + // get largest cluster in each shell + std::vector all_clusters; + for (unsigned int i = 1; i < cloud_segments_array.size(); i++) { + // add clusters from each shell + std::vector local_clusters = clusterAndColor( + cloud_segments_array[i], clusteringParams.clustering_thresholds[i], + clusteringParams.cluster_size_min, clusteringParams.cluster_size_max); + all_clusters.insert(all_clusters.end(), local_clusters.begin(), + local_clusters.end()); + } + + // merge clusters if possible, do this twice? + std::vector mid_clusters = + (all_clusters.size() > 0) + ? ProjectionUtils::checkAllForMerge( + all_clusters, clusteringParams.cluster_merge_threshold) + : all_clusters; + std::vector final_clusters = + (mid_clusters.size() > 0) + ? ProjectionUtils::checkAllForMerge( + mid_clusters, clusteringParams.cluster_merge_threshold) + : mid_clusters; + + // get boundingboxes for each & return all possible 3d bboxes (if valid) + std::vector bboxes; + for (const ClusterPtr &cluster : final_clusters) { + BBox3D b = cluster->getBoundingBox(); + if (cluster->isValid()) + bboxes.emplace_back(b); + } + return std::pair, std::vector>{final_clusters, + bboxes}; } std::vector ProjectionUtils::clusterAndColor( - const Cloud::Ptr& in_cloud_ptr, double in_max_cluster_distance, double cluster_size_min, double cluster_size_max) -{ - std::vector clusters; - if(in_cloud_ptr->size() == 0) return clusters; - - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - - // create 2d pc, by copying & making it flat - Cloud::Ptr cloud_2d(new pcl::PointCloud()); - pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d); - for (size_t i = 0; i < cloud_2d->points.size(); i++) - cloud_2d->points[i].z = 0; - - tree->setInputCloud(cloud_2d); - - std::vector cluster_indices; - - // perform clustering on 2d cloud : https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(in_max_cluster_distance); - ec.setMinClusterSize(cluster_size_min); - ec.setMaxClusterSize(cluster_size_max); - ec.setSearchMethod(tree); - ec.setInputCloud(cloud_2d); - ec.extract(cluster_indices); - - - // add pts at clustered indexes to cluster - for (const auto& cluster : cluster_indices) - { - ClusterPtr cloud_cluster = std::make_shared(in_cloud_ptr, cluster.indices); - clusters.emplace_back(cloud_cluster); - } - + const Cloud::Ptr &in_cloud_ptr, double in_max_cluster_distance, + double cluster_size_min, double cluster_size_max) { + std::vector clusters; + if (in_cloud_ptr->size() == 0) return clusters; -} + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree()); + + // create 2d pc, by copying & making it flat + Cloud::Ptr cloud_2d(new pcl::PointCloud()); + pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d); + for (size_t i = 0; i < cloud_2d->points.size(); i++) + cloud_2d->points[i].z = 0; + + tree->setInputCloud(cloud_2d); + + std::vector cluster_indices; + + // perform clustering on 2d cloud : + // https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(in_max_cluster_distance); + ec.setMinClusterSize(cluster_size_min); + ec.setMaxClusterSize(cluster_size_max); + ec.setSearchMethod(tree); + ec.setInputCloud(cloud_2d); + ec.extract(cluster_indices); + + // add pts at clustered indexes to cluster + for (const auto &cluster : cluster_indices) { + ClusterPtr cloud_cluster = + std::make_shared(in_cloud_ptr, cluster.indices); + clusters.emplace_back(cloud_cluster); + } + + return clusters; +} void ProjectionUtils::checkClusterMerge( size_t in_cluster_id, const std::vector &in_clusters, - std::vector& in_out_visited_clusters, - std::vector& out_merge_indices, double in_merge_threshold) -{ - - pcl::PointXYZ point_a = in_clusters[in_cluster_id]->getCentroid(); - - for (size_t i = 0; i < in_clusters.size(); i++) { - if (i != in_cluster_id && !in_out_visited_clusters[i]) - { - pcl::PointXYZ point_b = in_clusters[i]->getCentroid(); - double distance = sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); - - if (distance <= in_merge_threshold) - { - in_out_visited_clusters[i] = true; - out_merge_indices.push_back(i); - // look for all other clusters that can be merged with this merge-able cluster - checkClusterMerge(i, in_clusters, in_out_visited_clusters, out_merge_indices, in_merge_threshold); - } - } + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, double in_merge_threshold) { + + pcl::PointXYZ point_a = in_clusters[in_cluster_id]->getCentroid(); + + for (size_t i = 0; i < in_clusters.size(); i++) { + if (i != in_cluster_id && !in_out_visited_clusters[i]) { + pcl::PointXYZ point_b = in_clusters[i]->getCentroid(); + double distance = + sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); + + if (distance <= in_merge_threshold) { + in_out_visited_clusters[i] = true; + out_merge_indices.push_back(i); + // look for all other clusters that can be merged with this merge-able + // cluster + checkClusterMerge(i, in_clusters, in_out_visited_clusters, + out_merge_indices, in_merge_threshold); + } } + } } +ClusterPtr +ProjectionUtils::mergeClusters(const std::vector &in_clusters, + const std::vector &in_merge_indices, + std::vector &in_out_merged_clusters) { + ColorCloud merged_cloud; -ClusterPtr ProjectionUtils::mergeClusters( - const std::vector& in_clusters, - const std::vector& in_merge_indices, - std::vector &in_out_merged_clusters) -{ - ColorCloud merged_cloud; - - // for each cluster in merge cloud indices, merge into larger cloud - for (size_t i = 0; i < in_merge_indices.size(); i++) - { - merged_cloud += *(in_clusters[in_merge_indices[i]]->getCloud()); - in_out_merged_clusters[in_merge_indices[i]] = true; - } + // for each cluster in merge cloud indices, merge into larger cloud + for (size_t i = 0; i < in_merge_indices.size(); i++) { + merged_cloud += *(in_clusters[in_merge_indices[i]]->getCloud()); + in_out_merged_clusters[in_merge_indices[i]] = true; + } - Cloud merged_cloud_uncoloured; - pcl::copyPointCloud(merged_cloud, merged_cloud_uncoloured); + Cloud merged_cloud_uncoloured; + pcl::copyPointCloud(merged_cloud, merged_cloud_uncoloured); - Cloud::Ptr merged_cloud_ptr(new Cloud(merged_cloud_uncoloured)); + Cloud::Ptr merged_cloud_ptr(new Cloud(merged_cloud_uncoloured)); - std::vector temp; - ClusterPtr merged_cluster = std::make_shared(merged_cloud_ptr, temp); - return merged_cluster; + std::vector temp; + ClusterPtr merged_cluster = std::make_shared(merged_cloud_ptr, temp); + return merged_cluster; } -std::vector ProjectionUtils::checkAllForMerge( - const std::vector& in_clusters, float in_merge_threshold) -{ - std::vector out_clusters; +std::vector +ProjectionUtils::checkAllForMerge(const std::vector &in_clusters, + float in_merge_threshold) { + std::vector out_clusters; - std::vector visited_clusters(in_clusters.size(), false); - std::vector merged_clusters(in_clusters.size(), false); + std::vector visited_clusters(in_clusters.size(), false); + std::vector merged_clusters(in_clusters.size(), false); - for (size_t i = 0; i < in_clusters.size(); i++) { - if (!visited_clusters[i]) { - visited_clusters[i] = true; + for (size_t i = 0; i < in_clusters.size(); i++) { + if (!visited_clusters[i]) { + visited_clusters[i] = true; - std::vector merge_indices; - checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold); - ClusterPtr mergedCluster = mergeClusters(in_clusters, merge_indices, merged_clusters); + std::vector merge_indices; + checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, + in_merge_threshold); + ClusterPtr mergedCluster = + mergeClusters(in_clusters, merge_indices, merged_clusters); - out_clusters.emplace_back(mergedCluster); - } + out_clusters.emplace_back(mergedCluster); } - for (size_t i = 0; i < in_clusters.size(); i++) { - // check for clusters not merged, add them to the output - if (!merged_clusters[i]) { - out_clusters.push_back(in_clusters[i]); - } + } + for (size_t i = 0; i < in_clusters.size(); i++) { + // check for clusters not merged, add them to the output + if (!merged_clusters[i]) { + out_clusters.push_back(in_clusters[i]); } + } - return out_clusters; + return out_clusters; } From 5ba2255d4bd294e2e139c90a6e2c39382a308cac Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Mon, 10 Jun 2024 01:57:15 +0000 Subject: [PATCH 11/13] linter --- .../dets_2d_3d/config/nuscenes_config.yaml | 2 +- .../tracking/dets_2d_3d/include/cluster.hpp | 4 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 32 ++--- .../dets_2d_3d/include/projection_utils.hpp | 58 ++++---- .../tracking/dets_2d_3d/src/cluster.cpp | 31 ++--- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 126 +++++++----------- .../dets_2d_3d/src/projection_utils.cpp | 123 +++++++---------- 7 files changed, 151 insertions(+), 225 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml index 04756127..4bd841da 100644 --- a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml +++ b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml @@ -1,4 +1,4 @@ -camera_object_detection_node: +dets_2d_3d_node: ros__parameters: camera_info_topic: /CAM_FRONT/camera_info lidar_topic: /LIDAR_TOP diff --git a/src/perception/tracking/dets_2d_3d/include/cluster.hpp b/src/perception/tracking/dets_2d_3d/include/cluster.hpp index b19c4241..09d620b4 100644 --- a/src/perception/tracking/dets_2d_3d/include/cluster.hpp +++ b/src/perception/tracking/dets_2d_3d/include/cluster.hpp @@ -8,7 +8,7 @@ #include class Cluster { -public: + public: // makes a cloud from all points `in_cloud_ptr` that are indexed by // `in_cluster_indices` Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, @@ -21,7 +21,7 @@ class Cluster { bool isValid(); int size() { return cloud_->size(); } -private: + private: pcl::PointCloud::Ptr cloud_; pcl::PointXYZ centroid_; diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index 0f40db83..9d1a9d0b 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -24,10 +24,10 @@ struct ClusteringParams; class TrackingNode : public rclcpp::Node { -public: + public: TrackingNode(); -private: + private: std::map clusteringParams; // lidar and camera frame names @@ -45,18 +45,13 @@ class TrackingNode : public rclcpp::Node { // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection // boxes - rclcpp::Subscription::SharedPtr - camInfo_subscriber_; - rclcpp::Subscription::SharedPtr - lidar_subscriber_; - rclcpp::Subscription::SharedPtr - det_subscriber_; + rclcpp::Subscription::SharedPtr camInfo_subscriber_; + rclcpp::Subscription::SharedPtr lidar_subscriber_; + rclcpp::Subscription::SharedPtr det_subscriber_; // publish the 3d detections - rclcpp::Publisher::SharedPtr - det3d_publisher_; - rclcpp::Publisher::SharedPtr - marker_publisher_; + rclcpp::Publisher::SharedPtr det3d_publisher_; + rclcpp::Publisher::SharedPtr marker_publisher_; rclcpp::Publisher::SharedPtr pc_publisher_; rclcpp::Publisher::SharedPtr pc_publisher2_; @@ -65,13 +60,11 @@ class TrackingNode : public rclcpp::Node { void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg); void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void - receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); + void receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); - int highestIOUScoredBBox( - const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D &detBBox, - const std::vector> &clusters); + int highestIOUScoredBBox(const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D &detBBox, + const std::vector> &clusters); double overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &bboxA, const vision_msgs::msg::BoundingBox2D &bboxB); double iouScore(const vision_msgs::msg::BoundingBox2D &bboxA, @@ -82,8 +75,7 @@ class TrackingNode : public rclcpp::Node { template T getDefaultOrValue(std::map m, std::string key) { - if (m.find(key) == m.end()) - return m[key]; + if (m.find(key) == m.end()) return m[key]; return m["default"]; } }; diff --git a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp index c5e2f169..6c973312 100644 --- a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp +++ b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp @@ -2,10 +2,10 @@ #include "cluster.hpp" -#include -#include #include #include +#include +#include #include #include @@ -26,51 +26,43 @@ struct ClusteringParams { // main helper functions used by the node -- should all be static class ProjectionUtils { -public: - static void - pointsInBbox(const pcl::PointCloud::Ptr &inlierCloud, - const pcl::PointCloud::Ptr &lidarCloud, - const std::vector &projs2d, - const vision_msgs::msg::BoundingBox2D &bbox); + public: + static void pointsInBbox(const pcl::PointCloud::Ptr &inlierCloud, + const pcl::PointCloud::Ptr &lidarCloud, + const std::vector &projs2d, + const vision_msgs::msg::BoundingBox2D &bbox); // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to // camera frame - static std::optional - projectLidarToCamera(const geometry_msgs::msg::TransformStamped &transform, - const std::array &P, - const pcl::PointXYZ &pt); + static std::optional projectLidarToCamera( + const geometry_msgs::msg::TransformStamped &transform, const std::array &P, + const pcl::PointXYZ &pt); static bool isPointInBbox(const geometry_msgs::msg::Point &pt, const vision_msgs::msg::BoundingBox2D &bbox); - static void - removeFloor(const pcl::PointCloud::Ptr &lidarCloud, - const pcl::PointCloud::Ptr &cloud_filtered); + static void removeFloor(const pcl::PointCloud::Ptr &lidarCloud, + const pcl::PointCloud::Ptr &cloud_filtered); static std::pair>, std::vector> getClusteredBBoxes(const pcl::PointCloud::Ptr &lidarCloud, const ClusteringParams &clusteringParams); -private: - static std::vector> - clusterAndColor(const pcl::PointCloud::Ptr &in_cloud_ptr, - double in_max_cluster_distance, double cluster_size_min, - double cluster_size_max); + private: + static std::vector> clusterAndColor( + const pcl::PointCloud::Ptr &in_cloud_ptr, double in_max_cluster_distance, + double cluster_size_min, double cluster_size_max); - static void - checkClusterMerge(size_t in_cluster_id, - const std::vector> &in_clusters, - std::vector &in_out_visited_clusters, - std::vector &out_merge_indices, - double in_merge_threshold); + static void checkClusterMerge(size_t in_cluster_id, + const std::vector> &in_clusters, + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, double in_merge_threshold); - static std::shared_ptr - mergeClusters(const std::vector> &in_clusters, - const std::vector &in_merge_indices, - std::vector &in_out_merged_clusters); + static std::shared_ptr mergeClusters( + const std::vector> &in_clusters, + const std::vector &in_merge_indices, std::vector &in_out_merged_clusters); - static std::vector> - checkAllForMerge(const std::vector> &in_clusters, - float in_merge_threshold); + static std::vector> checkAllForMerge( + const std::vector> &in_clusters, float in_merge_threshold); }; diff --git a/src/perception/tracking/dets_2d_3d/src/cluster.cpp b/src/perception/tracking/dets_2d_3d/src/cluster.cpp index 69d3a9f1..3b81b8f0 100644 --- a/src/perception/tracking/dets_2d_3d/src/cluster.cpp +++ b/src/perception/tracking/dets_2d_3d/src/cluster.cpp @@ -1,10 +1,10 @@ #include "cluster.hpp" -#include #include #include #include #include +#include #include #include @@ -18,17 +18,15 @@ typedef pcl::PointCloud ClusterCloud; typedef vision_msgs::msg::BoundingBox3D BBox3D; int Cluster::color_index = 0; -std::vector> Cluster::colors = {{0, 0, 255}, {0, 255, 0}, - {255, 0, 0}, {255, 255, 0}, - {0, 255, 255}, {255, 0, 255}}; +std::vector> Cluster::colors = {{0, 0, 255}, {0, 255, 0}, {255, 0, 0}, + {255, 255, 0}, {0, 255, 255}, {255, 0, 255}}; Cluster::Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, const std::vector &in_cluster_indices) : cloud_{new ClusterCloud()} { bool indexesGiven = in_cluster_indices.size() > 0; - size_t max_index = - (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + size_t max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); min_point_.x = std::numeric_limits::max(); min_point_.y = std::numeric_limits::max(); @@ -49,19 +47,13 @@ Cluster::Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, pt.g = colors[color_index % colors.size()][1]; pt.b = colors[color_index % colors.size()][2]; - if (pt.x < min_point_.x) - min_point_.x = pt.x; - if (pt.y < min_point_.y) - min_point_.y = pt.y; - if (pt.z < min_point_.z) - min_point_.z = pt.z; + if (pt.x < min_point_.x) min_point_.x = pt.x; + if (pt.y < min_point_.y) min_point_.y = pt.y; + if (pt.z < min_point_.z) min_point_.z = pt.z; - if (pt.x > max_point_.x) - max_point_.x = pt.x; - if (pt.y > max_point_.y) - max_point_.y = pt.y; - if (pt.z > max_point_.z) - max_point_.z = pt.z; + if (pt.x > max_point_.x) max_point_.x = pt.x; + if (pt.y > max_point_.y) max_point_.y = pt.y; + if (pt.z > max_point_.z) max_point_.z = pt.z; cloud_->emplace_back(pt); } @@ -72,8 +64,7 @@ Cluster::Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, BBox3D Cluster::getBoundingBox() { BBox3D bounding_box_; - if (cloud_->size() == 0) - return bounding_box_; + if (cloud_->size() == 0) return bounding_box_; double length_ = max_point_.x - min_point_.x; double width_ = max_point_.y - min_point_.y; diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index d5ef416a..fd63994e 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -1,8 +1,8 @@ #include "dets_2d_3d_node.hpp" -#include #include #include +#include #include #include @@ -11,12 +11,11 @@ #include TrackingNode::TrackingNode() - : Node("dets_2d_3d", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)), - transformInited_{false}, lidarCloud_{ - new pcl::PointCloud()} { + : Node("dets_2d_3d", rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)), + transformInited_{false}, + lidarCloud_{new pcl::PointCloud()} { rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); @@ -33,17 +32,14 @@ TrackingNode::TrackingNode() this->get_parameter("lidar_topic").as_string(), 10, std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); - det_subscriber_ = - this->create_subscription( - this->get_parameter("detections_topic").as_string(), 10, - std::bind(&TrackingNode::receiveDetections, this, - std::placeholders::_1)); + det_subscriber_ = this->create_subscription( + this->get_parameter("detections_topic").as_string(), 10, + std::bind(&TrackingNode::receiveDetections, this, std::placeholders::_1)); det3d_publisher_ = this->create_publisher( this->get_parameter("publish_detections_topic").as_string(), 10); - marker_publisher_ = - this->create_publisher( - this->get_parameter("publish_markers_topic").as_string(), 10); + marker_publisher_ = this->create_publisher( + this->get_parameter("publish_markers_topic").as_string(), 10); pc_publisher_ = this->create_publisher( this->get_parameter("publish_clusters_topic").as_string(), 10); @@ -55,12 +51,10 @@ TrackingNode::TrackingNode() clusteringParams = initializeClusteringParams(clustering_params_map); } -std::map -TrackingNode::initializeClusteringParams( +std::map TrackingNode::initializeClusteringParams( const std::map &clustering_params_map) { std::map clusteringParams; - for (auto iter = clustering_params_map.begin(); - iter != clustering_params_map.end(); ++iter) { + for (auto iter = clustering_params_map.begin(); iter != clustering_params_map.end(); ++iter) { std::string key = iter->first; size_t split_loc = key.find_first_of('.'); std::string det_type = key.substr(0, split_loc); @@ -83,8 +77,7 @@ TrackingNode::initializeClusteringParams( return clusteringParams; } -void TrackingNode::receiveLidar( - const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) { +void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) { // save latest lidar info std::lock_guard guard_lidar(lidarCloud_mutex_); @@ -92,19 +85,16 @@ void TrackingNode::receiveLidar( pcl::fromROSMsg(pointCloud, *lidarCloud_); } -void TrackingNode::readCameraInfo( - const sensor_msgs::msg::CameraInfo::SharedPtr msg) { +void TrackingNode::readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { std::cout << msg->distortion_model << std::endl; camInfo_ = msg; camInfo_subscriber_.reset(); } -void TrackingNode::receiveDetections( - const vision_msgs::msg::Detection2DArray::SharedPtr msg) { +void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg) { if (!transformInited_) { try { - transform_ = tf_buffer_->lookupTransform(cameraFrame_, lidarFrame_, - msg->header.stamp); + transform_ = tf_buffer_->lookupTransform(cameraFrame_, lidarFrame_, msg->header.stamp); transformInited_ = true; } catch (const tf2::TransformException &ex) { RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); @@ -113,22 +103,20 @@ void TrackingNode::receiveDetections( } // remove floor from lidar cloud - pcl::PointCloud::Ptr lidarCloudNoFloor( - new pcl::PointCloud()); + pcl::PointCloud::Ptr lidarCloudNoFloor(new pcl::PointCloud()); { std::lock_guard guard_lidar(lidarCloud_mutex_); ProjectionUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); if (lidarCloudNoFloor->size() == 0) - return; // do not process more, if all the points are floor points - RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", - lidarCloud_->size(), lidarCloudNoFloor->size()); + return; // do not process more, if all the points are floor points + RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", lidarCloud_->size(), + lidarCloudNoFloor->size()); } // transform all lidar pts to 2d points in the camera std::vector lidar2dProjs; - pcl::PointCloud::Ptr inCameraPoints( - new pcl::PointCloud()); + pcl::PointCloud::Ptr inCameraPoints(new pcl::PointCloud()); for (const pcl::PointXYZ &pt : lidarCloudNoFloor->points) { std::optional proj = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, pt); @@ -142,7 +130,7 @@ void TrackingNode::receiveDetections( visualization_msgs::msg::MarkerArray markerArray3d; vision_msgs::msg::Detection3DArray detArray3d; pcl::PointCloud - mergedClusters; // coloured pc to visualize points in each cluster + mergedClusters; // coloured pc to visualize points in each cluster // process each detection in det array int bboxId = 0; @@ -150,10 +138,8 @@ void TrackingNode::receiveDetections( vision_msgs::msg::BoundingBox2D bbox = det.bbox; // process lidarcloud with extrinsic trans from lidar to cam frame - pcl::PointCloud::Ptr inlierPoints( - new pcl::PointCloud()); - ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, - bbox); + pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); + ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, bbox); if (inlierPoints->size() == 0) { RCLCPP_INFO(this->get_logger(), "no inliers found for detection %s", det.results[0].hypothesis.class_id.c_str()); @@ -162,16 +148,13 @@ void TrackingNode::receiveDetections( // clustering auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes( - inlierPoints, - getDefaultOrValue( - clusteringParams, det.results[0].hypothesis.class_id.c_str())); + inlierPoints, getDefaultOrValue( + clusteringParams, det.results[0].hypothesis.class_id.c_str())); std::vector> clusters = - clusterAndBBoxes.first; // needed? for viz purposes only - std::vector allBBoxes = - clusterAndBBoxes.second; + clusterAndBBoxes.first; // needed? for viz purposes only + std::vector allBBoxes = clusterAndBBoxes.second; - if (clusters.size() == 0 || allBBoxes.size() == 0) - continue; + if (clusters.size() == 0 || allBBoxes.size() == 0) continue; // score each 3d bbox based on iou with teh original 2d bbox & the density // of points in the cluster @@ -208,14 +191,12 @@ void TrackingNode::receiveDetections( pc_publisher_->publish(pubCloud); - RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", - markerArray3d.markers.size()); + RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", markerArray3d.markers.size()); } -int TrackingNode::highestIOUScoredBBox( - const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D &detBBox, - const std::vector> &clusters) { +int TrackingNode::highestIOUScoredBBox(const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D &detBBox, + const std::vector> &clusters) { int bestScore = 0; int bestBBox = 0; @@ -233,14 +214,11 @@ int TrackingNode::highestIOUScoredBBox( bottom_right.z = b.center.position.z + b.size.z / 2; std::optional top_left2d = - ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, - top_left); + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); std::optional bottom_right2d = - ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, - bottom_right); + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); - if (!top_left2d || !bottom_right2d) - return 0; + if (!top_left2d || !bottom_right2d) return 0; vision_msgs::msg::BoundingBox2D bbox2d; bbox2d.center.position.x = ((top_left2d->x) + (bottom_right2d->x)) / 2; @@ -262,22 +240,17 @@ int TrackingNode::highestIOUScoredBBox( return bestBBox; } -double -TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &boxA, - const vision_msgs::msg::BoundingBox2D &boxB) { - double overlapHeight = std::min(boxA.center.position.y + boxA.size_y / 2, - boxB.center.position.y + boxB.size_y / 2) - - std::max(boxA.center.position.y - boxA.size_y / 2, - boxB.center.position.y - boxB.size_y / 2); - if (overlapHeight <= 0) - return 0; - - double overlapWidth = std::min(boxA.center.position.x + boxA.size_x / 2, - boxB.center.position.x + boxB.size_x / 2) - - std::max(boxA.center.position.x - boxA.size_x / 2, - boxB.center.position.x - boxB.size_x / 2); - if (overlapWidth <= 0) - return 0; +double TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &boxA, + const vision_msgs::msg::BoundingBox2D &boxB) { + double overlapHeight = + std::min(boxA.center.position.y + boxA.size_y / 2, boxB.center.position.y + boxB.size_y / 2) - + std::max(boxA.center.position.y - boxA.size_y / 2, boxB.center.position.y - boxB.size_y / 2); + if (overlapHeight <= 0) return 0; + + double overlapWidth = + std::min(boxA.center.position.x + boxA.size_x / 2, boxB.center.position.x + boxB.size_x / 2) - + std::max(boxA.center.position.x - boxA.size_x / 2, boxB.center.position.x - boxB.size_x / 2); + if (overlapWidth <= 0) return 0; return overlapHeight * overlapWidth; } @@ -285,8 +258,7 @@ TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &boxA, double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D &bboxA, const vision_msgs::msg::BoundingBox2D &bboxB) { double overlap = overlapBoundingBox(bboxA, bboxB); - return (overlap / (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - - overlap)); + return (overlap / (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - overlap)); } int main(int argc, char **argv) { diff --git a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp index fa1f29d3..4fc73234 100644 --- a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp @@ -1,8 +1,8 @@ #include "projection_utils.hpp" -#include #include #include +#include #include #include @@ -20,20 +20,17 @@ typedef pcl::PointCloud ColorCloud; typedef std::shared_ptr ClusterPtr; typedef vision_msgs::msg::BoundingBox3D BBox3D; -void ProjectionUtils::pointsInBbox( - const Cloud::Ptr &inlierCloud, const Cloud::Ptr &lidarCloud, - const std::vector &projs2d, - const vision_msgs::msg::BoundingBox2D &bbox) { +void ProjectionUtils::pointsInBbox(const Cloud::Ptr &inlierCloud, const Cloud::Ptr &lidarCloud, + const std::vector &projs2d, + const vision_msgs::msg::BoundingBox2D &bbox) { for (size_t i = 0; i < projs2d.size(); ++i) { // P * [x y z 1]^T, P is row major - if (isPointInBbox(projs2d[i], bbox)) - inlierCloud->push_back(lidarCloud->points[i]); + if (isPointInBbox(projs2d[i], bbox)) inlierCloud->push_back(lidarCloud->points[i]); } } -bool ProjectionUtils::isPointInBbox( - const geometry_msgs::msg::Point &pt, - const vision_msgs::msg::BoundingBox2D &bbox) { +bool ProjectionUtils::isPointInBbox(const geometry_msgs::msg::Point &pt, + const vision_msgs::msg::BoundingBox2D &bbox) { double padding = 0; if (bbox.center.position.x - bbox.size_x / 2 - padding < pt.x && @@ -46,8 +43,8 @@ bool ProjectionUtils::isPointInBbox( } std::optional ProjectionUtils::projectLidarToCamera( - const geometry_msgs::msg::TransformStamped &transform, - const std::array &p, const pcl::PointXYZ &pt) { + const geometry_msgs::msg::TransformStamped &transform, const std::array &p, + const pcl::PointXYZ &pt) { // lidar to camera frame auto trans_pt = geometry_msgs::msg::Point(); auto orig_pt = geometry_msgs::msg::Point(); @@ -57,8 +54,7 @@ std::optional ProjectionUtils::projectLidarToCamera( tf2::doTransform(orig_pt, trans_pt, transform); - if (trans_pt.z < 1) - return std::nullopt; + if (trans_pt.z < 1) return std::nullopt; // camera frame to camera 2D projection double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; @@ -70,14 +66,12 @@ std::optional ProjectionUtils::projectLidarToCamera( proj_pt.y = v / w; // check if inside camera frame bounds - if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) - return proj_pt; + if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) return proj_pt; return std::nullopt; } // https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html -void ProjectionUtils::removeFloor(const Cloud::Ptr &lidarCloud, - const Cloud::Ptr &cloud_filtered) { +void ProjectionUtils::removeFloor(const Cloud::Ptr &lidarCloud, const Cloud::Ptr &cloud_filtered) { pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); @@ -89,7 +83,7 @@ void ProjectionUtils::removeFloor(const Cloud::Ptr &lidarCloud, seg.setMaxIterations(100); seg.setAxis(Eigen::Vector3f(0, 0, 1)); seg.setEpsAngle(0.1); - seg.setDistanceThreshold(0.5); // floor distance + seg.setDistanceThreshold(0.5); // floor distance seg.setOptimizeCoefficients(true); seg.setInputCloud(lidarCloud); seg.segment(*inliers, *coefficients); @@ -104,13 +98,11 @@ void ProjectionUtils::removeFloor(const Cloud::Ptr &lidarCloud, extract.filter(*cloud_filtered); } -std::pair, std::vector> -ProjectionUtils::getClusteredBBoxes(const Cloud::Ptr &lidarCloud, - const ClusteringParams &clusteringParams) { +std::pair, std::vector> ProjectionUtils::getClusteredBBoxes( + const Cloud::Ptr &lidarCloud, const ClusteringParams &clusteringParams) { std::vector::Ptr> cloud_segments_array(5); for (size_t i = 0; i < cloud_segments_array.size(); ++i) { - pcl::PointCloud::Ptr temp( - new pcl::PointCloud()); + pcl::PointCloud::Ptr temp(new pcl::PointCloud()); cloud_segments_array[i] = temp; } @@ -133,51 +125,44 @@ ProjectionUtils::getClusteredBBoxes(const Cloud::Ptr &lidarCloud, std::vector all_clusters; for (unsigned int i = 1; i < cloud_segments_array.size(); i++) { // add clusters from each shell - std::vector local_clusters = clusterAndColor( - cloud_segments_array[i], clusteringParams.clustering_thresholds[i], - clusteringParams.cluster_size_min, clusteringParams.cluster_size_max); - all_clusters.insert(all_clusters.end(), local_clusters.begin(), - local_clusters.end()); + std::vector local_clusters = + clusterAndColor(cloud_segments_array[i], clusteringParams.clustering_thresholds[i], + clusteringParams.cluster_size_min, clusteringParams.cluster_size_max); + all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end()); } // merge clusters if possible, do this twice? std::vector mid_clusters = - (all_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge( - all_clusters, clusteringParams.cluster_merge_threshold) - : all_clusters; + (all_clusters.size() > 0) ? ProjectionUtils::checkAllForMerge( + all_clusters, clusteringParams.cluster_merge_threshold) + : all_clusters; std::vector final_clusters = - (mid_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge( - mid_clusters, clusteringParams.cluster_merge_threshold) - : mid_clusters; + (mid_clusters.size() > 0) ? ProjectionUtils::checkAllForMerge( + mid_clusters, clusteringParams.cluster_merge_threshold) + : mid_clusters; // get boundingboxes for each & return all possible 3d bboxes (if valid) std::vector bboxes; for (const ClusterPtr &cluster : final_clusters) { BBox3D b = cluster->getBoundingBox(); - if (cluster->isValid()) - bboxes.emplace_back(b); + if (cluster->isValid()) bboxes.emplace_back(b); } - return std::pair, std::vector>{final_clusters, - bboxes}; + return std::pair, std::vector>{final_clusters, bboxes}; } -std::vector ProjectionUtils::clusterAndColor( - const Cloud::Ptr &in_cloud_ptr, double in_max_cluster_distance, - double cluster_size_min, double cluster_size_max) { +std::vector ProjectionUtils::clusterAndColor(const Cloud::Ptr &in_cloud_ptr, + double in_max_cluster_distance, + double cluster_size_min, + double cluster_size_max) { std::vector clusters; - if (in_cloud_ptr->size() == 0) - return clusters; + if (in_cloud_ptr->size() == 0) return clusters; - pcl::search::KdTree::Ptr tree( - new pcl::search::KdTree()); + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); // create 2d pc, by copying & making it flat Cloud::Ptr cloud_2d(new pcl::PointCloud()); pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d); - for (size_t i = 0; i < cloud_2d->points.size(); i++) - cloud_2d->points[i].z = 0; + for (size_t i = 0; i < cloud_2d->points.size(); i++) cloud_2d->points[i].z = 0; tree->setInputCloud(cloud_2d); @@ -195,43 +180,40 @@ std::vector ProjectionUtils::clusterAndColor( // add pts at clustered indexes to cluster for (const auto &cluster : cluster_indices) { - ClusterPtr cloud_cluster = - std::make_shared(in_cloud_ptr, cluster.indices); + ClusterPtr cloud_cluster = std::make_shared(in_cloud_ptr, cluster.indices); clusters.emplace_back(cloud_cluster); } return clusters; } -void ProjectionUtils::checkClusterMerge( - size_t in_cluster_id, const std::vector &in_clusters, - std::vector &in_out_visited_clusters, - std::vector &out_merge_indices, double in_merge_threshold) { - +void ProjectionUtils::checkClusterMerge(size_t in_cluster_id, + const std::vector &in_clusters, + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, + double in_merge_threshold) { pcl::PointXYZ point_a = in_clusters[in_cluster_id]->getCentroid(); for (size_t i = 0; i < in_clusters.size(); i++) { if (i != in_cluster_id && !in_out_visited_clusters[i]) { pcl::PointXYZ point_b = in_clusters[i]->getCentroid(); - double distance = - sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); + double distance = sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); if (distance <= in_merge_threshold) { in_out_visited_clusters[i] = true; out_merge_indices.push_back(i); // look for all other clusters that can be merged with this merge-able // cluster - checkClusterMerge(i, in_clusters, in_out_visited_clusters, - out_merge_indices, in_merge_threshold); + checkClusterMerge(i, in_clusters, in_out_visited_clusters, out_merge_indices, + in_merge_threshold); } } } } -ClusterPtr -ProjectionUtils::mergeClusters(const std::vector &in_clusters, - const std::vector &in_merge_indices, - std::vector &in_out_merged_clusters) { +ClusterPtr ProjectionUtils::mergeClusters(const std::vector &in_clusters, + const std::vector &in_merge_indices, + std::vector &in_out_merged_clusters) { ColorCloud merged_cloud; // for each cluster in merge cloud indices, merge into larger cloud @@ -250,9 +232,8 @@ ProjectionUtils::mergeClusters(const std::vector &in_clusters, return merged_cluster; } -std::vector -ProjectionUtils::checkAllForMerge(const std::vector &in_clusters, - float in_merge_threshold) { +std::vector ProjectionUtils::checkAllForMerge( + const std::vector &in_clusters, float in_merge_threshold) { std::vector out_clusters; std::vector visited_clusters(in_clusters.size(), false); @@ -263,10 +244,8 @@ ProjectionUtils::checkAllForMerge(const std::vector &in_clusters, visited_clusters[i] = true; std::vector merge_indices; - checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, - in_merge_threshold); - ClusterPtr mergedCluster = - mergeClusters(in_clusters, merge_indices, merged_clusters); + checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold); + ClusterPtr mergedCluster = mergeClusters(in_clusters, merge_indices, merged_clusters); out_clusters.emplace_back(mergedCluster); } From 39eee83239d028c39675111de4a97f359d76b2a0 Mon Sep 17 00:00:00 2001 From: Dan Huynh Date: Thu, 27 Jun 2024 23:05:31 -0400 Subject: [PATCH 12/13] for linter? --- src/samples/python/aggregator/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/samples/python/aggregator/setup.py b/src/samples/python/aggregator/setup.py index b0afb9f6..f77c1804 100755 --- a/src/samples/python/aggregator/setup.py +++ b/src/samples/python/aggregator/setup.py @@ -14,7 +14,7 @@ # Include our package.xml file (os.path.join('share', package_name), ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), \ + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))), ], install_requires=['setuptools'], From fc23ea3cc719ae102ce33ea2ed1b25130e6f7013 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Fri, 4 Oct 2024 00:10:34 +0000 Subject: [PATCH 13/13] cleanup and add viz --- .../dets_2d_3d/config/nuscenes_config.yaml | 17 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 2 +- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 15 +- src/perception/tracking/requirements.txt | 3 + .../tracking_viz/config/basic_config.yaml | 8 + .../tracking_viz/launch/basic_viz.launch.py | 23 ++ .../tracking/tracking_viz/package.xml | 24 +++ .../tracking_viz/resource/tracking_viz | 0 .../tracking/tracking_viz/setup.cfg | 4 + src/perception/tracking/tracking_viz/setup.py | 30 +++ .../tracking_viz/tracking_viz/__init__.py | 0 .../tracking_viz/tracking_viz/draw_tracks.py | 204 ++++++++++++++++++ 12 files changed, 321 insertions(+), 9 deletions(-) create mode 100644 src/perception/tracking/requirements.txt create mode 100644 src/perception/tracking/tracking_viz/config/basic_config.yaml create mode 100644 src/perception/tracking/tracking_viz/launch/basic_viz.launch.py create mode 100644 src/perception/tracking/tracking_viz/package.xml create mode 100755 src/perception/tracking/tracking_viz/resource/tracking_viz create mode 100644 src/perception/tracking/tracking_viz/setup.cfg create mode 100644 src/perception/tracking/tracking_viz/setup.py create mode 100755 src/perception/tracking/tracking_viz/tracking_viz/__init__.py create mode 100755 src/perception/tracking/tracking_viz/tracking_viz/draw_tracks.py diff --git a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml index 4bd841da..c5d1131f 100644 --- a/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml +++ b/src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml @@ -12,9 +12,22 @@ dets_2d_3d_node: lidar_frame: LIDAR_TOP clustering_params: - car: + default: clustering_distances: [5., 30., 45., 60.] clustering_thresholds: [0.5, 1.1, 1.6, 2.1, 2.6] - cluster_size_min: 20. + cluster_size_min: 5. cluster_size_max: 100000. cluster_merge_threshold: 1.5 + person: + clustering_distances: [5., 30., 45., 60.] + clustering_thresholds: [3., 3.5, 4.5, 5.0, 5.6] + cluster_size_min: 2. + cluster_size_max: 100. + cluster_merge_threshold: 1.5 + traffic_light: + clustering_distances: [5., 30., 45., 60.] + clustering_thresholds: [3., 3.5, 4.5, 5.0, 5.6] + cluster_size_min: 2. + cluster_size_max: 100. + cluster_merge_threshold: 1.5 + \ No newline at end of file diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index 9d1a9d0b..289e5184 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -75,7 +75,7 @@ class TrackingNode : public rclcpp::Node { template T getDefaultOrValue(std::map m, std::string key) { - if (m.find(key) == m.end()) return m[key]; + if (auto it = m.find(key); it != m.end()) return it->second; return m["default"]; } }; diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index fd63994e..9642dcc3 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -61,6 +61,10 @@ std::map TrackingNode::initializeClusteringParams std::string field_type = key.substr(split_loc + 1); ClusteringParams params; + if (auto it = clusteringParams.find(det_type); it != clusteringParams.end()) { + params = it->second; + } + if (field_type == "clustering_distances") params.clustering_distances = iter->second.as_double_array(); else if (field_type == "clustering_thresholds") @@ -94,7 +98,7 @@ void TrackingNode::readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg) { if (!transformInited_) { try { - transform_ = tf_buffer_->lookupTransform(cameraFrame_, lidarFrame_, msg->header.stamp); + transform_ = tf_buffer_->lookupTransform(cameraFrame_, lidarFrame_, tf2::TimePointZero); transformInited_ = true; } catch (const tf2::TransformException &ex) { RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); @@ -147,11 +151,10 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S } // clustering - auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes( - inlierPoints, getDefaultOrValue( - clusteringParams, det.results[0].hypothesis.class_id.c_str())); - std::vector> clusters = - clusterAndBBoxes.first; // needed? for viz purposes only + auto params = getDefaultOrValue( + clusteringParams, det.results[0].hypothesis.class_id.c_str()); + auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes(inlierPoints, params); + std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only std::vector allBBoxes = clusterAndBBoxes.second; if (clusters.size() == 0 || allBBoxes.size() == 0) continue; diff --git a/src/perception/tracking/requirements.txt b/src/perception/tracking/requirements.txt new file mode 100644 index 00000000..2924f148 --- /dev/null +++ b/src/perception/tracking/requirements.txt @@ -0,0 +1,3 @@ +setuptools==65.5.1 +numpy==1.24.2 +scipy diff --git a/src/perception/tracking/tracking_viz/config/basic_config.yaml b/src/perception/tracking/tracking_viz/config/basic_config.yaml new file mode 100644 index 00000000..8f34133b --- /dev/null +++ b/src/perception/tracking/tracking_viz/config/basic_config.yaml @@ -0,0 +1,8 @@ +tracking_viz_node: + ros__parameters: + image_topic: /annotated_img + publish_viz_topic: /annotated_3d_img + det_3d_topic: /detections_3d + camera_info_topic: /CAM_FRONT/camera_info + camera_frame: CAM_FRONT + lidar_frame: LIDAR_TOP diff --git a/src/perception/tracking/tracking_viz/launch/basic_viz.launch.py b/src/perception/tracking/tracking_viz/launch/basic_viz.launch.py new file mode 100644 index 00000000..a8b2721b --- /dev/null +++ b/src/perception/tracking/tracking_viz/launch/basic_viz.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('tracking_viz'), + 'config', + 'basic_config.yaml' + ) + + tracking_viz_node = Node( + package='tracking_viz', + executable='tracking_viz_node', + name='tracking_viz_node', + parameters=[config], + arguments=['--ros-args', '--log-level', 'info'] + ) + + return LaunchDescription([tracking_viz_node]) diff --git a/src/perception/tracking/tracking_viz/package.xml b/src/perception/tracking/tracking_viz/package.xml new file mode 100644 index 00000000..c585c087 --- /dev/null +++ b/src/perception/tracking/tracking_viz/package.xml @@ -0,0 +1,24 @@ + + + + tracking_viz + 0.0.0 + The tracking viz package + Steven Gong + TODO: License declaration + + cv_bridge + sensor_msgs + vision_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + + diff --git a/src/perception/tracking/tracking_viz/resource/tracking_viz b/src/perception/tracking/tracking_viz/resource/tracking_viz new file mode 100755 index 00000000..e69de29b diff --git a/src/perception/tracking/tracking_viz/setup.cfg b/src/perception/tracking/tracking_viz/setup.cfg new file mode 100644 index 00000000..3cf91db1 --- /dev/null +++ b/src/perception/tracking/tracking_viz/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tracking_viz +[install] +install_scripts=$base/lib/tracking_viz diff --git a/src/perception/tracking/tracking_viz/setup.py b/src/perception/tracking/tracking_viz/setup.py new file mode 100644 index 00000000..4249a7c1 --- /dev/null +++ b/src/perception/tracking/tracking_viz/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'tracking_viz' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Meshva', + maintainer_email='meshvaD@watonomous.ca', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'tracking_viz_node = tracking_viz.draw_tracks:main' + ], + }, +) diff --git a/src/perception/tracking/tracking_viz/tracking_viz/__init__.py b/src/perception/tracking/tracking_viz/tracking_viz/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/perception/tracking/tracking_viz/tracking_viz/draw_tracks.py b/src/perception/tracking/tracking_viz/tracking_viz/draw_tracks.py new file mode 100755 index 00000000..af2ef097 --- /dev/null +++ b/src/perception/tracking/tracking_viz/tracking_viz/draw_tracks.py @@ -0,0 +1,204 @@ +import rclpy +from rclpy.node import Node +import os + +from geometry_msgs.msg import Pose +from sensor_msgs.msg import Image, CameraInfo +from vision_msgs.msg import Detection3DArray + +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +import tf2_geometry_msgs +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +import cv2 +from cv_bridge import CvBridge, CvBridgeError +import numpy as np + +import time +from collections import deque +from multiprocessing import Lock +from scipy.spatial.transform import Rotation +from random import randint + +mutex = Lock() + +class DrawBasicDetections(Node): + def __init__(self): + super().__init__("tracking_viz_node") + self.get_logger().info("Creating tracking viz node...") + + self.declare_parameter("image_topic", "/image") + self.declare_parameter("publish_viz_topic", "/annotated_3d_det_img") + self.declare_parameter("det_3d_topic", "/det_3d_topic") + self.declare_parameter("camera_info_topic", "/camera_info") + self.declare_parameter("camera_frame", "/camera") + self.declare_parameter("lidar_frame", "/lidar") + + self.image_topic = self.get_parameter("image_topic").value + self.publish_viz_topic = self.get_parameter("publish_viz_topic").value + self.det_3d_topic = self.get_parameter("det_3d_topic").value + self.camera_info_topic = self.get_parameter("camera_info_topic").value + self.camera_frame = self.get_parameter("camera_frame").value + self.lidar_frame = self.get_parameter("lidar_frame").value + + # subscribes to images & 3D dets + self.unprocessed_images = deque() + self.unprocessed_dets = deque() + self.camera_info = None + self.transform = None + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.cv_bridge = CvBridge() + + self.image_subscription = self.create_subscription( + Image, + self.image_topic, + self.image_callback, + qos_profile=QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + ), + ) + + self.det_3d_subscription = self.create_subscription( + Detection3DArray, + self.det_3d_topic, + self.det_3d_callback, + qos_profile=QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + ), + ) + + self.camera_info_subscription = self.create_subscription( + CameraInfo, + self.camera_info_topic, + self.camera_info_callback, + qos_profile=QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + ), + ) + + self.viz_publisher = self.create_publisher(Image, self.publish_viz_topic, 10) + + def image_callback(self, msg): + with mutex: + self.unprocessed_images.append(msg) + self.try_draw() + + def det_3d_callback(self, msg): + with mutex: + self.unprocessed_dets.append(msg) + + # get transform from lidar -> camera + if self.transform is None: + try: + self.transform = self.tf_buffer.lookup_transform( + self.camera_frame, + self.lidar_frame, + rclpy.time.Time()) + except TransformException as ex: + self.get_logger().info( + f'Could not transform from {self.lidar_frame} to {self.camera_frame}: {ex}') + return + + self.try_draw() + + def camera_info_callback(self, msg): + self.camera_info = np.array(msg.p).reshape(3, 4) + + self.get_logger().info(f"GOT CAMERA INFO... {self.camera_info}") + + self.destroy_subscription(self.camera_info_subscription) + + def try_draw(self): + if not self.unprocessed_images or not self.unprocessed_dets or self.transform is None or self.camera_info is None: + return + + with mutex: + image_msg = self.unprocessed_images.popleft() + det_3d_msg = self.unprocessed_dets.popleft() + + try: + image = self.cv_bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough") + except CvBridgeError as e: + self.get_logger().error(str(e)) + return + + self.get_logger().info(f"PROCESSING IMAGE + DET3D...") + + for det_msg in det_3d_msg.detections: + bbox = det_msg.bbox + + center = np.array([bbox.center.position.x, bbox.center.position.y, bbox.center.position.z]) + rot = Rotation.from_quat([bbox.center.orientation.x, bbox.center.orientation.y, bbox.center.orientation.z, bbox.center.orientation.w]) + size = np.array([bbox.size.x, bbox.size.y, bbox.size.z]) + + # get all 8 corners + vert = [ center + rot.apply(np.multiply(size , np.array([-1, 1, 1]))), + center + rot.apply(np.multiply(size , np.array([-1, -1, 1]))), + center + rot.apply(np.multiply(size , np.array([-1, -1, -1]))), + center + rot.apply(np.multiply(size , np.array([-1, 1, -1]))), + center + rot.apply(np.multiply(size , np.array([1, 1, 1]))), + center + rot.apply(np.multiply(size , np.array([1, -1, 1]))), + center + rot.apply(np.multiply(size , np.array([1, -1, -1]))), + center + rot.apply(np.multiply(size , np.array([1, 1, -1]))), + ] + + color = (randint(0, 255), randint(0, 255), randint(0, 255)) + verts_2d = [] + + # project each 3d vert to 2d + for v in vert: + # convert v into a pos2d message + v_msg = Pose() + v_msg.position.x = v[0] + v_msg.position.y = v[1] + v_msg.position.z = v[2] + + # lidar to camera frame + v_trans = tf2_geometry_msgs.do_transform_pose(v_msg, self.transform) + v_trans = np.array([v_trans.position.x, v_trans.position.y, v_trans.position.z, 1]) + + # project 3d camera frame to 2d camera plane + v_2d = self.camera_info @ v_trans + v_2d = np.array([int(v_2d[0] / v_2d[2]), int(v_2d[1] / v_2d[2])]) + verts_2d.append(v_2d) + + # draw vertex onto image + # image = cv2.circle(image, v_2d, 5, color, thickness=-1) + + # draw edges + for i in range(4): + image = cv2.line(image, verts_2d[i], verts_2d[(i+1) % 4], color, 10) # face 1 + image = cv2.line(image, verts_2d[i+4], verts_2d[(i+1) % 4 + 4], color, 10) # face 2 + image = cv2.line(image, verts_2d[i], verts_2d[i+4], color, 10) # connect faces + + self.publish_viz(image, image_msg) + + def publish_viz(self, cv_img, msg): + imgmsg = self.cv_bridge.cv2_to_imgmsg(cv_img, "bgr8") + imgmsg.header.stamp = msg.header.stamp + imgmsg.header.frame_id = msg.header.frame_id + self.viz_publisher.publish(imgmsg) + +def main(args=None): + rclpy.init(args=args) + + tracking_viz_node = DrawBasicDetections() + rclpy.spin(tracking_viz_node) + tracking_viz_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()