├── README.md └── image_pointcloud_fusion ├── CMakeLists.txt ├── package.xml └── scripts ├── camera_lidar_detection.py ├── fusion_detection.py ├── imgpc_fusion_detection.py ├── pc2img_projection.py └── yolo11s.pt /README.md: -------------------------------------------------------------------------------- 1 | Camera LIDAR pre-fusion, post-fusion 2 | method：pointcloud cluster+yolo11 3 | video：https://www.bilibili.com/video/BV1fwMwzMEFW/?spm_id_from=333.1387.upload.video_card.click 4 | -------------------------------------------------------------------------------- /image_pointcloud_fusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(image_pointcloud_fusion) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | sensor_msgs 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # sensor_msgs# std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES image_pointcloud_fusion 108 | # CATKIN_DEPENDS rospy sensor_msgs std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/image_pointcloud_fusion.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/image_pointcloud_fusion_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_image_pointcloud_fusion.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /image_pointcloud_fusion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | image_pointcloud_fusion 4 | 0.0.0 5 | The image_pointcloud_fusion package 6 | 7 | 8 | 9 | 10 | jiewang 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | sensor_msgs 54 | std_msgs 55 | rospy 56 | sensor_msgs 57 | std_msgs 58 | rospy 59 | sensor_msgs 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /image_pointcloud_fusion/scripts/camera_lidar_detection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | import numpy as np 6 | import cv2 7 | from cv_bridge import CvBridge, CvBridgeError 8 | from sensor_msgs.msg import Image, PointCloud2 9 | import sensor_msgs.point_cloud2 as pc2 10 | from std_msgs.msg import Header 11 | from visualization_msgs.msg import Marker, MarkerArray 12 | from geometry_msgs.msg import Point 13 | from message_filters import ApproximateTimeSynchronizer, Subscriber 14 | import time 15 | import os 16 | import torch 17 | from ultralytics import YOLO 18 | 19 | class YoloLidarFusion: 20 | def __init__(self): 21 | rospy.init_node('yolo_lidar_fusion', anonymous=True) 22 | 23 | # 获取参数 24 | self.camera_topic = rospy.get_param('~camera_topic', '/camera/color/image_raw') 25 | self.lidar_topic = rospy.get_param('~lidar_topic', '/livox/lidar') 26 | self.detection_image_topic = rospy.get_param('~detection_image_topic', '/detection/image') 27 | self.bbox_markers_topic = rospy.get_param('~bbox_markers_topic', '/detection/bbox_markers') 28 | self.camera_frame = rospy.get_param('~camera_frame', 'camera') 29 | self.lidar_frame = rospy.get_param('~lidar_frame', 'livox_frame') 30 | self.detection_threshold = rospy.get_param('~detection_threshold', 0.7) 31 | self.yolo_model_path = rospy.get_param('~yolo_model_path', 'yolo11s.pt') 32 | 33 | # 相机内参矩阵 34 | fx = rospy.get_param('~fx', 909.783) 35 | fy = rospy.get_param('~fy', 909.004) 36 | cx = rospy.get_param('~cx', 650.365) 37 | cy = rospy.get_param('~cy', 381.295) 38 | self.camera_matrix = np.array([ 39 | [fx, 0, cx], 40 | [0, fy, cy], 41 | [0, 0, 1] 42 | ]) 43 | 44 | # 激光雷达到相机的外参矩阵 45 | extrinsic_matrix = rospy.get_param('~extrinsic_matrix', None) 46 | if extrinsic_matrix is None: 47 | self.extrinsic_matrix = np.array([ 48 | [0.0, -1.0, 0.0, 0.03], 49 | [0.0, 0.0, -1.0, -0.022], 50 | [1.0, 0.0, 0.0, -0.04], 51 | [0, 0, 0, 1] 52 | ]) 53 | else: 54 | self.extrinsic_matrix = np.array(extrinsic_matrix).reshape(4, 4) 55 | 56 | # 相机到激光雷达的变换矩阵（用于将相机坐标转换到激光雷达坐标） 57 | self.camera_to_lidar_matrix = np.linalg.inv(self.extrinsic_matrix) 58 | 59 | rospy.loginfo("Camera matrix: \n{}".format(self.camera_matrix)) 60 | rospy.loginfo("Extrinsic matrix: \n{}".format(self.extrinsic_matrix)) 61 | 62 | # 初始化CV Bridge 63 | self.bridge = CvBridge() 64 | 65 | # 加载YOLOv11模型 66 | try: 67 | rospy.loginfo("Loading YOLOv11 model from: {}".format(self.yolo_model_path)) 68 | self.yolo_model = self.load_yolo_model(self.yolo_model_path) 69 | rospy.loginfo("YOLOv11 model loaded successfully") 70 | except Exception as e: 71 | rospy.logerr("Failed to load YOLOv11 model: {}".format(e)) 72 | raise 73 | 74 | # 创建发布者 75 | self.detection_image_pub = rospy.Publisher(self.detection_image_topic, Image, queue_size=1) 76 | self.bbox_markers_pub = rospy.Publisher(self.bbox_markers_topic, MarkerArray, queue_size=1) 77 | 78 | # 创建订阅者并进行时间同步 79 | self.image_sub = Subscriber(self.camera_topic, Image) 80 | self.lidar_sub = Subscriber(self.lidar_topic, PointCloud2) 81 | 82 | # 使用Approximate Time Synchronizer同步数据 83 | self.ts = ApproximateTimeSynchronizer([self.image_sub, self.lidar_sub], queue_size=10, slop=0.2) 84 | self.ts.registerCallback(self.callback) 85 | 86 | rospy.loginfo("YoloLidarFusion node initialized") 87 | 88 | def load_yolo_model(self, model_path): 89 | """加载YOLO模型(使用ultralytics库)""" 90 | # 检查模型文件是否存在 91 | if not os.path.exists(model_path): 92 | raise FileNotFoundError(f"YOLO model file not found: {model_path}") 93 | 94 | # 使用ultralytics库加载YOLO模型 95 | try: 96 | # 导入ultralytics 97 | from ultralytics import YOLO 98 | 99 | # 加载模型 100 | model = YOLO(model_path) 101 | 102 | # 设置推理参数 103 | model.conf = self.detection_threshold # 置信度阈值 104 | rospy.loginfo(f"Successfully loaded YOLO model from: {model_path}") 105 | rospy.loginfo(f"Model type: {type(model).__name__}") 106 | return model 107 | 108 | except Exception as e: 109 | rospy.logerr(f"Error loading YOLO model with ultralytics: {e}") 110 | raise 111 | 112 | def callback(self, image_msg, pointcloud_msg): 113 | """处理同步到的图像和点云数据""" 114 | try: 115 | start_time = time.time() 116 | 117 | # 将ROS图像消息转换为OpenCV图像 118 | cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") 119 | 120 | # 从点云消息中提取点云数据 121 | points_list = [] 122 | for point in pc2.read_points(pointcloud_msg, field_names=("x", "y", "z"), skip_nans=True): 123 | points_list.append(point) 124 | 125 | # 将点云数据转换为NumPy数组 126 | points = np.array(points_list) 127 | 128 | # 使用YOLOv11进行目标检测 129 | detections = self.detect_objects(cv_image) 130 | 131 | # 在图像上绘制检测结果 132 | detection_image = self.draw_detections_on_image(cv_image, detections) 133 | 134 | # 为点云中的检测对象创建3D边界框 135 | markers = self.create_3d_bboxes(points, detections, pointcloud_msg.header) 136 | 137 | # 发布检测结果图像 138 | detection_image_msg = self.bridge.cv2_to_imgmsg(detection_image, "bgr8") 139 | detection_image_msg.header = image_msg.header 140 | self.detection_image_pub.publish(detection_image_msg) 141 | 142 | # 发布3D边界框标记 143 | self.bbox_markers_pub.publish(markers) 144 | 145 | processing_time = time.time() - start_time 146 | rospy.logdebug("Processing time: {:.3f} seconds".format(processing_time)) 147 | 148 | except CvBridgeError as e: 149 | rospy.logerr("CvBridge Error: {0}".format(e)) 150 | except Exception as e: 151 | rospy.logerr("Error in callback: {0}".format(e)) 152 | 153 | def detect_objects(self, image): 154 | """使用ultralytics YOLO进行目标检测""" 155 | try: 156 | # 使用YOLO模型进行推理 157 | results = self.yolo_model(image) 158 | 159 | # 准备检测结果列表 160 | detections = [] 161 | 162 | # ultralytics YOLO返回Results对象列表 163 | for result in results: 164 | # 获取检测框 165 | boxes = result.boxes 166 | 167 | # 遍历每个检测框 168 | for box in boxes: 169 | # 获取边界框坐标 170 | x1, y1, x2, y2 = box.xyxy[0].cpu().numpy() 171 | 172 | # 获取置信度 173 | conf = box.conf[0].cpu().numpy() 174 | 175 | # 获取类别ID和名称 176 | class_id = int(box.cls[0].cpu().numpy()) 177 | class_name = result.names[class_id] 178 | 179 | detection = { 180 | 'bbox': [int(x1), int(y1), int(x2), int(y2)], 181 | 'confidence': float(conf), 182 | 'class_id': class_id, 183 | 'class_name': class_name 184 | } 185 | detections.append(detection) 186 | 187 | rospy.loginfo(f"Detected {len(detections)} objects") 188 | return detections 189 | 190 | except Exception as e: 191 | rospy.logerr(f"Error during object detection: {e}") 192 | import traceback 193 | rospy.logerr(traceback.format_exc()) 194 | return [] 195 | 196 | def draw_detections_on_image(self, image, detections): 197 | """在图像上绘制检测结果""" 198 | result_image = image.copy() 199 | 200 | # 定义类别-颜色映射 201 | colors = { 202 | 'person': (0, 255, 0), # 绿色 203 | 'car': (0, 0, 255), # 红色 204 | 'truck': (255, 0, 0), # 蓝色 205 | 'bicycle': (255, 255, 0), # 青色 206 | 'dog': (255, 0, 255), # 紫色 207 | 'cat': (0, 255, 255), # 黄色 208 | } 209 | 210 | default_color = (255, 255, 255) # 白色，用于未定义颜色的类别 211 | 212 | for det in detections: 213 | # 提取边界框坐标 214 | x1, y1, x2, y2 = det['bbox'] 215 | 216 | # 获取类别名称和置信度 217 | class_name = det['class_name'] 218 | confidence = det['confidence'] 219 | 220 | # 选择颜色 221 | color = colors.get(class_name.lower(), default_color) 222 | 223 | # 绘制边界框 224 | cv2.rectangle(result_image, (x1, y1), (x2, y2), color, 2) 225 | 226 | # 准备标签文本 227 | label = "{}: {:.2f}".format(class_name, confidence) 228 | 229 | # 计算标签的大小 230 | (text_width, text_height), baseline = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2) 231 | 232 | # 绘制标签背景 233 | cv2.rectangle(result_image, (x1, y1 - text_height - 10), (x1 + text_width, y1), color, -1) 234 | 235 | # 绘制标签文本 236 | cv2.putText(result_image, label, (x1, y1 - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2) 237 | 238 | return result_image 239 | 240 | def project_points_to_image(self, points): 241 | """将点云投影到图像平面上""" 242 | # 为点云添加第四个坐标（齐次坐标） 243 | points_homogeneous = np.ones((points.shape[0], 4)) 244 | points_homogeneous[:, :3] = points 245 | 246 | # 使用外参矩阵将点从激光雷达坐标系变换到相机坐标系 247 | points_camera = np.dot(self.extrinsic_matrix, points_homogeneous.T).T 248 | 249 | # 过滤掉相机后方的点（Z < 0） 250 | valid_indices = points_camera[:, 2] > 0 251 | points_camera = points_camera[valid_indices] 252 | valid_original_points = points[valid_indices] 253 | 254 | if len(points_camera) == 0: 255 | return None, None, None 256 | 257 | # 归一化坐标 258 | points_normalized = points_camera[:, :3] / points_camera[:, 2:3] 259 | 260 | # 使用相机内参矩阵将归一化坐标投影到像素坐标 261 | pixel_coords = np.dot(self.camera_matrix, points_normalized[:, :3].T).T 262 | 263 | # 取整得到像素坐标 264 | pixel_coords = np.round(pixel_coords[:, :2]).astype(np.int32) 265 | 266 | return pixel_coords, valid_original_points, points_camera[:, 2] # 返回像素坐标、对应的原始点和深度 267 | 268 | def create_3d_bboxes(self, points, detections, header): 269 | """为检测到的对象创建3D边界框""" 270 | # 将点云投影到图像平面 271 | pixel_coords, valid_points, depths = self.project_points_to_image(points) 272 | 273 | if pixel_coords is None: 274 | return MarkerArray() 275 | 276 | markers_array = MarkerArray() 277 | marker_id = 0 278 | 279 | # 为每个检测结果创建3D边界框 280 | for det in detections: 281 | # 获取2D边界框坐标 282 | x1, y1, x2, y2 = det['bbox'] 283 | 284 | # 获取类别名称 285 | class_name = det['class_name'] 286 | 287 | # 找出落在边界框内的点 288 | mask = ((pixel_coords[:, 0] >= x1) & (pixel_coords[:, 0] <= x2) & 289 | (pixel_coords[:, 1] >= y1) & (pixel_coords[:, 1] <= y2)) 290 | 291 | # 如果边界框内没有点，则跳过 292 | if not np.any(mask): 293 | continue 294 | 295 | # 获取边界框内的点 296 | bbox_points = valid_points[mask] 297 | 298 | # 计算点云的边界框 299 | if len(bbox_points) < 5: # 需要足够的点来确定边界 300 | continue 301 | 302 | # 计算3D边界框 303 | x_min, y_min, z_min = np.min(bbox_points, axis=0) 304 | x_max, y_max, z_max = np.max(bbox_points, axis=0) 305 | 306 | # 创建边界框标记 307 | marker = Marker() 308 | marker.header = header 309 | marker.ns = "detection" 310 | marker.id = marker_id 311 | marker.type = Marker.CUBE 312 | marker.action = Marker.ADD 313 | 314 | # 设置边界框中心位置 315 | marker.pose.position.x = (x_min + x_max) / 2 316 | marker.pose.position.y = (y_min + y_max) / 2 317 | marker.pose.position.z = (z_min + z_max) / 2 318 | 319 | # 设置边界框方向（默认为无旋转） 320 | marker.pose.orientation.x = 0.0 321 | marker.pose.orientation.y = 0.0 322 | marker.pose.orientation.z = 0.0 323 | marker.pose.orientation.w = 1.0 324 | 325 | # 设置边界框尺寸 326 | marker.scale.x = x_max - x_min 327 | marker.scale.y = y_max - y_min 328 | marker.scale.z = z_max - z_min 329 | 330 | # 设置边界框颜色（根据类别） 331 | if class_name.lower() == 'person': 332 | marker.color.r = 0.0 333 | marker.color.g = 1.0 334 | marker.color.b = 0.0 335 | elif class_name.lower() == 'car': 336 | marker.color.r = 1.0 337 | marker.color.g = 0.0 338 | marker.color.b = 0.0 339 | elif class_name.lower() == 'truck': 340 | marker.color.r = 0.0 341 | marker.color.g = 0.0 342 | marker.color.b = 1.0 343 | else: 344 | marker.color.r = 1.0 345 | marker.color.g = 1.0 346 | marker.color.b = 0.0 347 | 348 | marker.color.a = 0.5 # 半透明 349 | 350 | # 设置标记的持续时间 351 | marker.lifetime = rospy.Duration(0.1) 352 | 353 | # 添加标记到数组 354 | markers_array.markers.append(marker) 355 | 356 | # 创建标签标记 357 | text_marker = Marker() 358 | text_marker.header = header 359 | text_marker.ns = "detection_label" 360 | text_marker.id = marker_id + 10000 # 避免ID冲突 361 | text_marker.type = Marker.TEXT_VIEW_FACING 362 | text_marker.action = Marker.ADD 363 | 364 | # 设置标签位置（在边界框顶部） 365 | text_marker.pose.position.x = (x_min + x_max) / 2 366 | text_marker.pose.position.y = (y_min + y_max) / 2 367 | text_marker.pose.position.z = z_max + 0.2 # 稍微高于边界框 368 | 369 | # 设置标签内容 370 | text_marker.text = "{}: {:.2f}".format(class_name, det['confidence']) 371 | 372 | # 设置标签大小 373 | text_marker.scale.z = 0.5 # 文本高度 374 | 375 | # 设置标签颜色（白色） 376 | text_marker.color.r = 1.0 377 | text_marker.color.g = 1.0 378 | text_marker.color.b = 1.0 379 | text_marker.color.a = 1.0 380 | 381 | # 设置标记的持续时间 382 | text_marker.lifetime = rospy.Duration(0.1) 383 | 384 | # 添加标记到数组 385 | markers_array.markers.append(text_marker) 386 | 387 | marker_id += 1 388 | 389 | return markers_array 390 | 391 | def main(): 392 | try: 393 | yolo_lidar_fusion = YoloLidarFusion() 394 | rospy.spin() 395 | except rospy.ROSInterruptException: 396 | pass 397 | 398 | if __name__ == '__main__': 399 | main() -------------------------------------------------------------------------------- /image_pointcloud_fusion/scripts/fusion_detection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import numpy as np 4 | import open3d as o3d 5 | import time 6 | import cv2 7 | from ultralytics import YOLO 8 | import threading 9 | from queue import Queue 10 | 11 | from sensor_msgs.msg import PointCloud2, PointField, Image 12 | from cv_bridge import CvBridge, CvBridgeError 13 | import sensor_msgs.point_cloud2 as pc2 14 | from visualization_msgs.msg import Marker, MarkerArray 15 | from geometry_msgs.msg import PoseArray, Pose, Point 16 | from std_msgs.msg import Header, ColorRGBA 17 | from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray 18 | 19 | class MultiModalFusion: 20 | def __init__(self): 21 | # 初始化ROS节点 22 | rospy.init_node('multimodal_fusion', anonymous=True) 23 | # 读取ROS参数 24 | self.sensor_model = rospy.get_param('~sensor_model', 'HDL-32E') 25 | self.print_fps = rospy.get_param('~print_fps', True) 26 | self.leaf = rospy.get_param('~leaf', 1) 27 | self.z_axis_min = rospy.get_param('~z_axis_min', -0.5) 28 | self.z_axis_max = rospy.get_param('~z_axis_max', 1.5) 29 | self.cluster_size_min = rospy.get_param('~cluster_size_min', 10) 30 | self.cluster_size_max = rospy.get_param('~cluster_size_max', 2000) 31 | self.camera_topic = rospy.get_param('~camera_topic', '/camera/color/image_raw') 32 | self.lidar_topic = rospy.get_param('~lidar_topic', '/livox/lidar') 33 | self.yolo_confidence = rospy.get_param('~yolo_confidence', 0.5) 34 | self.yolo_model_path = rospy.get_param('~yolo_model_path', 'yolo11s.pt') 35 | 36 | """realsense 相机""" 37 | # 相机参数 38 | self.camera_matrix = np.array([ 39 | [909.783, 0, 650.365], 40 | [0, 909.004, 381.295], 41 | [0, 0, 1] 42 | ]) 43 | self.dist_coeffs = np.zeros(5) # 无畸变 44 | self.image_width = 1280 # 图像宽度 45 | self.image_height = 720 # 图像高度 46 | 47 | # 雷达到相机的变换矩阵 48 | self.lidar_to_camera = np.array([ 49 | [0.0, -1.0, 0.0, 0.03], 50 | [0.0, 0.0, -1.0, -0.022], 51 | [1.0, 0.0, 0.0, -0.04], 52 | [0, 0, 0, 1] 53 | ]) 54 | 55 | """广角相机""" 56 | # 相机内参 57 | # self.camera_matrix = np.array([ 58 | # [801.685, 0, 645.146], 59 | # [0, 800.967, 354.575], 60 | # [0, 0, 1] 61 | # ]) 62 | # self.dist_coeffs = np.array([-0.399, 0.208, 0, 0, -0.070]) # 畸变系数 63 | # self.image_width = 1280 # 图像宽度 64 | # self.image_height = 720 # 图像高度 65 | 66 | # # 雷达到相机的变换矩阵 67 | # self.lidar_to_camera = np.array([ 68 | # [0.0, -1.0, 0.0, 0.0], 69 | # [0.342, 0.0, -0.939, -0.1], 70 | # [0.939, 0.0, 0.342, -0.15], 71 | # [0, 0, 0, 1] 72 | # ]) 73 | 74 | # 初始化点云聚类参数 75 | self.region_max = 10 76 | self.regions = np.zeros(100, dtype=int) 77 | self.init_regions() 78 | 79 | # 性能统计 80 | self.frames = 0 81 | self.start_time = 0 82 | self.reset = True 83 | 84 | # 用于点云/图像处理的线程安全队列 85 | self.lidar_queue = Queue(maxsize=1) 86 | self.image_queue = Queue(maxsize=1) 87 | self.fusion_results = Queue(maxsize=10) 88 | 89 | # 初始化CV Bridge 90 | self.bridge = CvBridge() 91 | 92 | # 加载YOLO模型 93 | rospy.loginfo("正在加载YOLO模型") 94 | try: 95 | self.model = YOLO(self.yolo_model_path) 96 | rospy.loginfo("YOLO模型加载完成") 97 | except Exception as e: 98 | rospy.logerr(f"加载YOLO模型失败: {e}") 99 | self.model = None 100 | 101 | # 设置点云处理线程 102 | self.lidar_thread = threading.Thread(target=self.process_lidar_thread) 103 | self.lidar_thread.daemon = True 104 | self.lidar_thread.start() 105 | 106 | # 设置图像处理线程 107 | self.image_thread = threading.Thread(target=self.process_image_thread) 108 | self.image_thread.daemon = True 109 | self.image_thread.start() 110 | 111 | # 设置融合处理线程 112 | self.fusion_thread = threading.Thread(target=self.fusion_thread) 113 | self.fusion_thread.daemon = True 114 | self.fusion_thread.start() 115 | 116 | # 设置订阅者 117 | self.lidar_sub = rospy.Subscriber(self.lidar_topic, PointCloud2, self.lidar_callback, queue_size=1) 118 | self.image_sub = rospy.Subscriber(self.camera_topic, Image, self.image_callback, queue_size=1) 119 | 120 | # 设置发布者 121 | self.cloud_filtered_pub = rospy.Publisher('~cloud_filtered', PointCloud2, queue_size=1) 122 | self.cluster_marker_pub = rospy.Publisher('~cluster_markers', MarkerArray, queue_size=1) 123 | self.pose_array_pub = rospy.Publisher('~cluster_poses', PoseArray, queue_size=1) 124 | self.detection_image_pub = rospy.Publisher('~detection_image', Image, queue_size=1) 125 | self.bbox3d_pub = rospy.Publisher('~bounding_boxes3d', BoundingBoxArray, queue_size=1) 126 | 127 | rospy.loginfo("多模态融合节点初始化完成") 128 | 129 | def init_regions(self): 130 | """初始化不同传感器的区域大小""" 131 | if self.sensor_model == "VLP-16": 132 | self.regions[0:14] = [2, 3, 3, 3, 3, 3, 3, 2, 3, 3, 3, 3, 3, 3] 133 | elif self.sensor_model == "HDL-32E": 134 | self.regions[0:14] = [4, 5, 4, 5, 4, 5, 5, 4, 5, 4, 5, 5, 4, 5] 135 | elif self.sensor_model == "HDL-64E": 136 | self.regions[0:5] = [14, 14, 14, 15, 14] 137 | else: 138 | rospy.logfatal("Unknown sensor model!") 139 | 140 | def lidar_callback(self, lidar_msg): 141 | """LiDAR点云回调函数""" 142 | # 将消息放入队列 143 | if not self.lidar_queue.full(): 144 | self.lidar_queue.put(lidar_msg) 145 | 146 | def image_callback(self, image_msg): 147 | """图像回调函数""" 148 | # 将消息放入队列 149 | if not self.image_queue.full(): 150 | self.image_queue.put(image_msg) 151 | 152 | def process_lidar_thread(self): 153 | """处理LiDAR数据的线程""" 154 | while not rospy.is_shutdown(): 155 | try: 156 | if not self.lidar_queue.empty(): 157 | lidar_msg = self.lidar_queue.get() 158 | 159 | # 将ROS点云转换为numpy数组 160 | pc_array = self.ros_to_numpy(lidar_msg) 161 | 162 | # 创建Open3D点云 163 | pcd = o3d.geometry.PointCloud() 164 | pcd.points = o3d.utility.Vector3dVector(pc_array[:, :3]) 165 | 166 | # 处理点云 167 | filtered_pcd, clusters, centroids, boxes = self.process_point_cloud(pcd) 168 | 169 | # 发布过滤后的点云 170 | if self.cloud_filtered_pub.get_num_connections() > 0: 171 | filtered_points = np.asarray(filtered_pcd.points) 172 | filtered_cloud_msg = self.numpy_to_ros(lidar_msg.header, filtered_points) 173 | self.cloud_filtered_pub.publish(filtered_cloud_msg) 174 | 175 | # 发布未标记的聚类 176 | self.publish_cluster_markers(lidar_msg.header, clusters, boxes, None) 177 | 178 | # 存储处理结果，等待与图像检测结果融合 179 | lidar_data = { 180 | 'timestamp': lidar_msg.header.stamp, 181 | 'frame_id': lidar_msg.header.frame_id, 182 | 'header': lidar_msg.header, 183 | 'clusters': clusters, 184 | 'centroids': centroids, 185 | 'boxes': boxes 186 | } 187 | 188 | # 结果放入融合队列 189 | if not self.fusion_results.full(): 190 | self.fusion_results.put(('lidar', lidar_data)) 191 | 192 | time.sleep(0.001) # 避免CPU占用过高 193 | except Exception as e: 194 | rospy.logerr(f"点云处理线程错误: {e}") 195 | 196 | def process_image_thread(self): 197 | """处理图像数据的线程""" 198 | while not rospy.is_shutdown(): 199 | try: 200 | if not self.image_queue.empty() and self.model is not None: 201 | image_msg = self.image_queue.get() 202 | 203 | # 将ROS图像转换为OpenCV格式 204 | try: 205 | cv_img = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") 206 | except CvBridgeError as e: 207 | rospy.logerr(f"CV Bridge错误: {e}") 208 | continue 209 | 210 | # 运行YOLO目标检测 211 | results = self.model(cv_img, conf=self.yolo_confidence) 212 | 213 | # 创建处理后的图像 214 | processed_img = cv_img.copy() 215 | 216 | # 提取检测结果 217 | yolo_detections = [] 218 | 219 | for result in results: 220 | boxes = result.boxes 221 | for box in boxes: 222 | x1, y1, x2, y2 = box.xyxy[0].cpu().numpy() 223 | conf = box.conf[0].item() 224 | cls = int(box.cls[0].item()) 225 | class_name = self.model.names[cls] 226 | 227 | # 转换为整数坐标 228 | x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) 229 | 230 | # 绘制边界框和标签 231 | label = f'{class_name} {conf:.2f}' 232 | cv2.rectangle(processed_img, (x1, y1), (x2, y2), (255, 0, 0), 2) 233 | cv2.putText(processed_img, label, (x1, y1-10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) 234 | 235 | yolo_detections.append({ 236 | 'bbox': (x1, y1, x2, y2), 237 | 'confidence': conf, 238 | 'class_id': cls, 239 | 'class_name': class_name 240 | }) 241 | 242 | # 发布处理后的图像 243 | try: 244 | detection_image_msg = self.bridge.cv2_to_imgmsg(processed_img, "bgr8") 245 | detection_image_msg.header = image_msg.header 246 | self.detection_image_pub.publish(detection_image_msg) 247 | except CvBridgeError as e: 248 | rospy.logerr(f"CV Bridge错误: {e}") 249 | 250 | # 存储处理结果，等待与点云聚类结果融合 251 | image_data = { 252 | 'timestamp': image_msg.header.stamp, 253 | 'frame_id': image_msg.header.frame_id, 254 | 'header': image_msg.header, 255 | 'detections': yolo_detections, 256 | 'image': cv_img, 257 | 'processed_image': processed_img 258 | } 259 | 260 | # 结果放入融合队列 261 | if not self.fusion_results.full(): 262 | self.fusion_results.put(('image', image_data)) 263 | 264 | time.sleep(0.001) # 避免CPU占用过高 265 | except Exception as e: 266 | rospy.logerr(f"图像处理线程错误: {e}") 267 | 268 | def fusion_thread(self): 269 | """融合LiDAR和图像数据的线程""" 270 | lidar_data = None 271 | image_data = None 272 | 273 | while not rospy.is_shutdown(): 274 | try: 275 | if not self.fusion_results.empty(): 276 | data_type, data = self.fusion_results.get() 277 | 278 | if data_type == 'lidar': 279 | lidar_data = data 280 | elif data_type == 'image': 281 | image_data = data 282 | 283 | # 检查是否有足够的数据进行融合 284 | if lidar_data is not None and image_data is not None: 285 | # 检查时间戳是否足够接近 286 | lidar_time = lidar_data['timestamp'] 287 | image_time = image_data['timestamp'] 288 | time_diff = abs((lidar_time - image_time).to_sec()) 289 | 290 | if time_diff < 0.1: # 100ms内的数据视为同步 291 | self.fuse_lidar_and_image(lidar_data, image_data) 292 | # 清除已处理的数据 293 | lidar_data = None 294 | image_data = None 295 | elif lidar_time < image_time: 296 | # LiDAR数据太旧，丢弃 297 | lidar_data = None 298 | else: 299 | # 图像数据太旧，丢弃 300 | image_data = None 301 | 302 | time.sleep(0.001) # 避免CPU占用过高 303 | except Exception as e: 304 | rospy.logerr(f"融合线程错误: {e}") 305 | 306 | def fuse_lidar_and_image(self, lidar_data, image_data): 307 | """融合LiDAR聚类和图像检测结果""" 308 | try: 309 | # 获取变换矩阵 (使用提供的雷达到相机的变换矩阵) 310 | transform_matrix = self.lidar_to_camera 311 | 312 | # 从聚类中提取边界框 313 | clusters = lidar_data['clusters'] 314 | boxes = lidar_data['boxes'] 315 | detections = image_data['detections'] 316 | cv_image = image_data['processed_image'].copy() # 使用已处理的图像 317 | 318 | # 创建带有类别的3D边界框消息 319 | bbox3d_array = BoundingBoxArray() 320 | bbox3d_array.header = lidar_data['header'] 321 | 322 | # 用于标记的聚类 323 | classified_clusters = [] 324 | 325 | # 处理每个3D边界框 326 | for cluster_idx, box in enumerate(boxes): 327 | min_point = box['min'] 328 | max_point = box['max'] 329 | 330 | # 创建边界框角点 331 | corners = np.array([ 332 | [min_point[0], min_point[1], min_point[2], 1], 333 | [max_point[0], min_point[1], min_point[2], 1], 334 | [max_point[0], max_point[1], min_point[2], 1], 335 | [min_point[0], max_point[1], min_point[2], 1], 336 | [min_point[0], min_point[1], max_point[2], 1], 337 | [max_point[0], min_point[1], max_point[2], 1], 338 | [max_point[0], max_point[1], max_point[2], 1], 339 | [min_point[0], max_point[1], max_point[2], 1] 340 | ]) 341 | 342 | # 将角点从LiDAR坐标系转换到相机坐标系 343 | camera_corners = [] 344 | for corner in corners: 345 | # 应用变换 346 | camera_corner = np.dot(transform_matrix, corner) 347 | camera_corners.append(camera_corner[:3]) 348 | 349 | camera_corners = np.array(camera_corners) 350 | 351 | # 将角点从相机3D坐标投影到图像平面 352 | image_points, _ = cv2.projectPoints( 353 | camera_corners, 354 | np.zeros(3), # 已在坐标系中，无需旋转 355 | np.zeros(3), # 已在坐标系中，无需平移 356 | self.camera_matrix, 357 | self.dist_coeffs 358 | ) 359 | image_points = image_points.reshape(-1, 2) 360 | 361 | # 检查点是否在图像前方（Z > 0） 362 | if np.any(np.array([p[2] for p in camera_corners]) <= 0): 363 | continue # 边界框在相机后方，跳过 364 | 365 | # 将3D边界框投影到图像上 366 | # 绘制底面 367 | for i in range(4): 368 | pt1 = (int(image_points[i][0]), int(image_points[i][1])) 369 | pt2 = (int(image_points[(i+1)%4][0]), int(image_points[(i+1)%4][1])) 370 | cv2.line(cv_image, pt1, pt2, (0, 0, 255), 2) 371 | 372 | # 绘制顶面 373 | for i in range(4): 374 | pt1 = (int(image_points[i+4][0]), int(image_points[i+4][1])) 375 | pt2 = (int(image_points[((i+1)%4)+4][0]), int(image_points[((i+1)%4)+4][1])) 376 | cv2.line(cv_image, pt1, pt2, (0, 0, 255), 2) 377 | 378 | # 绘制侧面 379 | for i in range(4): 380 | pt1 = (int(image_points[i][0]), int(image_points[i][1])) 381 | pt2 = (int(image_points[i+4][0]), int(image_points[i+4][1])) 382 | cv2.line(cv_image, pt1, pt2, (0, 0, 255), 2) 383 | 384 | # 计算边界框在图像中的2D投影包围框 385 | x_coords = image_points[:, 0] 386 | y_coords = image_points[:, 1] 387 | 388 | # 如果边界框完全超出图像，则跳过 389 | if np.all(x_coords < 0) or np.all(x_coords >= self.image_width) or \ 390 | np.all(y_coords < 0) or np.all(y_coords >= self.image_height): 391 | continue 392 | 393 | # 将超出图像范围的坐标限制在图像边界内 394 | x_coords = np.clip(x_coords, 0, self.image_width - 1) 395 | y_coords = np.clip(y_coords, 0, self.image_height - 1) 396 | 397 | x_min = int(np.min(x_coords)) 398 | y_min = int(np.min(y_coords)) 399 | x_max = int(np.max(x_coords)) 400 | y_max = int(np.max(y_coords)) 401 | 402 | # 与2D检测框进行融合 403 | best_iou = 0 404 | best_detection = None 405 | 406 | for detection in detections: 407 | det_x1, det_y1, det_x2, det_y2 = detection['bbox'] 408 | 409 | # 计算IoU 410 | x_left = max(x_min, det_x1) 411 | y_top = max(y_min, det_y1) 412 | x_right = min(x_max, det_x2) 413 | y_bottom = min(y_max, det_y2) 414 | 415 | if x_right < x_left or y_bottom < y_top: 416 | continue # 没有重叠 417 | 418 | intersection_area = (x_right - x_left) * (y_bottom - y_top) 419 | box1_area = (x_max - x_min) * (y_max - y_min) 420 | box2_area = (det_x2 - det_x1) * (det_y2 - det_y1) 421 | iou = intersection_area / float(box1_area + box2_area - intersection_area) 422 | 423 | if iou > best_iou and iou > 0.3: # IoU阈值 424 | best_iou = iou 425 | best_detection = detection 426 | 427 | # 如果找到匹配的检测，标记聚类 428 | if best_detection is not None: 429 | # 获取类别信息 430 | class_name = best_detection['class_name'] 431 | confidence = best_detection['confidence'] 432 | 433 | # 在3D边界框中心绘制类别标签 434 | centroid = np.mean(image_points, axis=0).astype(int) 435 | label = f"{class_name}: {confidence:.2f}" 436 | cv2.putText(cv_image, label, (centroid[0], centroid[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 2) 437 | 438 | # 添加到已分类的聚类列表 439 | classified_clusters.append({ 440 | 'cluster_idx': cluster_idx, 441 | 'class_name': class_name, 442 | 'confidence': confidence, 443 | 'box': box 444 | }) 445 | 446 | # 创建3D边界框消息 447 | bbox3d = BoundingBox() 448 | bbox3d.header = lidar_data['header'] 449 | bbox3d.pose.position.x = (min_point[0] + max_point[0]) / 2 450 | bbox3d.pose.position.y = (min_point[1] + max_point[1]) / 2 451 | bbox3d.pose.position.z = (min_point[2] + max_point[2]) / 2 452 | bbox3d.pose.orientation.w = 1.0 # 单位四元数，无旋转 453 | bbox3d.dimensions.x = max_point[0] - min_point[0] 454 | bbox3d.dimensions.y = max_point[1] - min_point[1] 455 | bbox3d.dimensions.z = max_point[2] - min_point[2] 456 | bbox3d.value = best_detection['class_id'] # 使用类别ID作为值 457 | bbox3d.label = best_detection['class_id'] 458 | 459 | bbox3d_array.boxes.append(bbox3d) 460 | 461 | # 发布3D边界框消息 462 | self.bbox3d_pub.publish(bbox3d_array) 463 | 464 | # 发布带有分类信息的聚类标记 465 | self.publish_cluster_markers(lidar_data['header'], lidar_data['clusters'], lidar_data['boxes'], classified_clusters) 466 | 467 | # 发布融合后的图像 468 | try: 469 | fusion_image_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") 470 | fusion_image_msg.header = image_data['header'] 471 | self.detection_image_pub.publish(fusion_image_msg) 472 | except CvBridgeError as e: 473 | rospy.logerr(f"CV Bridge错误: {e}") 474 | 475 | except Exception as e: 476 | rospy.logerr(f"融合处理错误: {e}") 477 | 478 | def ros_to_numpy(self, ros_point_cloud): 479 | """将ROS点云转换为numpy数组""" 480 | points_list = [] 481 | for point in pc2.read_points(ros_point_cloud, field_names=("x", "y", "z", "intensity"), skip_nans=True): 482 | points_list.append(point) 483 | return np.array(points_list) 484 | 485 | def numpy_to_ros(self, header, points): 486 | """将numpy数组转换为ROS点云""" 487 | fields = [ 488 | PointField('x', 0, PointField.FLOAT32, 1), 489 | PointField('y', 4, PointField.FLOAT32, 1), 490 | PointField('z', 8, PointField.FLOAT32, 1), 491 | PointField('intensity', 12, PointField.FLOAT32, 1), 492 | ] 493 | 494 | # 确保点云有intensity字段，如果没有则添加默认值0 495 | if points.shape[1] == 3: 496 | points_with_intensity = np.column_stack([points, np.zeros(len(points))]) 497 | else: 498 | points_with_intensity = points 499 | 500 | return pc2.create_cloud(header, fields, points_with_intensity) 501 | 502 | def filter_point_cloud(self, pcd): 503 | """降采样和高度过滤""" 504 | points = np.asarray(pcd.points) 505 | 506 | # 高度过滤和降采样 507 | indices = [] 508 | for i in range(0, len(points), self.leaf): 509 | if self.z_axis_min <= points[i, 2] <= self.z_axis_max: 510 | indices.append(i) 511 | 512 | filtered_points = points[indices] 513 | filtered_pcd = o3d.geometry.PointCloud() 514 | filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) 515 | 516 | return filtered_pcd, indices 517 | 518 | def divide_into_regions(self, pcd, indices): 519 | """将点云分成嵌套的环形区域""" 520 | points = np.asarray(pcd.points) 521 | indices_array = [[] for _ in range(self.region_max)] 522 | 523 | for i in range(len(indices)): 524 | point_idx = indices[i] 525 | point = points[point_idx] 526 | 527 | # 计算点到原点的距离 528 | distance = np.sqrt(point[0]**2 + point[1]**2 + point[2]**2) 529 | 530 | range_val = 0.0 531 | for j in range(self.region_max): 532 | next_range = range_val + self.regions[j] 533 | if range_val < distance <= next_range: 534 | indices_array[j].append(point_idx) 535 | break 536 | range_val = next_range 537 | 538 | return indices_array 539 | 540 | def euclidean_clustering(self, pcd, indices_array): 541 | """对每个区域执行欧几里得聚类""" 542 | all_clusters = [] 543 | boxes = [] 544 | points = np.asarray(pcd.points) 545 | 546 | tolerance = 0.0 547 | for i in range(self.region_max): 548 | tolerance += 0.1 # 容差随区域递增 549 | 550 | if len(indices_array[i]) > self.cluster_size_min: 551 | # 创建该区域的点云 552 | region_pcd = o3d.geometry.PointCloud() 553 | region_pcd.points = o3d.utility.Vector3dVector(points[indices_array[i]]) 554 | 555 | # 使用Open3D的DBSCAN聚类 556 | with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Error): 557 | labels = np.array(region_pcd.cluster_dbscan(eps=tolerance, min_points=self.cluster_size_min, print_progress=False)) 558 | 559 | # 处理聚类结果 560 | max_label = labels.max() if len(labels) > 0 and labels.max() > -1 else -1 561 | for j in range(max_label + 1): 562 | cluster_indices = np.where(labels == j)[0] 563 | if self.cluster_size_min <= len(cluster_indices) <= self.cluster_size_max: 564 | # 获取该聚类的原始点云索引 565 | original_indices = [indices_array[i][idx] for idx in cluster_indices] 566 | 567 | # 创建该聚类的点云 568 | cluster_pcd = o3d.geometry.PointCloud() 569 | cluster_pcd.points = o3d.utility.Vector3dVector(points[original_indices]) 570 | 571 | # 计算边界框 572 | cluster_points = np.asarray(cluster_pcd.points) 573 | min_point = np.min(cluster_points, axis=0) 574 | max_point = np.max(cluster_points, axis=0) 575 | 576 | box = { 577 | 'min': min_point, 578 | 'max': max_point 579 | } 580 | 581 | all_clusters.append(cluster_pcd) 582 | boxes.append(box) 583 | 584 | return all_clusters, boxes 585 | 586 | def compute_centroids(self, clusters): 587 | """计算每个聚类的中心点""" 588 | centroids = [] 589 | for cluster in clusters: 590 | centroid = np.mean(np.asarray(cluster.points), axis=0) 591 | centroids.append(centroid) 592 | return centroids 593 | 594 | def process_point_cloud(self, pcd): 595 | """处理点云的主函数""" 596 | # 1. 降采样和高度过滤 597 | filtered_pcd, indices = self.filter_point_cloud(pcd) 598 | # 2. 点云分区 599 | indices_array = self.divide_into_regions(pcd, indices) 600 | # 3. 欧几里得聚类 601 | clusters, boxes = self.euclidean_clustering(pcd, indices_array) 602 | # 4. 计算聚类中心 603 | centroids = self.compute_centroids(clusters) 604 | return filtered_pcd, clusters, centroids, boxes 605 | 606 | def publish_cluster_markers(self, header, clusters, boxes, classified_clusters): 607 | """发布聚类标记""" 608 | clear_marker = Marker() 609 | clear_marker.header = header 610 | clear_marker.ns = "adaptive_clustering_labels" 611 | clear_marker.id = 0 612 | clear_marker.action = Marker.DELETEALL 613 | 614 | clear_array = MarkerArray() 615 | clear_array.markers.append(clear_marker) 616 | self.cluster_marker_pub.publish(clear_array) 617 | # 发布位姿数组 618 | if self.pose_array_pub.get_num_connections() > 0: 619 | pose_array = PoseArray() 620 | pose_array.header = header 621 | 622 | for i, cluster in enumerate(clusters): 623 | # 计算质心 624 | centroid = np.mean(np.asarray(cluster.points), axis=0) 625 | 626 | pose = Pose() 627 | pose.position.x = centroid[0] 628 | pose.position.y = centroid[1] 629 | pose.position.z = centroid[2] 630 | pose.orientation.w = 1.0 631 | 632 | pose_array.poses.append(pose) 633 | 634 | self.pose_array_pub.publish(pose_array) 635 | 636 | # 发布边界框标记 637 | if self.cluster_marker_pub.get_num_connections() > 0: 638 | marker_array = MarkerArray() 639 | 640 | for i, box in enumerate(boxes): 641 | min_point = box['min'] 642 | max_point = box['max'] 643 | 644 | # 查找是否有分类信息 645 | classification = None 646 | if classified_clusters is not None: 647 | for cls in classified_clusters: 648 | if cls['cluster_idx'] == i: 649 | classification = cls 650 | break 651 | 652 | # 创建边界框标记 653 | marker = Marker() 654 | marker.header = header 655 | marker.ns = "adaptive_clustering" 656 | marker.id = i 657 | marker.type = Marker.LINE_LIST 658 | marker.action = Marker.ADD 659 | marker.scale.x = 0.02 # 线宽 660 | 661 | # 设置颜色 - 未分类为绿色，已分类根据类别设置不同颜色 662 | if classification is None: 663 | marker.color.a = 1.0 664 | marker.color.r = 0.0 665 | marker.color.g = 1.0 666 | marker.color.b = 0.5 667 | else: 668 | # 使用类别名称生成颜色 669 | class_name = classification['class_name'] 670 | 671 | # 为不同类别设置不同颜色 672 | if class_name in ['person', 'pedestrian']: 673 | marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # 红色 674 | elif class_name in ['car', 'vehicle', 'truck', 'bus']: 675 | marker.color = ColorRGBA(0.0, 0.0, 1.0, 1.0) # 蓝色 676 | elif class_name in ['bicycle', 'bike']: 677 | marker.color = ColorRGBA(1.0, 0.5, 0.0, 1.0) # 橙色 678 | else: 679 | marker.color = ColorRGBA(0.8, 0.2, 0.8, 1.0) # 紫色 680 | 681 | # 创建边界框角点 682 | p = [None] * 24 683 | p[0] = Point(max_point[0], max_point[1], max_point[2]) 684 | p[1] = Point(min_point[0], max_point[1], max_point[2]) 685 | p[2] = Point(max_point[0], max_point[1], max_point[2]) 686 | p[3] = Point(max_point[0], min_point[1], max_point[2]) 687 | p[4] = Point(max_point[0], max_point[1], max_point[2]) 688 | p[5] = Point(max_point[0], max_point[1], min_point[2]) 689 | p[6] = Point(min_point[0], min_point[1], min_point[2]) 690 | p[7] = Point(max_point[0], min_point[1], min_point[2]) 691 | p[8] = Point(min_point[0], min_point[1], min_point[2]) 692 | p[9] = Point(min_point[0], max_point[1], min_point[2]) 693 | p[10] = Point(min_point[0], min_point[1], min_point[2]) 694 | p[11] = Point(min_point[0], min_point[1], max_point[2]) 695 | p[12] = Point(min_point[0], max_point[1], max_point[2]) 696 | p[13] = Point(min_point[0], max_point[1], min_point[2]) 697 | p[14] = Point(min_point[0], max_point[1], max_point[2]) 698 | p[15] = Point(min_point[0], min_point[1], max_point[2]) 699 | p[16] = Point(max_point[0], min_point[1], max_point[2]) 700 | p[17] = Point(max_point[0], min_point[1], min_point[2]) 701 | p[18] = Point(max_point[0], min_point[1], max_point[2]) 702 | p[19] = Point(min_point[0], min_point[1], max_point[2]) 703 | p[20] = Point(max_point[0], max_point[1], min_point[2]) 704 | p[21] = Point(min_point[0], max_point[1], min_point[2]) 705 | p[22] = Point(max_point[0], max_point[1], min_point[2]) 706 | p[23] = Point(max_point[0], min_point[1], min_point[2]) 707 | 708 | # 添加所有点到标记 709 | for point in p: 710 | marker.points.append(point) 711 | 712 | # 如果已分类，添加文本标记 713 | if classification is not None: 714 | text_marker = Marker() 715 | text_marker.header = header 716 | text_marker.ns = "adaptive_clustering_labels" 717 | text_marker.id = i 718 | text_marker.type = Marker.TEXT_VIEW_FACING 719 | text_marker.action = Marker.ADD 720 | 721 | # 文本位置（在边界框上方） 722 | text_marker.pose.position.x = (min_point[0] + max_point[0]) / 2 723 | text_marker.pose.position.y = (min_point[1] + max_point[1]) / 2 724 | text_marker.pose.position.z = max_point[2] + 0.5 725 | text_marker.pose.orientation.w = 1.0 726 | 727 | # 文本内容 728 | text = f"{classification['class_name']}: {classification['confidence']:.2f}" 729 | text_marker.text = text 730 | 731 | # 文本样式 732 | text_marker.scale.z = 0.4 # 文本大小 733 | text_marker.color = marker.color # 与边界框相同的颜色 734 | marker_array.markers.append(text_marker) 735 | 736 | marker.lifetime = rospy.Duration(0.1) 737 | marker_array.markers.append(marker) 738 | 739 | if marker_array.markers: 740 | self.cluster_marker_pub.publish(marker_array) 741 | 742 | if __name__ == "__main__": 743 | try: 744 | fusion = MultiModalFusion() 745 | rospy.spin() 746 | except rospy.ROSInterruptException: 747 | pass -------------------------------------------------------------------------------- /image_pointcloud_fusion/scripts/imgpc_fusion_detection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import numpy as np 4 | import open3d as o3d 5 | import time 6 | import cv2 7 | from ultralytics import YOLO 8 | import threading 9 | from queue import Queue 10 | 11 | from sensor_msgs.msg import PointCloud2, PointField, Image 12 | from cv_bridge import CvBridge, CvBridgeError 13 | import sensor_msgs.point_cloud2 as pc2 14 | from visualization_msgs.msg import Marker, MarkerArray 15 | from geometry_msgs.msg import PoseArray, Pose, Point 16 | from std_msgs.msg import Header, ColorRGBA 17 | from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray 18 | 19 | class MultiModalFusion: 20 | def __init__(self): 21 | # 初始化ROS节点 22 | rospy.init_node('multimodal_fusion', anonymous=True) 23 | # 读取ROS参数 24 | self.sensor_model = rospy.get_param('~sensor_model', 'HDL-32E') 25 | self.print_fps = rospy.get_param('~print_fps', True) 26 | self.leaf = rospy.get_param('~leaf', 1) 27 | self.z_axis_min = rospy.get_param('~z_axis_min', -0.5) 28 | self.z_axis_max = rospy.get_param('~z_axis_max', 1.8) 29 | self.cluster_size_min = rospy.get_param('~cluster_size_min', 3) 30 | self.cluster_size_max = rospy.get_param('~cluster_size_max', 2200000) 31 | self.camera_topic = rospy.get_param('~camera_topic', '/camera/color/image_raw') 32 | self.lidar_topic = rospy.get_param('~lidar_topic', '/livox/lidar') 33 | self.yolo_confidence = rospy.get_param('~yolo_confidence', 0.5) 34 | self.yolo_model_path = rospy.get_param('~yolo_model_path', 'yolo11s.pt') 35 | 36 | """realsense 相机""" 37 | # 相机参数 38 | self.camera_matrix = np.array([ 39 | [909.783, 0, 650.365], 40 | [0, 909.004, 381.295], 41 | [0, 0, 1] 42 | ]) 43 | self.dist_coeffs = np.zeros(5) # 无畸变 44 | self.image_width = 1280 # 图像宽度 45 | self.image_height = 720 # 图像高度 46 | 47 | # 雷达到相机的变换矩阵 48 | self.lidar_to_camera = np.array([ 49 | [0.0, -1.0, 0.0, 0.03], 50 | [0.0, 0.0, -1.0, -0.022], 51 | [1.0, 0.0, 0.0, -0.04], 52 | [0, 0, 0, 1] 53 | ]) 54 | 55 | """广角相机""" 56 | # 相机内参 57 | # self.camera_matrix = np.array([ 58 | # [801.685, 0, 645.146], 59 | # [0, 800.967, 354.575], 60 | # [0, 0, 1] 61 | # ]) 62 | # self.dist_coeffs = np.array([-0.399, 0.208, 0, 0, -0.070]) # 畸变系数 63 | # self.image_width = 1280 # 图像宽度 64 | # self.image_height = 720 # 图像高度 65 | 66 | # # 雷达到相机的变换矩阵 67 | # self.lidar_to_camera = np.array([ 68 | # [0.0, -1.0, 0.0, 0.0], 69 | # [0.342, 0.0, -0.939, -0.1], 70 | # [0.939, 0.0, 0.342, -0.15], 71 | # [0, 0, 0, 1] 72 | # ]) 73 | 74 | # 初始化点云聚类参数 75 | self.region_max = 10 76 | self.regions = np.zeros(100, dtype=int) 77 | self.init_regions() 78 | 79 | # 性能统计 80 | self.frames = 0 81 | self.start_time = 0 82 | self.reset = True 83 | 84 | # 用于点云/图像处理的线程安全队列 85 | self.lidar_queue = Queue(maxsize=1) 86 | self.image_queue = Queue(maxsize=1) 87 | self.fusion_results = Queue(maxsize=10) 88 | 89 | # 初始化CV Bridge 90 | self.bridge = CvBridge() 91 | 92 | # 加载YOLO模型 93 | rospy.loginfo("正在加载YOLO模型") 94 | try: 95 | self.model = YOLO(self.yolo_model_path) 96 | rospy.loginfo("YOLO模型加载完成") 97 | except Exception as e: 98 | rospy.logerr(f"加载YOLO模型失败: {e}") 99 | self.model = None 100 | 101 | # 设置点云处理线程 102 | self.lidar_thread = threading.Thread(target=self.process_lidar_thread) 103 | self.lidar_thread.daemon = True 104 | self.lidar_thread.start() 105 | 106 | # 设置图像处理线程 107 | self.image_thread = threading.Thread(target=self.process_image_thread) 108 | self.image_thread.daemon = True 109 | self.image_thread.start() 110 | 111 | # 设置融合处理线程 112 | self.fusion_thread = threading.Thread(target=self.fusion_thread) 113 | self.fusion_thread.daemon = True 114 | self.fusion_thread.start() 115 | 116 | # 设置订阅者 117 | self.lidar_sub = rospy.Subscriber(self.lidar_topic, PointCloud2, self.lidar_callback, queue_size=1) 118 | self.image_sub = rospy.Subscriber(self.camera_topic, Image, self.image_callback, queue_size=1) 119 | 120 | # 设置发布者 121 | self.cloud_filtered_pub = rospy.Publisher('~cloud_filtered', PointCloud2, queue_size=1) 122 | self.cluster_marker_pub = rospy.Publisher('~cluster_markers', MarkerArray, queue_size=1) 123 | self.pose_array_pub = rospy.Publisher('~cluster_poses', PoseArray, queue_size=1) 124 | self.detection_image_pub = rospy.Publisher('~detection_image', Image, queue_size=1) 125 | self.bbox3d_pub = rospy.Publisher('~bounding_boxes3d', BoundingBoxArray, queue_size=1) 126 | 127 | rospy.loginfo("多模态融合节点初始化完成") 128 | 129 | def init_regions(self): 130 | """初始化不同传感器的区域大小""" 131 | if self.sensor_model == "VLP-16": 132 | self.regions[0:14] = [2, 3, 3, 3, 3, 3, 3, 2, 3, 3, 3, 3, 3, 3] 133 | elif self.sensor_model == "HDL-32E": 134 | self.regions[0:14] = [4, 5, 4, 5, 4, 5, 5, 4, 5, 4, 5, 5, 4, 5] 135 | elif self.sensor_model == "HDL-64E": 136 | self.regions[0:5] = [14, 14, 14, 15, 14] 137 | else: 138 | rospy.logfatal("Unknown sensor model!") 139 | 140 | def lidar_callback(self, lidar_msg): 141 | """LiDAR点云回调函数""" 142 | # 将消息放入队列 143 | if not self.lidar_queue.full(): 144 | self.lidar_queue.put(lidar_msg) 145 | 146 | def image_callback(self, image_msg): 147 | """图像回调函数""" 148 | # 将消息放入队列 149 | if not self.image_queue.full(): 150 | self.image_queue.put(image_msg) 151 | 152 | def process_lidar_thread(self): 153 | """处理LiDAR数据的线程""" 154 | while not rospy.is_shutdown(): 155 | try: 156 | if not self.lidar_queue.empty(): 157 | lidar_msg = self.lidar_queue.get() 158 | 159 | # 将ROS点云转换为numpy数组 160 | pc_array = self.ros_to_numpy(lidar_msg) 161 | 162 | # 创建Open3D点云 163 | pcd = o3d.geometry.PointCloud() 164 | pcd.points = o3d.utility.Vector3dVector(pc_array[:, :3]) 165 | 166 | # 处理点云 167 | filtered_pcd, clusters, centroids, boxes = self.process_point_cloud(pcd) 168 | 169 | # 发布过滤后的点云 170 | if self.cloud_filtered_pub.get_num_connections() > 0: 171 | filtered_points = np.asarray(filtered_pcd.points) 172 | filtered_cloud_msg = self.numpy_to_ros(lidar_msg.header, filtered_points) 173 | self.cloud_filtered_pub.publish(filtered_cloud_msg) 174 | 175 | # 注意：不再在这里发布未标记的聚类边界框 176 | self.publish_cluster_markers(lidar_msg.header, clusters, boxes, None) 177 | 178 | # 存储处理结果，等待与图像检测结果融合 179 | lidar_data = { 180 | 'timestamp': lidar_msg.header.stamp, 181 | 'frame_id': lidar_msg.header.frame_id, 182 | 'header': lidar_msg.header, 183 | 'clusters': clusters, 184 | 'centroids': centroids, 185 | 'boxes': boxes 186 | } 187 | 188 | # 结果放入融合队列 189 | if not self.fusion_results.full(): 190 | self.fusion_results.put(('lidar', lidar_data)) 191 | 192 | time.sleep(0.001) # 避免CPU占用过高 193 | except Exception as e: 194 | rospy.logerr(f"点云处理线程错误: {e}") 195 | 196 | def process_image_thread(self): 197 | """处理图像数据的线程""" 198 | while not rospy.is_shutdown(): 199 | try: 200 | if not self.image_queue.empty() and self.model is not None: 201 | image_msg = self.image_queue.get() 202 | 203 | # 将ROS图像转换为OpenCV格式 204 | try: 205 | cv_img = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") 206 | except CvBridgeError as e: 207 | rospy.logerr(f"CV Bridge错误: {e}") 208 | continue 209 | 210 | # 运行YOLO目标检测 211 | results = self.model(cv_img, conf=self.yolo_confidence) 212 | 213 | # 创建处理后的图像 214 | processed_img = cv_img.copy() 215 | 216 | # 提取检测结果 217 | yolo_detections = [] 218 | 219 | for result in results: 220 | boxes = result.boxes 221 | for box in boxes: 222 | x1, y1, x2, y2 = box.xyxy[0].cpu().numpy() 223 | conf = box.conf[0].item() 224 | cls = int(box.cls[0].item()) 225 | class_name = self.model.names[cls] 226 | 227 | # 转换为整数坐标 228 | x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) 229 | 230 | # 绘制边界框和标签 231 | label = f'{class_name} {conf:.2f}' 232 | cv2.rectangle(processed_img, (x1, y1), (x2, y2), (255, 0, 0), 2) 233 | cv2.putText(processed_img, label, (x1, y1-10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) 234 | 235 | yolo_detections.append({ 236 | 'bbox': (x1, y1, x2, y2), 237 | 'confidence': conf, 238 | 'class_id': cls, 239 | 'class_name': class_name 240 | }) 241 | 242 | # 发布处理后的图像 243 | try: 244 | detection_image_msg = self.bridge.cv2_to_imgmsg(processed_img, "bgr8") 245 | detection_image_msg.header = image_msg.header 246 | self.detection_image_pub.publish(detection_image_msg) 247 | except CvBridgeError as e: 248 | rospy.logerr(f"CV Bridge错误: {e}") 249 | 250 | # 存储处理结果，等待与点云聚类结果融合 251 | image_data = { 252 | 'timestamp': image_msg.header.stamp, 253 | 'frame_id': image_msg.header.frame_id, 254 | 'header': image_msg.header, 255 | 'detections': yolo_detections, 256 | 'image': cv_img, 257 | 'processed_image': processed_img 258 | } 259 | 260 | # 结果放入融合队列 261 | if not self.fusion_results.full(): 262 | self.fusion_results.put(('image', image_data)) 263 | 264 | time.sleep(0.001) # 避免CPU占用过高 265 | except Exception as e: 266 | rospy.logerr(f"图像处理线程错误: {e}") 267 | 268 | def fusion_thread(self): 269 | """融合LiDAR和图像数据的线程""" 270 | lidar_data = None 271 | image_data = None 272 | 273 | while not rospy.is_shutdown(): 274 | try: 275 | if not self.fusion_results.empty(): 276 | data_type, data = self.fusion_results.get() 277 | 278 | if data_type == 'lidar': 279 | lidar_data = data 280 | elif data_type == 'image': 281 | image_data = data 282 | 283 | # 检查是否有足够的数据进行融合 284 | if lidar_data is not None and image_data is not None: 285 | # 检查时间戳是否足够接近 286 | lidar_time = lidar_data['timestamp'] 287 | image_time = image_data['timestamp'] 288 | time_diff = abs((lidar_time - image_time).to_sec()) 289 | 290 | if time_diff < 0.1: # 100ms内的数据视为同步 291 | self.fuse_lidar_and_image(lidar_data, image_data) 292 | # 清除已处理的数据 293 | lidar_data = None 294 | image_data = None 295 | elif lidar_time < image_time: 296 | # LiDAR数据太旧，丢弃 297 | lidar_data = None 298 | else: 299 | # 图像数据太旧，丢弃 300 | image_data = None 301 | 302 | time.sleep(0.001) # 避免CPU占用过高 303 | except Exception as e: 304 | rospy.logerr(f"融合线程错误: {e}") 305 | 306 | def fuse_lidar_and_image(self, lidar_data, image_data): 307 | """融合LiDAR聚类和图像检测结果""" 308 | try: 309 | # 获取变换矩阵 (使用提供的雷达到相机的变换矩阵) 310 | transform_matrix = self.lidar_to_camera 311 | 312 | # 从聚类中提取边界框 313 | clusters = lidar_data['clusters'] 314 | boxes = lidar_data['boxes'] 315 | detections = image_data['detections'] 316 | cv_image = image_data['processed_image'].copy() # 使用已处理的图像 317 | 318 | # 创建带有类别的3D边界框消息 319 | bbox3d_array = BoundingBoxArray() 320 | bbox3d_array.header = lidar_data['header'] 321 | 322 | # 用于标记的聚类 323 | classified_clusters = [] 324 | 325 | # 处理每个3D边界框 326 | for cluster_idx, box in enumerate(boxes): 327 | min_point = box['min'] 328 | max_point = box['max'] 329 | 330 | # 创建边界框角点 331 | corners = np.array([ 332 | [min_point[0], min_point[1], min_point[2], 1], 333 | [max_point[0], min_point[1], min_point[2], 1], 334 | [max_point[0], max_point[1], min_point[2], 1], 335 | [min_point[0], max_point[1], min_point[2], 1], 336 | [min_point[0], min_point[1], max_point[2], 1], 337 | [max_point[0], min_point[1], max_point[2], 1], 338 | [max_point[0], max_point[1], max_point[2], 1], 339 | [min_point[0], max_point[1], max_point[2], 1] 340 | ]) 341 | 342 | # 将角点从LiDAR坐标系转换到相机坐标系 343 | camera_corners = [] 344 | for corner in corners: 345 | # 应用变换 346 | camera_corner = np.dot(transform_matrix, corner) 347 | camera_corners.append(camera_corner[:3]) 348 | 349 | camera_corners = np.array(camera_corners) 350 | 351 | # 将角点从相机3D坐标投影到图像平面 352 | image_points, _ = cv2.projectPoints( 353 | camera_corners, 354 | np.zeros(3), # 已在坐标系中，无需旋转zz 355 | np.zeros(3), # 已在坐标系中，无需平移 356 | self.camera_matrix, 357 | self.dist_coeffs 358 | ) 359 | image_points = image_points.reshape(-1, 2) 360 | 361 | # 检查点是否在图像前方（Z > 0） 362 | if np.any(np.array([p[2] for p in camera_corners]) <= 0): 363 | continue # 边界框在相机后方，跳过 364 | 365 | # 将3D边界框投影到图像上 366 | # 绘制底面 367 | for i in range(4): 368 | pt1 = (int(image_points[i][0]), int(image_points[i][1])) 369 | pt2 = (int(image_points[(i+1)%4][0]), int(image_points[(i+1)%4][1])) 370 | cv2.line(cv_image, pt1, pt2, (0, 0, 255), 2) 371 | 372 | # 绘制顶面 373 | for i in range(4): 374 | pt1 = (int(image_points[i+4][0]), int(image_points[i+4][1])) 375 | pt2 = (int(image_points[((i+1)%4)+4][0]), int(image_points[((i+1)%4)+4][1])) 376 | cv2.line(cv_image, pt1, pt2, (0, 0, 255), 2) 377 | 378 | # 绘制侧面 379 | for i in range(4): 380 | pt1 = (int(image_points[i][0]), int(image_points[i][1])) 381 | pt2 = (int(image_points[i+4][0]), int(image_points[i+4][1])) 382 | cv2.line(cv_image, pt1, pt2, (0, 0, 255), 2) 383 | 384 | # 计算边界框在图像中的2D投影包围框 385 | x_coords = image_points[:, 0] 386 | y_coords = image_points[:, 1] 387 | 388 | # 如果边界框完全超出图像，则跳过 389 | if np.all(x_coords < 0) or np.all(x_coords >= self.image_width) or \ 390 | np.all(y_coords < 0) or np.all(y_coords >= self.image_height): 391 | continue 392 | 393 | # 将超出图像范围的坐标限制在图像边界内 394 | x_coords = np.clip(x_coords, 0, self.image_width - 1) 395 | y_coords = np.clip(y_coords, 0, self.image_height - 1) 396 | 397 | x_min = int(np.min(x_coords)) 398 | y_min = int(np.min(y_coords)) 399 | x_max = int(np.max(x_coords)) 400 | y_max = int(np.max(y_coords)) 401 | 402 | # 与2D检测框进行融合 403 | best_iou = 0 404 | best_detection = None 405 | 406 | for detection in detections: 407 | det_x1, det_y1, det_x2, det_y2 = detection['bbox'] 408 | 409 | # 计算IoU 410 | x_left = max(x_min, det_x1) 411 | y_top = max(y_min, det_y1) 412 | x_right = min(x_max, det_x2) 413 | y_bottom = min(y_max, det_y2) 414 | 415 | if x_right < x_left or y_bottom < y_top: 416 | continue # 没有重叠 417 | 418 | intersection_area = (x_right - x_left) * (y_bottom - y_top) 419 | box1_area = (x_max - x_min) * (y_max - y_min) 420 | box2_area = (det_x2 - det_x1) * (det_y2 - det_y1) 421 | iou = intersection_area / float(box1_area + box2_area - intersection_area) 422 | 423 | if iou > best_iou and iou > 0.3: # IoU阈值 424 | best_iou = iou 425 | best_detection = detection 426 | 427 | # 如果找到匹配的检测，标记聚类 428 | if best_detection is not None: 429 | # 获取类别信息 430 | class_name = best_detection['class_name'] 431 | confidence = best_detection['confidence'] 432 | 433 | # 在3D边界框中心绘制类别标签 434 | centroid = np.mean(image_points, axis=0).astype(int) 435 | label = f"{class_name}: {confidence:.2f}" 436 | cv2.putText(cv_image, label, (centroid[0], centroid[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 2) 437 | 438 | # 添加到已分类的聚类列表 439 | classified_clusters.append({ 440 | 'cluster_idx': cluster_idx, 441 | 'class_name': class_name, 442 | 'confidence': confidence, 443 | 'box': box 444 | }) 445 | 446 | # 创建3D边界框消息 447 | bbox3d = BoundingBox() 448 | bbox3d.header = lidar_data['header'] 449 | bbox3d.pose.position.x = (min_point[0] + max_point[0]) / 2 450 | bbox3d.pose.position.y = (min_point[1] + max_point[1]) / 2 451 | bbox3d.pose.position.z = (min_point[2] + max_point[2]) / 2 452 | bbox3d.pose.orientation.w = 1.0 # 单位四元数，无旋转 453 | bbox3d.dimensions.x = max_point[0] - min_point[0] 454 | bbox3d.dimensions.y = max_point[1] - min_point[1] 455 | bbox3d.dimensions.z = max_point[2] - min_point[2] 456 | bbox3d.value = best_detection['class_id'] # 使用类别ID作为值 457 | bbox3d.label = best_detection['class_id'] 458 | 459 | bbox3d_array.boxes.append(bbox3d) 460 | 461 | # 发布3D边界框消息 462 | self.bbox3d_pub.publish(bbox3d_array) 463 | 464 | # 只发布已分类的聚类标记 465 | if classified_clusters: 466 | self.publish_cluster_markers(lidar_data['header'], lidar_data['clusters'], lidar_data['boxes'], classified_clusters) 467 | else: 468 | # 如果没有已分类的聚类，发布空的标记数组以清除先前的标记 469 | self.clear_markers(lidar_data['header']) 470 | 471 | # 发布融合后的图像 472 | try: 473 | fusion_image_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") 474 | fusion_image_msg.header = image_data['header'] 475 | self.detection_image_pub.publish(fusion_image_msg) 476 | except CvBridgeError as e: 477 | rospy.logerr(f"CV Bridge错误: {e}") 478 | 479 | except Exception as e: 480 | rospy.logerr(f"融合处理错误: {e}") 481 | 482 | def clear_markers(self, header): 483 | """清除所有标记""" 484 | clear_marker = Marker() 485 | clear_marker.header = header 486 | clear_marker.ns = "adaptive_clustering" 487 | clear_marker.id = 0 488 | clear_marker.action = Marker.DELETEALL 489 | 490 | clear_text_marker = Marker() 491 | clear_text_marker.header = header 492 | clear_text_marker.ns = "adaptive_clustering_labels" 493 | clear_text_marker.id = 0 494 | clear_text_marker.action = Marker.DELETEALL 495 | 496 | clear_array = MarkerArray() 497 | clear_array.markers.append(clear_marker) 498 | clear_array.markers.append(clear_text_marker) 499 | self.cluster_marker_pub.publish(clear_array) 500 | 501 | def ros_to_numpy(self, ros_point_cloud): 502 | """将ROS点云转换为numpy数组""" 503 | points_list = [] 504 | for point in pc2.read_points(ros_point_cloud, field_names=("x", "y", "z", "intensity"), skip_nans=True): 505 | points_list.append(point) 506 | return np.array(points_list) 507 | 508 | def numpy_to_ros(self, header, points): 509 | """将numpy数组转换为ROS点云""" 510 | fields = [ 511 | PointField('x', 0, PointField.FLOAT32, 1), 512 | PointField('y', 4, PointField.FLOAT32, 1), 513 | PointField('z', 8, PointField.FLOAT32, 1), 514 | PointField('intensity', 12, PointField.FLOAT32, 1), 515 | ] 516 | 517 | # 确保点云有intensity字段，如果没有则添加默认值0 518 | if points.shape[1] == 3: 519 | points_with_intensity = np.column_stack([points, np.zeros(len(points))]) 520 | else: 521 | points_with_intensity = points 522 | 523 | return pc2.create_cloud(header, fields, points_with_intensity) 524 | 525 | def filter_point_cloud(self, pcd): 526 | """降采样和高度过滤""" 527 | points = np.asarray(pcd.points) 528 | 529 | # 高度过滤和降采样 530 | indices = [] 531 | for i in range(0, len(points), self.leaf): 532 | if self.z_axis_min <= points[i, 2] <= self.z_axis_max: 533 | indices.append(i) 534 | 535 | filtered_points = points[indices] 536 | filtered_pcd = o3d.geometry.PointCloud() 537 | filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) 538 | 539 | return filtered_pcd, indices 540 | 541 | def divide_into_regions(self, pcd, indices): 542 | """将点云分成嵌套的环形区域""" 543 | points = np.asarray(pcd.points) 544 | indices_array = [[] for _ in range(self.region_max)] 545 | 546 | for i in range(len(indices)): 547 | point_idx = indices[i] 548 | point = points[point_idx] 549 | 550 | # 计算点到原点的距离 551 | distance = np.sqrt(point[0]**2 + point[1]**2 + point[2]**2) 552 | 553 | range_val = 0.0 554 | for j in range(self.region_max): 555 | next_range = range_val + self.regions[j] 556 | if range_val < distance <= next_range: 557 | indices_array[j].append(point_idx) 558 | break 559 | range_val = next_range 560 | 561 | return indices_array 562 | 563 | def euclidean_clustering(self, pcd, indices_array): 564 | """对每个区域执行欧几里得聚类""" 565 | all_clusters = [] 566 | boxes = [] 567 | points = np.asarray(pcd.points) 568 | 569 | tolerance = 0.0 570 | for i in range(self.region_max): 571 | tolerance += 0.1 # 容差随区域递增 572 | 573 | if len(indices_array[i]) > self.cluster_size_min: 574 | # 创建该区域的点云 575 | region_pcd = o3d.geometry.PointCloud() 576 | region_pcd.points = o3d.utility.Vector3dVector(points[indices_array[i]]) 577 | 578 | # 使用Open3D的DBSCAN聚类 579 | with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Error): 580 | labels = np.array(region_pcd.cluster_dbscan(eps=tolerance, min_points=self.cluster_size_min, print_progress=False)) 581 | 582 | # 处理聚类结果 583 | max_label = labels.max() if len(labels) > 0 and labels.max() > -1 else -1 584 | for j in range(max_label + 1): 585 | cluster_indices = np.where(labels == j)[0] 586 | if self.cluster_size_min <= len(cluster_indices) <= self.cluster_size_max: 587 | # 获取该聚类的原始点云索引 588 | original_indices = [indices_array[i][idx] for idx in cluster_indices] 589 | 590 | # 创建该聚类的点云 591 | cluster_pcd = o3d.geometry.PointCloud() 592 | cluster_pcd.points = o3d.utility.Vector3dVector(points[original_indices]) 593 | 594 | # 计算边界框 595 | cluster_points = np.asarray(cluster_pcd.points) 596 | min_point = np.min(cluster_points, axis=0) 597 | max_point = np.max(cluster_points, axis=0) 598 | 599 | box = { 600 | 'min': min_point, 601 | 'max': max_point 602 | } 603 | 604 | all_clusters.append(cluster_pcd) 605 | boxes.append(box) 606 | 607 | return all_clusters, boxes 608 | 609 | def compute_centroids(self, clusters): 610 | """计算每个聚类的中心点""" 611 | centroids = [] 612 | for cluster in clusters: 613 | centroid = np.mean(np.asarray(cluster.points), axis=0) 614 | centroids.append(centroid) 615 | return centroids 616 | 617 | def process_point_cloud(self, pcd): 618 | """处理点云的主函数""" 619 | # 1. 降采样和高度过滤 620 | filtered_pcd, indices = self.filter_point_cloud(pcd) 621 | # 2. 点云分区 622 | indices_array = self.divide_into_regions(pcd, indices) 623 | # 3. 欧几里得聚类 624 | clusters, boxes = self.euclidean_clustering(pcd, indices_array) 625 | # 4. 计算聚类中心 626 | centroids = self.compute_centroids(clusters) 627 | return filtered_pcd, clusters, centroids, boxes 628 | 629 | def publish_cluster_markers(self, header, clusters, boxes, classified_clusters): 630 | """发布聚类标记 - 只发布已经分类的聚类""" 631 | # 首先清除所有现有的标记 632 | self.clear_markers(header) 633 | 634 | # 发布位姿数组 - 只发布已分类的聚类的位姿 635 | if self.pose_array_pub.get_num_connections() > 0 and classified_clusters: 636 | pose_array = PoseArray() 637 | pose_array.header = header 638 | 639 | for cls in classified_clusters: 640 | cluster_idx = cls['cluster_idx'] 641 | if cluster_idx < len(clusters): 642 | # 计算质心 643 | centroid = np.mean(np.asarray(clusters[cluster_idx].points), axis=0) 644 | 645 | pose = Pose() 646 | pose.position.x = centroid[0] 647 | pose.position.y = centroid[1] 648 | pose.position.z = centroid[2] 649 | pose.orientation.w = 1.0 650 | 651 | pose_array.poses.append(pose) 652 | 653 | self.pose_array_pub.publish(pose_array) 654 | 655 | # 发布边界框标记 - 只发布已分类的聚类 656 | if self.cluster_marker_pub.get_num_connections() > 0 and classified_clusters: 657 | marker_array = MarkerArray() 658 | 659 | # 为每个分类的聚类创建标记 660 | for i, cls in enumerate(classified_clusters): 661 | cluster_idx = cls['cluster_idx'] 662 | if cluster_idx >= len(boxes): 663 | continue 664 | 665 | box = boxes[cluster_idx] 666 | min_point = box['min'] 667 | max_point = box['max'] 668 | 669 | # 创建边界框标记 670 | marker = Marker() 671 | marker.header = header 672 | marker.ns = "adaptive_clustering" 673 | marker.id = i # 使用索引i作为ID，避免ID冲突 674 | marker.type = Marker.LINE_LIST 675 | marker.action = Marker.ADD 676 | marker.scale.x = 0.02 # 线宽 677 | 678 | # 获取类别和颜色信息 679 | class_name = cls['class_name'] 680 | 681 | # 为不同类别设置不同颜色 682 | if class_name in ['person', 'pedestrian']: 683 | marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # 红色 684 | elif class_name in ['car', 'vehicle', 'truck', 'bus']: 685 | marker.color = ColorRGBA(0.0, 0.0, 1.0, 1.0) # 蓝色 686 | elif class_name in ['bicycle', 'bike']: 687 | marker.color = ColorRGBA(1.0, 0.5, 0.0, 1.0) # 橙色 688 | else: 689 | marker.color = ColorRGBA(0.8, 0.2, 0.8, 1.0) # 紫色 690 | 691 | # 创建边界框角点 692 | p = [None] * 24 693 | p[0] = Point(max_point[0], max_point[1], max_point[2]) 694 | p[1] = Point(min_point[0], max_point[1], max_point[2]) 695 | p[2] = Point(max_point[0], max_point[1], max_point[2]) 696 | p[3] = Point(max_point[0], min_point[1], max_point[2]) 697 | p[4] = Point(max_point[0], max_point[1], max_point[2]) 698 | p[5] = Point(max_point[0], max_point[1], min_point[2]) 699 | p[6] = Point(min_point[0], min_point[1], min_point[2]) 700 | p[7] = Point(max_point[0], min_point[1], min_point[2]) 701 | p[8] = Point(min_point[0], min_point[1], min_point[2]) 702 | p[9] = Point(min_point[0], max_point[1], min_point[2]) 703 | p[10] = Point(min_point[0], min_point[1], min_point[2]) 704 | p[11] = Point(min_point[0], min_point[1], max_point[2]) 705 | p[12] = Point(min_point[0], max_point[1], max_point[2]) 706 | p[13] = Point(min_point[0], max_point[1], min_point[2]) 707 | p[14] = Point(min_point[0], max_point[1], max_point[2]) 708 | p[15] = Point(min_point[0], min_point[1], max_point[2]) 709 | p[16] = Point(max_point[0], min_point[1], max_point[2]) 710 | p[17] = Point(max_point[0], min_point[1], min_point[2]) 711 | p[18] = Point(max_point[0], min_point[1], max_point[2]) 712 | p[19] = Point(min_point[0], min_point[1], max_point[2]) 713 | p[20] = Point(max_point[0], max_point[1], min_point[2]) 714 | p[21] = Point(min_point[0], max_point[1], min_point[2]) 715 | p[22] = Point(max_point[0], max_point[1], min_point[2]) 716 | p[23] = Point(max_point[0], min_point[1], min_point[2]) 717 | 718 | # 添加所有点到标记 719 | for point in p: 720 | marker.points.append(point) 721 | 722 | # 设置生命周期为短暂的（0.1秒） 723 | marker.lifetime = rospy.Duration(0.1) 724 | marker_array.markers.append(marker) 725 | 726 | # 添加文本标记 727 | text_marker = Marker() 728 | text_marker.header = header 729 | text_marker.ns = "adaptive_clustering_labels" 730 | text_marker.id = i # 使用索引i作为ID，避免ID冲突 731 | text_marker.type = Marker.TEXT_VIEW_FACING 732 | text_marker.action = Marker.ADD 733 | 734 | # 文本位置（在边界框上方） 735 | text_marker.pose.position.x = (min_point[0] + max_point[0]) / 2 736 | text_marker.pose.position.y = (min_point[1] + max_point[1]) / 2 737 | text_marker.pose.position.z = max_point[2] + 0.5 738 | text_marker.pose.orientation.w = 1.0 739 | 740 | # 文本内容 741 | confidence = cls['confidence'] 742 | text = f"{class_name}: {confidence:.2f}" 743 | text_marker.text = text 744 | 745 | # 文本样式 746 | text_marker.scale.z = 0.4 # 文本大小 747 | text_marker.color = marker.color # 与边界框相同的颜色 748 | # 设置生命周期为短暂的（0.1秒） 749 | text_marker.lifetime = rospy.Duration(0.1) 750 | marker_array.markers.append(text_marker) 751 | 752 | if marker_array.markers: 753 | self.cluster_marker_pub.publish(marker_array) 754 | 755 | if __name__ == "__main__": 756 | try: 757 | fusion = MultiModalFusion() 758 | rospy.spin() 759 | except rospy.ROSInterruptException: 760 | pass -------------------------------------------------------------------------------- /image_pointcloud_fusion/scripts/pc2img_projection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | import numpy as np 6 | import cv2 7 | from cv_bridge import CvBridge, CvBridgeError 8 | from sensor_msgs.msg import Image, PointCloud2 9 | import sensor_msgs.point_cloud2 as pc2 10 | from std_msgs.msg import Header 11 | from message_filters import ApproximateTimeSynchronizer, Subscriber 12 | 13 | class LidarCameraProjection: 14 | def __init__(self): 15 | rospy.init_node('lidar_camera_projection', anonymous=True) 16 | 17 | # 获取参数 18 | self.camera_frame = rospy.get_param('~camera_frame', 'camera') 19 | self.lidar_frame = rospy.get_param('~lidar_frame', 'livox_frame') 20 | 21 | # 相机内参矩阵 22 | self.camera_matrix = np.array([ 23 | [909.783, 0, 650.365], 24 | [0, 909.004, 381.295], 25 | [0, 0, 1] 26 | ]) 27 | 28 | # 激光雷达到相机的外参矩阵 29 | self.extrinsic_matrix = np.array([ 30 | [0.0, -1.0, 0.0, 0.03], 31 | [0.0, 0.0, -1.0, -0.022], 32 | [1.0, 0.0, 0.0, -0.04], 33 | [0, 0, 0, 1] 34 | ]) 35 | 36 | rospy.loginfo("Camera matrix: \n{}".format(self.camera_matrix)) 37 | rospy.loginfo("Extrinsic matrix: \n{}".format(self.extrinsic_matrix)) 38 | 39 | # 初始化CV Bridge 40 | self.bridge = CvBridge() 41 | 42 | # 创建发布者 43 | self.projected_image_pub = rospy.Publisher("/camera/projected_points_image", Image, queue_size=1) 44 | self.depth_image_pub = rospy.Publisher("/camera/depth_image", Image, queue_size=1) 45 | 46 | # 创建订阅者并进行时间同步 47 | self.image_sub = Subscriber("/camera/color/image_raw", Image) 48 | self.lidar_sub = Subscriber("/livox/lidar", PointCloud2) 49 | self.ts = ApproximateTimeSynchronizer([self.image_sub, self.lidar_sub], queue_size=10, slop=0.1) 50 | self.ts.registerCallback(self.callback) 51 | 52 | rospy.loginfo("LidarCameraProjection node initialized") 53 | 54 | def callback(self, image_msg, pointcloud_msg): 55 | """处理同步到的图像和点云数据""" 56 | try: 57 | # 将ROS图像消息转换为OpenCV图像 58 | cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") 59 | 60 | # 从点云消息中提取点云数据 61 | points_list = [] 62 | for point in pc2.read_points(pointcloud_msg, field_names=("x", "y", "z"), skip_nans=True): 63 | points_list.append(point) 64 | 65 | # 将点云数据转换为NumPy数组 66 | points = np.array(points_list) 67 | 68 | # 投影点云到图像并创建深度图 69 | projected_image, depth_image = self.project_points_to_image(points, cv_image) 70 | 71 | # 发布投影后的图像和深度图 72 | self.publish_images(projected_image, depth_image, image_msg.header) 73 | 74 | except CvBridgeError as e: 75 | rospy.logerr("CvBridge Error: {0}".format(e)) 76 | except Exception as e: 77 | rospy.logerr("Error: {0}".format(e)) 78 | 79 | def project_points_to_image(self, points, image): 80 | """将点云投影到图像平面上，并创建深度图""" 81 | h, w, _ = image.shape 82 | 83 | # 创建深度图，初始化为无穷大 84 | depth_image = np.full((h, w), np.inf, dtype=np.float32) 85 | 86 | # 创建投影图像的副本 87 | projected_image = image.copy() 88 | 89 | # 为点云添加第四个坐标（齐次坐标） 90 | points_homogeneous = np.ones((points.shape[0], 4)) 91 | points_homogeneous[:, :3] = points 92 | 93 | # 使用外参矩阵将点从激光雷达坐标系变换到相机坐标系 94 | points_camera = np.dot(self.extrinsic_matrix, points_homogeneous.T).T 95 | 96 | # 过滤掉相机后方的点（Z < 0） 97 | valid_indices = points_camera[:, 2] > 0 98 | points_camera = points_camera[valid_indices] 99 | 100 | if len(points_camera) == 0: 101 | rospy.logwarn("No points in front of the camera") 102 | return projected_image, depth_image 103 | 104 | # 归一化坐标 105 | points_normalized = points_camera[:, :3] / points_camera[:, 2:3] 106 | 107 | # 使用相机内参矩阵将归一化坐标投影到像素坐标 108 | pixel_coords = np.dot(self.camera_matrix, points_normalized[:, :3].T).T 109 | 110 | # 取整得到像素坐标 111 | pixel_coords = np.round(pixel_coords[:, :2]).astype(np.int32) 112 | 113 | # 提取Z值（深度） 114 | depths = points_camera[:, 2] 115 | 116 | # 创建彩色映射，用于可视化 117 | min_depth = np.min(depths) 118 | max_depth = np.max(depths) 119 | depth_range = max_depth - min_depth 120 | 121 | # 在点云范围内标记点，同时更新深度图 122 | for i, (u, v) in enumerate(pixel_coords): 123 | # 检查像素坐标是否在图像范围内 124 | if 0 <= u < w and 0 <= v < h: 125 | # 如果该像素当前深度值更小，则更新 126 | current_depth = depths[i] 127 | if current_depth < depth_image[v, u]: 128 | depth_image[v, u] = current_depth 129 | 130 | # 根据深度值计算颜色（越近越红，越远越蓝） 131 | normalized_depth = (current_depth - min_depth) / depth_range if depth_range > 0 else 0 132 | color = self.depth_to_color(normalized_depth) 133 | 134 | # 在图像上绘制点 135 | cv2.circle(projected_image, (u, v), 2, color, -1) 136 | 137 | # 处理深度图，将无穷大值设为0（或者您想要的其他值） 138 | depth_image[np.isinf(depth_image)] = 0 139 | 140 | # 归一化深度图以便可视化（可选） 141 | depth_vis = (depth_image - min_depth) / depth_range if depth_range > 0 else depth_image 142 | depth_vis = (depth_vis * 255).astype(np.uint8) 143 | # 应用颜色映射 144 | depth_colormap = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET) 145 | 146 | return projected_image, depth_image 147 | 148 | def depth_to_color(self, normalized_depth): 149 | """将归一化深度值转换为BGR颜色""" 150 | # 使用jet颜色映射，近:红色，远:蓝色 151 | if normalized_depth < 0.25: 152 | # 红色到黄色 153 | r = 255 154 | g = int(normalized_depth * 4 * 255) 155 | b = 0 156 | elif normalized_depth < 0.5: 157 | # 黄色到绿色 158 | r = int(255 - (normalized_depth - 0.25) * 4 * 255) 159 | g = 255 160 | b = 0 161 | elif normalized_depth < 0.75: 162 | # 绿色到青色 163 | r = 0 164 | g = 255 165 | b = int((normalized_depth - 0.5) * 4 * 255) 166 | else: 167 | # 青色到蓝色 168 | r = 0 169 | g = int(255 - (normalized_depth - 0.75) * 4 * 255) 170 | b = 255 171 | 172 | return (b, g, r) # OpenCV使用BGR格式 173 | 174 | def publish_images(self, projected_image, depth_image, header): 175 | """发布投影图像和深度图""" 176 | try: 177 | # 发布投影图像 178 | projected_msg = self.bridge.cv2_to_imgmsg(projected_image, "bgr8") 179 | projected_msg.header = header 180 | self.projected_image_pub.publish(projected_msg) 181 | 182 | # 发布深度图 183 | depth_msg = self.bridge.cv2_to_imgmsg(depth_image, "32FC1") 184 | depth_msg.header = header 185 | self.depth_image_pub.publish(depth_msg) 186 | 187 | except CvBridgeError as e: 188 | rospy.logerr("CvBridge Error: {0}".format(e)) 189 | 190 | def main(): 191 | try: 192 | lidar_camera_projection = LidarCameraProjection() 193 | rospy.spin() 194 | except rospy.ROSInterruptException: 195 | pass 196 | 197 | if __name__ == '__main__': 198 | main() -------------------------------------------------------------------------------- /image_pointcloud_fusion/scripts/yolo11s.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jie0110/image-pointcloud-fusion/3b177f04f9f989179fda8223bb3dcf029bfa51f4/image_pointcloud_fusion/scripts/yolo11s.pt --------------------------------------------------------------------------------