perception: ROS2 Node to convert from 2D to 3D detections

docker/perception/tracking/tracking.Dockerfile

@@ -21,8 +21,12 @@ 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)
 
+# 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

modules/docker-compose.perception.yaml

@@ -29,7 +29,7 @@ services:
             - driver: nvidia
               count: 1
               capabilities: [ gpu ]
-    command: /bin/bash -c "ros2 launch camera_object_detection eve.launch.py"
+    command: /bin/bash -c "ros2 launch camera_object_detection nuscenes_launch.py"
     volumes:
       - /mnt/wato-drive2/perception_models/yolov8m.pt:/perception_models/yolov8m.pt
       - /mnt/wato-drive2/perception_models/traffic_light.pt:/perception_models/traffic_light.pt
@@ -86,7 +86,7 @@ services:
         - "${PERCEPTION_TRACKING_IMAGE}:build_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"
 
   depth_estimation:
     build:

src/perception/tracking/dets_2d_3d/CMakeLists.txt

@@ -0,0 +1,70 @@
+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)
+find_package(std_msgs REQUIRED)
+
+add_library(projection_utils
+  src/projection_utils.cpp src/cluster.cpp) # add more non-ros node files here
+
+target_include_directories(projection_utils
+  PUBLIC include)
+
+ament_target_dependencies(projection_utils 
+  rclcpp 
+  sample_msgs 
+  sensor_msgs 
+  vision_msgs
+  visualization_msgs
+  pcl_conversions 
+  pcl_ros
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  std_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)
+
+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 projection_utils)
+
+# Copy executable to installation location
+install(TARGETS
+  dets_2d_3d_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Copy launch files to installation location
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

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})

src/perception/tracking/dets_2d_3d/config/nuscenes_config.yaml

@@ -0,0 +1,33 @@
+dets_2d_3d_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_params:
+      default:
+        clustering_distances: [5., 30., 45., 60.]
+        clustering_thresholds: [0.5, 1.1, 1.6, 2.1, 2.6]
+        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
+

src/perception/tracking/dets_2d_3d/include/cluster.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vision_msgs/msg/detection3_d_array.hpp>
+
+#include <vector>
+
+class Cluster {
+ public:
+  // makes a cloud from all points `in_cloud_ptr` that are indexed by
+  // `in_cluster_indices`
+  Cluster(const pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud_ptr,
+          const std::vector<int> &in_cluster_indices);
+
+  pcl::PointXYZ getCentroid() { return centroid_; }
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr getCloud() { return cloud_; }
+  vision_msgs::msg::BoundingBox3D getBoundingBox();
+
+  bool isValid();
+  int size() { return cloud_->size(); }
+
+ private:
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_;
+  pcl::PointXYZ centroid_;
+
+  pcl::PointXYZ max_point_;
+  pcl::PointXYZ min_point_;
+
+  static int color_index;
+  static std::vector<std::vector<int>> colors;
+};

src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <vision_msgs/msg/detection2_d_array.hpp>
+#include <vision_msgs/msg/detection3_d_array.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "cluster.hpp"
+#include "projection_utils.hpp"
+
+#include <map>
+
+struct ClusteringParams;
+
+class TrackingNode : public rclcpp::Node {
+ public:
+  TrackingNode();
+
+ private:
+  std::map<std::string, ClusteringParams> 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<pcl::PointXYZ>::Ptr lidarCloud_;
+
+  // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection
+  // boxes
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camInfo_subscriber_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_subscriber_;
+  rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr det_subscriber_;
+
+  // publish the 3d detections
+  rclcpp::Publisher<vision_msgs::msg::Detection3DArray>::SharedPtr det3d_publisher_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_publisher2_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> 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<vision_msgs::msg::BoundingBox3D> bboxes,
+                           const vision_msgs::msg::BoundingBox2D &detBBox,
+                           const std::vector<std::shared_ptr<Cluster>> &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<std::string, ClusteringParams> initializeClusteringParams(
+      const std::map<std::string, rclcpp::Parameter> &clustering_params_map);
+
+  template <typename T>
+  T getDefaultOrValue(std::map<std::string, T> m, std::string key) {
+    if (auto it = m.find(key); it != m.end()) return it->second;
+    return m["default"];
+  }
+};

src/perception/tracking/dets_2d_3d/include/projection_utils.hpp

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "cluster.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <vision_msgs/msg/bounding_box2_d.hpp>
+#include <vision_msgs/msg/bounding_box3_d.hpp>
+
+#include <optional>
+#include <vector>
+
+struct ClusteringParams {
+  // segment by distance
+  std::vector<double> clustering_distances;
+  // Map of the nearest neighbor distance threshold for each segment, for each
+  // detection
+  std::vector<double> 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:
+  static void pointsInBbox(const pcl::PointCloud<pcl::PointXYZ>::Ptr &inlierCloud,
+                           const pcl::PointCloud<pcl::PointXYZ>::Ptr &lidarCloud,
+                           const std::vector<geometry_msgs::msg::Point> &projs2d,
+                           const vision_msgs::msg::BoundingBox2D &bbox);
+
+  // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to
+  // camera frame
+  static std::optional<geometry_msgs::msg::Point> projectLidarToCamera(
+      const geometry_msgs::msg::TransformStamped &transform, const std::array<double, 12> &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<pcl::PointXYZ>::Ptr &lidarCloud,
+                          const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_filtered);
+
+  static std::pair<std::vector<std::shared_ptr<Cluster>>,
+                   std::vector<vision_msgs::msg::BoundingBox3D>>
+  getClusteredBBoxes(const pcl::PointCloud<pcl::PointXYZ>::Ptr &lidarCloud,
+                     const ClusteringParams &clusteringParams);
+
+ private:
+  static std::vector<std::shared_ptr<Cluster>> clusterAndColor(
+      const pcl::PointCloud<pcl::PointXYZ>::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<std::shared_ptr<Cluster>> &in_clusters,
+                                std::vector<bool> &in_out_visited_clusters,
+                                std::vector<size_t> &out_merge_indices, double in_merge_threshold);
+
+  static std::shared_ptr<Cluster> mergeClusters(
+      const std::vector<std::shared_ptr<Cluster>> &in_clusters,
+      const std::vector<size_t> &in_merge_indices, std::vector<bool> &in_out_merged_clusters);
+
+  static std::vector<std::shared_ptr<Cluster>> checkAllForMerge(
+      const std::vector<std::shared_ptr<Cluster>> &in_clusters, float in_merge_threshold);
+};

src/perception/tracking/dets_2d_3d/launch/dets_2d_3d.launch.py

@@ -0,0 +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():
+    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']
+    )
+
+    # finalize
+    ld.add_action(detections_2d_3d_node)
+
+    return ld