diff --git a/src/tracking_car/config.yaml b/src/tracking_car/config.yaml index c3172b2713..209856d86a 100644 --- a/src/tracking_car/config.yaml +++ b/src/tracking_car/config.yaml @@ -55,8 +55,7 @@ det_label_font_scale: 0.5 det_label_padding: 5 view: -default_mode: "satellite" # 默认视角模式: satellite, behind, first_person -satellite_height: 50.0 # 卫星视角高度(米) -behind_distance: 10.0 # 后方视角距离(米) -first_person_height: 1.6 # 第一人称视角高度 -update_frequency: 1.0 # 视角更新频率(Hz),1.0=每秒更新一次 \ No newline at end of file +default_mode: "satellite" # 默认视角模式: satellite, behind, first_person +satellite_height: 50.0 # 卫星视角高度(米) +behind_distance: 10.0 # 后方视角距离(米) +first_person_height: 1.6 # 第一人称视角高度(米) \ No newline at end of file diff --git a/src/tracking_car/main.py b/src/tracking_car/main.py index bc5a996aa5..46de81d9b4 100644 --- a/src/tracking_car/main.py +++ b/src/tracking_car/main.py @@ -260,7 +260,118 @@ def __init__(self, config): self.cpu_usage_history = [] self.memory_usage_history = [] - logger.info("[OK] Visualizer initialized (Color ID encoding + Independent statistics window)") + # 3D点云可视化相关属性(新增) + self.show_pointcloud = False # 是否显示点云窗口 + self.pcd_window_name = "LiDAR Point Cloud" + self.pcd_vis = None # Open3D可视化器对象 + self.pcd_geometry_added = False + self.pcd_update_counter = 0 # 点云更新计数器 + + logger.info("[OK] Visualizer initialized (Color ID encoding + Independent statistics window + PointCloud)") + + def init_pointcloud_visualizer(self): + """初始化点云可视化器""" + if not self.config.get('use_lidar', True): + return False + + try: + import open3d as o3d + self.pcd_vis = o3d.visualization.Visualizer() + self.pcd_vis.create_window( + window_name=self.pcd_window_name, + width=800, + height=600, + left=100, + top=100 + ) + + # 设置背景颜色 + opt = self.pcd_vis.get_render_option() + opt.background_color = np.array([0.1, 0.1, 0.1]) # 深灰色背景 + opt.point_size = 1.5 + + self.pcd_geometry_added = False + logger.info("[POINTCLOUD] 点云可视化器初始化完成") + return True + + except Exception as e: + logger.warning(f"点云可视化器初始化失败: {e}") + return False + + def update_pointcloud(self, pointcloud_data): + """ + 更新点云显示 + + Args: + pointcloud_data: 点云数据 (numpy array) + """ + if not self.show_pointcloud or not self.pcd_vis or pointcloud_data is None: + return + + try: + # 每2帧更新一次,避免性能问题 + self.pcd_update_counter += 1 + if self.pcd_update_counter % 2 != 0: + return + + import open3d as o3d + + # 创建点云对象 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(pointcloud_data) + + # 根据高度着色 + if len(pointcloud_data) > 0: + z_min = pointcloud_data[:, 2].min() + z_max = pointcloud_data[:, 2].max() + z_range = max(z_max - z_min, 1e-6) + + colors = np.zeros((len(pointcloud_data), 3)) + normalized_z = (pointcloud_data[:, 2] - z_min) / z_range + colors[:, 0] = normalized_z # 红色通道(高处) + colors[:, 2] = 1 - normalized_z # 蓝色通道(低处) + pcd.colors = o3d.utility.Vector3dVector(colors) + + # 更新或添加几何体 + if not self.pcd_geometry_added: + self.pcd_vis.add_geometry(pcd) + self.pcd_geometry_added = True + else: + self.pcd_vis.clear_geometries() + self.pcd_vis.add_geometry(pcd) + + # 更新可视化 + self.pcd_vis.poll_events() + self.pcd_vis.update_renderer() + + except Exception as e: + logger.warning(f"更新点云失败: {e}") + + def toggle_pointcloud_display(self): + """切换点云显示""" + if not self.config.get('use_lidar', True): + logger.warning("LiDAR功能已禁用,无法显示点云") + return + + self.show_pointcloud = not self.show_pointcloud + + if self.show_pointcloud: + # 初始化点云可视化器 + if not self.pcd_vis: + if not self.init_pointcloud_visualizer(): + self.show_pointcloud = False + return + logger.info("[POINTCLOUD] 点云显示已开启") + else: + # 关闭点云窗口 + if self.pcd_vis: + try: + self.pcd_vis.destroy_window() + except: + pass + self.pcd_vis = None + self.pcd_geometry_added = False + logger.info("[POINTCLOUD] 点云显示已关闭") def _get_behavior_color(self, track_info): """ @@ -852,7 +963,7 @@ def _draw_info_panel(self, image, track_count): h, w = image.shape[:2] # Information panel background (semi-transparent black) - panel_height = 80 + panel_height = 100 overlay = image.copy() cv2.rectangle(overlay, (0, 0), (w, panel_height), (0, 0, 0), -1) image = cv2.addWeighted(overlay, 0.7, image, 0.3, 0) @@ -866,7 +977,8 @@ def _draw_info_panel(self, image, track_count): status_lines = [ f"Tracked Objects: {track_count}", f"ESC: Exit | W: Weather | S: Screenshot", - f"P: Pause | T: Stats Window | M: Color Legend" + f"P: Pause | T: Stats Window | M: Color Legend", + f"L: PointCloud | V: View Mode" # 添加点云提示 ] # Draw status information @@ -994,6 +1106,14 @@ def show(self, image, stats_data=None): def destroy(self): """Destroy all windows""" + # 销毁点云可视化器 + if self.pcd_vis: + try: + self.pcd_vis.destroy_window() + except: + pass + + # 销毁其他窗口 cv2.destroyAllWindows() logger.info("[OK] All visualization windows closed") @@ -1186,6 +1306,11 @@ def _collect_statistics_data(self, fps, detection_time, tracking_time, tracks_in # Detection thread status detection_thread_status = 'Running' if self.detection_thread and self.detection_thread.is_alive() else 'Not running' + # 点云状态(新增) + pointcloud_status = 'Enabled' if self.config.get('use_lidar', True) else 'Disabled' + if self.visualizer: + pointcloud_status += ' | Showing' if self.visualizer.show_pointcloud else ' | Hidden' + return { # System status 'fps': fps, @@ -1206,6 +1331,11 @@ def _collect_statistics_data(self, fps, detection_time, tracking_time, tracks_in 'avg_tracking_time': tracking_time * 1000, # Convert to milliseconds 'avg_frame_time': perf_stats.get('avg_frame_time', 0), + # 点云状态(新增) + 'pointcloud_status': pointcloud_status, + 'pointcloud_enabled': self.config.get('use_lidar', True), + 'pointcloud_showing': self.visualizer.show_pointcloud if self.visualizer else False, + # Raw data (for charts) 'detection_time': detection_time, 'tracking_time': tracking_time, @@ -1299,6 +1429,10 @@ def run(self): tracks_info=tracks_info ) + # 添加点云显示(新增) + if sensor_data.get('pointcloud') is not None: + self.visualizer.update_pointcloud(sensor_data['pointcloud']) + # Add color legend (if enabled) if self.show_legend: result_image = self.visualizer.draw_color_legend(result_image) @@ -1380,6 +1514,11 @@ def _handle_keyboard_input(self, key): status = "Show" if self.show_legend else "Hide" logger.info(f"[LEGEND] Color legend: {status}") + # L键切换点云显示(新增) + elif key == ord('l') or key == ord('L'): + if self.visualizer: + self.visualizer.toggle_pointcloud_display() + # 新增:V键切换视角模式 elif key == ord('v') or key == ord('V'): if self.sensor_manager: diff --git a/src/tracking_car/sensors.py b/src/tracking_car/sensors.py index 99e863441d..7270c7135e 100644 --- a/src/tracking_car/sensors.py +++ b/src/tracking_car/sensors.py @@ -386,9 +386,11 @@ def __init__(self, world, ego_vehicle, config): self.ego_vehicle = ego_vehicle self.config = config self.spectator = None - self.view_mode = 'satellite' # 视角模式: 'satellite', 'behind', 'first_person' - self.view_height = 50.0 # 卫星视角高度 - self.follow_distance = 10.0 # 后方视角距离 + view_config = config.get('view', {}) + self.view_mode = view_config.get('default_mode', 'satellite') + self.view_height = view_config.get('satellite_height', 50.0) + self.follow_distance = view_config.get('behind_distance', 10.0) + self.first_person_height = view_config.get('first_person_height', 1.6) def setup(self): """设置视角""" @@ -502,7 +504,7 @@ def _set_first_person_view(self): location = carla.Location( x=vehicle_transform.location.x, y=vehicle_transform.location.y, - z=vehicle_transform.location.z + 1.6 # 驾驶员眼睛高度 + z=vehicle_transform.location.z + self.first_person_height # 改为使用配置 ) # 使用车辆的方向