# human_tracking

**Repository Path**: xapples/human_tracking

## Basic Information

- **Project Name**: human_tracking
- **Description**: hhhhhhhhhhhhhhhhhhhh
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-06-06
- **Last Updated**: 2026-06-16

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 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 可视化](#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 会自动下载。

---

## 编译

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

---

## 配置参数

配置文件：`config/params.yaml`

```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` 工具标定。

```bash
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）

```bash
ros2 launch human_tracking_depth human_tracking_depth.launch.py
```

### 覆盖话题（命令行参数）

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

### 指定自定义配置文件

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

### 仅运行节点（不用 launch）

```bash
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：

```bash
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. 验证话题是否同步

```bash
# 查看话题频率
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` 旋转部分有误

```bash
ros2 run rqt_image_view rqt_image_view
# 选择 /human_tracking_depth/depth_image
```

### 3. 验证 3D 位置合理性

```bash
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. 打印每帧检测数量

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

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

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

### 5. 检查时间戳同步

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

```bash
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: 节点启动后无输出，没有话题数据

检查订阅话题是否匹配：

```bash
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"`：

```yaml
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`）

所有模块文件顶部需要：

```python
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
```