human_tracking_depth

ROS2 人体追踪功能包：将 LiDAR 点云投影到相机图像，在 YOLO11 检测框内采样深度，估算目标 3D 位置与速度。

LiDAR ─────────────────────────────────────────────────────┐
   PointCloud2                                              │
                                                            ▼
RGB Camera ──► YOLO11 tracker ──► 2D bboxes ──► PositionEstimator ──► 3D pos / vel
   Image        (track IDs)                    (depth sampling)
                                                            │
                                                 DepthProjector
                                              (lidar → image plane)

---

目录

• 算法说明
• 依赖
• 编译
• 配置参数
• 启动
• 发布话题
• RViz2 可视化
• 调试方法
• 常见问题

---

算法说明

每帧处理流程（YOLO 与点云投影并行执行）：

1. 点云投影（DepthProjector）
   将 LiDAR 坐标系点云经外参矩阵 lidar_to_camera 变换到相机坐标系，再通过内参 K + 畸变系数投影到像素平面，得到稀疏深度图 Nx6 [u, v, depth_cam, xl, yl, zl]。

2. YOLO 追踪（YoloTracker）
   YOLO11 仅检测 class=0（person），persist=True 保持跨帧稳定 track ID。

3. 3D 位置估算（PositionEstimator）
   对每个 YOLO 框：

   • 筛选投影坐标落在框内的点；
   • 前景截取：depth <= d_min + depth_window（默认 0.8 m），排除背景；
   • 对前景点在 LiDAR 坐标系下取均值，得到 3D 质心。

4. 速度估算（TrackManager）
   维护每个 track ID 的位置历史（默认 5 帧），用首尾差分 Δpos / Δt 估算速度向量（m/s）。超过 1 秒未更新的 ID 自动清除。

---

依赖

ROS2 包（Foxy）

rclpy  sensor_msgs  geometry_msgs  visualization_msgs
std_msgs  cv_bridge  message_filters

Python 库

库	用途
ultralytics	YOLO11 推理
opencv-python	图像处理、投影
numpy	向量化点云解析

注意：本包不依赖 sensor_msgs_py，点云解析通过 np.frombuffer 向量化实现。

YOLO 模型文件

默认路径为 /home/jiewang/ros2_ws/src/human_tracking_3d/models/yolo11s.pt，可在 params.yaml 中修改 yolo_model 字段。首次运行如无本地模型，ultralytics 会自动下载。

---

编译

cd /home/jiewang/ros2_ws
colcon build --packages-select human_tracking_depth
source install/setup.bash

---

配置参数

配置文件：config/params.yaml

human_tracking_depth_node:
  ros__parameters:

    # ── 话题 ──────────────────────────────────────────────────────────────────
    image_topic:      "/camera/camera/color/image_raw"
    pointcloud_topic: "/livox/lidar"

    # ── 相机内参（RealSense D435 @ 1280×720） ─────────────────────────────────
    camera_fx: 909.783
    camera_fy: 909.004
    camera_cx: 650.365
    camera_cy: 381.295
    image_width:  1280
    image_height:  720

    # 畸变系数 [k1, k2, p1, p2, k3]（无畸变填零）
    dist_coeffs: [0.0, 0.0, 0.0, 0.0, 0.0]

    # ── LiDAR 到相机外参（行优先 4×4 齐次矩阵） ───────────────────────────────
    lidar_to_camera: [
      0.0, -1.0, 0.0,  0.0,
      0.0,  0.0,-1.0,  0.0,
      1.0,  0.0, 0.0, -0.05,
      0.0,  0.0, 0.0,  1.0
    ]

    # ── YOLO ──────────────────────────────────────────────────────────────────
    yolo_model: "/path/to/yolo11s.pt"
    yolo_conf:  0.5
    # yolo_device:
    #   ""     → ultralytics 自动选择（有 GPU 时优先使用）
    #   "cpu"  → 强制 CPU 推理（避免与其他节点抢 VRAM）
    #   "0"    → 强制 GPU 0
    yolo_device: ""

    # ── 点云空间滤波 ────────────────────────────────────────────────────────────
    pc_z_min:    -1.0   # LiDAR 坐标系 Z 轴下界（m）
    pc_z_max:     2.0   # LiDAR 坐标系 Z 轴上界（m）
    pc_range_max: 15.0  # XY 平面最大水平距离（m）

    # ── 3D 位置估算 ─────────────────────────────────────────────────────────────
    min_lidar_points: 3   # bbox 内至少需要的投影点数
    depth_window:     0.8  # 前景深度带宽（m，从最近点开始）

    # ── 速度估算 ────────────────────────────────────────────────────────────────
    velocity_history: 5   # 每个 track 保留的位置历史帧数

关键标定参数说明

相机内参：可从 camera_info 话题读取，或使用 camera_calibration 工具标定。

ros2 topic echo /camera/camera/color/camera_info --once
# 从 K 矩阵提取：[fx,0,cx,0,fy,cy,0,0,1]

LiDAR-Camera 外参：lidar_to_camera 是将 LiDAR 坐标系点变换到相机坐标系的 4×4 矩阵 T_cam_lidar，满足：

p_camera = T_cam_lidar · p_lidar

可通过手动量取安装偏移、或使用标定板联合标定工具（如 lidar_camera_calibration）获得。

---

启动

直接启动（使用默认 params.yaml）

ros2 launch human_tracking_depth human_tracking_depth.launch.py

覆盖话题（命令行参数）

ros2 launch human_tracking_depth human_tracking_depth.launch.py \
    image_topic:=/camera/color/image_raw \
    pointcloud_topic:=/velodyne_points

指定自定义配置文件

ros2 launch human_tracking_depth human_tracking_depth.launch.py \
    params_file:=/path/to/my_params.yaml

仅运行节点（不用 launch）

ros2 run human_tracking_depth human_tracking_depth_node \
    --ros-args --params-file /home/jiewang/ros2_ws/src/human_tracking_depth/config/params.yaml

---

发布话题

话题	类型	说明
human_tracking_depth/depth_image	sensor_msgs/Image	相机图像叠加 Jet 着色深度点（近→远：蓝→红）
human_tracking_depth/detection_image	sensor_msgs/Image	YOLO 检测框 + 3D 位置/速度文字标注
human_tracking_depth/projected_cloud	sensor_msgs/PointCloud2	能投影到图像内的 LiDAR 点，按深度着色（XYZRGB）
human_tracking_depth/markers	visualization_msgs/MarkerArray	球体、文字标签、速度箭头（LiDAR 坐标系）

MarkerArray 组成

每个追踪目标最多 3 个 Marker（namespace / ID 唯一）：

namespace	类型	内容
human_pos	SPHERE (0.3 m)	3D 质心球
human_label	TEXT_VIEW_FACING	ID:N\n(x, y, z) m\nV m/s
human_vel	ARROW	速度方向箭头（速度 > 0.05 m/s 时显示）

---

RViz2 可视化

启动 RViz2：

rviz2

添加以下显示项（Add → By topic）：

话题	Plugin	备注
/human_tracking_depth/depth_image	Image	深度叠加图
/human_tracking_depth/detection_image	Image	YOLO 检测框 + 3D 标注
/human_tracking_depth/projected_cloud	PointCloud2	设置 Color Transformer = RGB8
/human_tracking_depth/markers	MarkerArray	3D 球/文字/箭头

Fixed Frame：设置为 LiDAR 的 frame_id（通常为 livox_frame 或 lidar_link），与 pc_msg.header.frame_id 一致。

---

调试方法

1. 验证话题是否同步

# 查看话题频率
ros2 topic hz /camera/camera/color/image_raw
ros2 topic hz /livox/lidar

# 若频率差异较大（>5倍），考虑增大 slop 参数（代码中 ApproximateTimeSynchronizer slop=0.2）

2. 检查点云投影是否正确

订阅 depth_image 话题，观察相机图中的投影点：

• 点应落在物体表面，不应出现系统性偏移
• 若所有点偏移方向一致 → lidar_to_camera 平移部分有误
• 若点呈旋转错位 → lidar_to_camera 旋转部分有误

ros2 run rqt_image_view rqt_image_view
# 选择 /human_tracking_depth/depth_image

3. 验证 3D 位置合理性

ros2 topic echo /human_tracking_depth/markers --field markers[0].text
# 应输出类似：ID:1\n(2.30, 0.10, 0.05) m\n0.00 m/s

若 3D 位置异常：

• pos_3d 坐标在 LiDAR 坐标系下，Z 轴朝上（通常）
• 检查 min_lidar_points：若目标遮挡严重，可降低至 1~2
• 检查 depth_window：若目标前后有障碍物，可缩小至 0.3~0.5 m

4. 打印每帧检测数量

ros2 topic echo /human_tracking_depth/detection_image --no-arr | grep -c header
# 每帧打印一次，确认节点在运行

查看节点日志（含 YOLO 加载信息）：

ros2 run human_tracking_depth human_tracking_depth_node \
    --ros-args --log-level DEBUG

5. 检查时间戳同步

若 markers 更新缓慢或消失，通常是时间戳不匹配导致 ApproximateTimeSynchronizer 无法配对：

ros2 topic echo /camera/camera/color/image_raw --field header.stamp --no-arr
ros2 topic echo /livox/lidar --field header.stamp --no-arr

两者时间戳差值应 < 0.2 s（slop 值）。

---

常见问题

Q: 节点启动后无输出，没有话题数据

检查订阅话题是否匹配：

ros2 topic list | grep -E "image_raw|lidar"
# 确认与 params.yaml 中的 image_topic / pointcloud_topic 一致

Q: YOLO 加载报 GPU 内存不足（bad_alloc / exit code -9）

将 yolo_device 改为 "cpu"：

yolo_device: "cpu"

或同时运行其他 GPU 节点时，设置 yolo_device: "1" 指定另一块 GPU。

Q: 深度图中几乎没有投影点

1. 确认 pc_range_max 覆盖目标距离（默认 15 m）
2. 确认 pc_z_min / pc_z_max 包含 LiDAR 安装高度范围
3. 检查 lidar_to_camera 旋转矩阵是否正确（行列顺序、坐标轴方向）

快速验证旋转：以 LiDAR 前方 2 m、正中央一点 [2, 0, 0] 为例，手算变换后相机 Z > 0 且 u/v 应在图像中央附近。

Q: 速度估算抖动严重

增大 velocity_history（默认 5，可调到 8~10），或在 TrackManager 中对速度向量做指数滑动平均（EMA）。

Q: RViz2 中 MarkerArray 坐标系不对

Marker 使用 pc_msg.header.frame_id（LiDAR 原始坐标系）。确保 RViz2 的 Fixed Frame 设置为相同 frame id，并且 TF 树中有对应变换。

Q: Python 3.8 类型注解报错（'type' is not subscriptable）

所有模块文件顶部需要：

from __future__ import annotations

编译后重新 source install/setup.bash。

---

包结构

human_tracking_depth/
├── config/
│   └── params.yaml                  # 所有可调参数
├── human_tracking_depth/
│   ├── __init__.py
│   ├── human_tracking_depth_node.py # 主节点（订阅/发布/协调）
│   ├── depth_projector.py           # LiDAR → 图像平面投影
│   ├── position_estimator.py        # bbox 内深度采样 → 3D 质心
│   ├── yolo_tracker.py              # YOLO11 追踪封装
│   └── track_manager.py             # 速度估算 + ID 生命周期管理
├── launch/
│   └── human_tracking_depth.launch.py
├── package.xml
└── setup.py