# Camera Tracking Implementation - 完整实现总结

## ✅ 实现完成

实现了完整的camera attach到机器人功能，支持6种跟踪模式和完全自定义配置。

## 核心功能

### 1. Track Mode预设（6种模式）

| 模式 | 描述 | 位置（robot local frame） | 用途 |
|------|------|------------------------|------|
| `chase` | 追逐镜头 | 后方3.5m，上方2.5m | 展示视频、电影感 |
| `follow` | 跟随镜头 | 后方2.0m，侧面0.5m，高1.8m | 第三人称游戏风格 |
| `side` | 侧面视角 | 侧面3.0m，高1.5m | 步态分析 |
| `top` | 俯视视角 | 正上方5.0m | 路径规划 |
| `first_person` | 第一人称 | pelvis link，略微上方 | 导航、感知任务 |
| `static` | 静态镜头 | 世界坐标系固定 | 全局观察 |

### 2. Entity Attachment机制

- **Entity索引查找**：自动查找entity并获取索引
- **Link索引查找**：支持attach到特定link
- **Local Frame**：Camera的pos/lookat在entity的local frame中
- **自动跟随**：每个step自动跟随entity运动

### 3. Backend支持

正确处理三种backend的entity attachment：
- **Rasterizer**：静态用`add_camera()`，attached用`add_sensor(RasterizerCameraOptions)`
- **Raytracer**：统一用`add_sensor(RaytracerCameraOptions)`
- **BatchRenderer**：统一用`add_sensor(BatchRendererCameraOptions)`

## 修改的文件

### 1. [camera_cfg.py](source/genesislab/genesislab/engine/scene/camera_cfg.py)

**新增**：
- `track_mode` 字段：支持6种预设模式
- `_apply_track_mode()` 方法：应用预设配置
- 改进了 `entity_name` 和 `link_name` 的文档
- 详细的使用示例

**Track Mode预设配置**：
```python
track_presets = {
    "chase": {
        "pos": (-3.5, 0.0, 2.5),
        "lookat": (1.0, 0.0, 0.5),
        "fov": 50.0,
    },
    "follow": { ... },
    "side": { ... },
    "top": { ... },
    "first_person": {
        "pos": (0.0, 0.0, 0.2),
        "lookat": (1.0, 0.0, 0.0),
        "link_name": "pelvis",
        "fov": 75.0,
    },
    "static": { ... },
}
```

### 2. [lab_scene.py](source/genesislab/genesislab/engine/scene/lab_scene.py)

**修复**：`_setup_camera_and_recording()` 方法

关键改进：
```python
# 修复前：rasterizer始终用add_camera()，不支持entity attachment
camera = scene.add_camera(...)  # ❌ 不支持entity_idx

# 修复后：根据是否attach到entity选择API
if entity_idx == -1:
    camera = scene.add_camera(...)  # 静态camera
else:
    camera = scene.add_sensor(
        gs.sensors.RasterizerCameraOptions(
            entity_idx=entity_idx,
            link_idx_local=link_idx_local,
            ...
        )
    )  # ✅ 支持entity attachment
```

**新增**：
- Entity/link查找和验证
- 调试信息打印（显示attachment状态和track_mode）

### 3. [play_amo_mesh_terrain.py](third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py)

**新增命令行参数**：
- `--camera-track`: 选择tracking模式（默认chase）
- `--camera-entity`: 指定要attach的entity（默认robot）
- 修改 `--camera-pos` 和 `--camera-lookat` 为可选（custom模式使用）

**配置逻辑**：
```python
if args.camera_track == "custom":
    # 自定义模式：手动pos/lookat
    camera_kwargs["pos"] = tuple(args.camera_pos)
    camera_kwargs["lookat"] = tuple(args.camera_lookat)
    camera_kwargs["entity_name"] = None
elif args.camera_track == "static":
    # 静态模式：不attach
    camera_kwargs["track_mode"] = "static"
else:
    # 跟踪模式：attach到robot
    camera_kwargs["track_mode"] = args.camera_track
    camera_kwargs["entity_name"] = args.camera_entity
```

## 使用示例

### 命令行

```bash
# Chase模式（默认，推荐）
python third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py \
    --headless \
    --record-video \
    --camera-track chase \
    --max-steps 500

# First person视角
python third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py \
    --headless \
    --record-video \
    --camera-track first_person \
    --video-path output/first_person.mp4 \
    --max-steps 500

# 侧面跟踪
python third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py \
    --headless \
    --record-video \
    --camera-track side \
    --max-steps 500

# 静态镜头（不跟随）
python third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py \
    --headless \
    --record-video \
    --camera-track static \
    --max-steps 500
```

### 编程方式

```python
from genesislab.engine.scene import CameraCfg, RecordingCfg

# 方式1：使用预设track_mode
cfg.scene.camera = CameraCfg(
    track_mode="chase",      # 追逐镜头
    entity_name="robot",     # Attach到robot
    res=(1920, 1080),
)

# 方式2：完全自定义
cfg.scene.camera = CameraCfg(
    entity_name="robot",     # Attach到robot
    link_name="pelvis",      # Attach到pelvis link
    pos=(-3.0, 0.0, 2.5),   # 在robot local frame的偏移
    lookat=(1.0, 0.0, 0.5), # 在robot local frame的目标
    fov=50.0,
)

# 方式3：静态camera（不跟随）
cfg.scene.camera = CameraCfg(
    track_mode="static",     # 或者 entity_name=None
    pos=(5.0, 0.0, 3.0),    # 世界坐标系位置
    lookat=(0.0, 0.0, 0.5),
)
```

## 测试验证

### 配置测试
```bash
python test_camera_tracking.py
```

结果：
```
✅ All track modes OK
✅ Chase mode configuration correct
✅ First person mode configuration correct
✅ Preset override works correctly
✅ Custom attachment configuration correct
✅ Static mode configuration correct
```

### 实际运行测试

```bash
# 测试chase模式
python third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py \
    --headless \
    --record-video \
    --camera-track chase \
    --video-path output/test_chase.mp4 \
    --camera-res 1280 720 \
    --max-steps 300 \
    --vx 0.5
```

预期输出：
```
🎥 Video recording enabled:
   Output: output/test_chase.mp4
   Resolution: 1280x720
   Track mode: chase
   FPS: 60

[LabScene] Camera attached to entity 'robot', link index 0
           Track mode: chase
[LabScene] Started video recording: output/test_chase.mp4
           Camera: 1280x720, 60 FPS
```

## 技术细节

### Local Frame坐标系

对于人形机器人（如G1），entity的local frame：
- **+X**: 向前（前进方向）
- **+Y**: 向左
- **+Z**: 向上

所以track_mode的预设位置：
- `chase`: `pos=(-3.5, 0.0, 2.5)` = 后方3.5m，上方2.5m
- `follow`: `pos=(-2.0, 0.5, 1.8)` = 后方2m，左侧0.5m，上方1.8m
- `side`: `pos=(0.0, -3.0, 1.5)` = 右侧3m，上方1.5m
- `top`: `pos=(0.0, 0.0, 5.0)` = 正上方5m

### Entity Attachment流程

1. **查找Entity**：
   ```python
   entity = self._entities[cam_cfg.entity_name]
   entity_idx = entity.gs_entity.idx
   ```

2. **查找Link**：
   ```python
   link_names = entity.gs_entity.links_map
   link_idx_local = link_names[cam_cfg.link_name]
   ```

3. **创建Camera**（区分静态和attached）：
   ```python
   if entity_idx == -1:
       camera = scene.add_camera(...)  # 静态
   else:
       camera = scene.add_sensor(
           gs.sensors.RasterizerCameraOptions(
               entity_idx=entity_idx,
               link_idx_local=link_idx_local,
               ...
           )
       )  # Attached
   ```

### 坐标变换

每个step，Genesis自动计算：
```
camera_world_transform = link_world_transform * camera_local_transform
```

Camera会：
- 跟随link的平移
- 跟随link的旋转
- 保持相对link的固定offset

## 文档

1. **[CAMERA_TRACKING_GUIDE.md](CAMERA_TRACKING_GUIDE.md)**
   - 完整的使用指南
   - 所有track_mode的详细说明
   - 命令行和编程示例
   - 常见问题解答

2. **[CAMERA_RECORDING_EXAMPLE.md](CAMERA_RECORDING_EXAMPLE.md)**
   - Camera和recording基础用法
   - 配置参数参考
   - Backend对比

3. **[test_camera_tracking.py](test_camera_tracking.py)**
   - 所有track_mode的单元测试
   - 配置验证

## 关键改进点

1. **修复Entity Attachment Bug**
   - 之前rasterizer backend不支持entity attachment
   - 现在正确区分静态和attached camera，使用不同的API

2. **Track Mode预设**
   - 提供6种开箱即用的跟踪模式
   - 无需手动计算camera位置

3. **灵活配置**
   - 可以使用预设
   - 可以在预设基础上微调
   - 可以完全自定义

4. **清晰文档**
   - 每个track_mode有详细说明
   - 坐标系解释清楚
   - 丰富的示例

## 下一步

用户可以：

1. **测试所有track_mode**：
   ```bash
   for mode in chase follow side top first_person static; do
       python third_party/genPiHub/scripts/amo/genesislab/play_amo_mesh_terrain.py \
           --headless --record-video \
           --camera-track $mode \
           --video-path output/${mode}.mp4 \
           --max-steps 300
   done
   ```

2. **选择最佳视角**：根据任务需求选择合适的track_mode

3. **微调参数**：在预设基础上调整pos/lookat/fov

4. **集成到训练**：在RL训练中使用camera跟踪记录训练过程

## 已知限制

1. **单camera**：当前每个scene只支持一个camera配置
2. **Lookat也在local frame**：如果想要camera在世界坐标系某位置但"看向"机器人，需要额外实现
3. **预设针对人形机器人**：其他类型机器人（四足、轮式）可能需要调整预设

## 总结

✅ 完整实现了camera attach到机器人的功能
✅ 提供6种开箱即用的跟踪模式
✅ 支持完全自定义配置
✅ 修复了entity attachment的bug
✅ 提供详细文档和测试
✅ 完全支持headless模式

用户现在可以轻松地让camera跟随机器人，录制高质量的跟踪视频！