# Orbbec 数据采集 & D2C 对齐工具

基于 PyQt5 的奥比中光相机数据采集与深度图软件对齐一体化工具。支持实时预览、一键拍照、批量 D2C 转换，并提供两种 D2C 算法（正向投影 / 逆向投影）供选择。

---

## 功能特点

| 功能 | 说明 |
|------|------|
| **数据采集** | 实时预览 RGB + 深度伪彩图，支持 D2C 预览切换；一键拍照保存多模态数据 |
| **批量 D2C** | 对 Orbbec Viewer 导出的深度图批量软件对齐，无需连接相机 |
| **双算法支持** | 正向投影 + Splat 填充 / 逆向投影（SDK 同等），可实时切换预览 |
| **自动参数获取** | 从相机实时提取内外参，自动保存为 JSON |
| **热插拔监听** | 设备拔出即时 Toast 提示；设备插入后自动加载设备列表 |
| **分辨率自适应** | 支持字体缩放（0.8× ~ 2.0×），适配不同 DPI 屏幕 |

---

## 环境依赖

| 包 | 版本 |
|----|------|
| Python | 3.8 ~ 3.13 |
| pyorbbecsdk | 2.x |
| numpy | — |
| opencv-python | — |
| PyQt5 | — |

```bash
pip install numpy opencv-python PyQt5
```

### 安装 pyorbbecsdk

> 官方文档：https://orbbec.github.io/pyorbbecsdk/source/2_installation/install_the_package.html

```bash
pip install pyorbbecsdk2
```

### 设备驱动配置

**Windows**（管理员 PowerShell）：
```powershell
Set-ExecutionPolicy -ExecutionPolicy RemoteSigned -Scope CurrentUser
.\obsensor_metadata_win10.ps1 -op install_all
```

**Linux**：
```bash
cd pyorbbecsdk/scripts/env_setup
sudo chmod +x ./install_udev_rules.sh
sudo ./install_udev_rules.sh
sudo udevadm control --reload && sudo udevadm trigger
```

---

## 文件说明

| 文件 | 说明 |
|------|------|
| `main.py` | GUI 主程序（数据采集 + D2C 批量转换） |
| `d2c_batch.py` | 命令行 D2C 批量转换工具（可独立使用） |
| `utils.py` | SDK 辅助函数（帧格式转换等） |
| `camera_params.json` | 相机参数缓存（自动生成，可复用） |

---

## GUI 使用方法

```bash
python main.py
```

### 标签页一：数据采集

1. **加载设备**：点击获取相机支持的 RGB / Depth 分辨率列表
2. **流配置**：选择 RGB 分辨率、Depth 分辨率、Preset（可选）
3. **保存目录 & 采集内容**：
   - 设置根目录和子目录名称，点击「创建 / 载入」
   - 勾选需要采集的数据类型（RGB、Depth、Depth D2C、IR、点云等）
   - 选择 **D2C 方式**：
     - **逆向投影（SDK 同等）**：使用 SDK `AlignFilter`，速度快，无空洞
     - **正向投影 + Splat**：纯软件实现，支持 Splat 邻域扩散（关闭 / 3×3 / 5×5 / 7×7）
4. **预览**：勾选「D2C 预览」可实时查看对齐效果，切换 D2C 方式预览即时更新
5. **拍照**：点击「拍照」或按快捷键 **S**，数据自动保存到指定目录

### 标签页二：Orbbec Viewer 深度图 D2C

1. **加载设备**并选择 RGB / Depth 分辨率，点击「从设备获取参数」
   - 或手动选择已有的 `camera_params.json`
2. **选择深度图目录**：包含 PNG 深度图的文件夹
3. **选择转换方式**：
   - 正向投影 + Splat（默认）
   - 逆向投影（SDK 同等）
4. 设置 **Splat 大小**（正向投影时可用）
5. 点击「开始批量 D2C 转换」

---

## 命令行使用方法（d2c_batch.py）

```bash
python d2c_batch.py            # 完整流程：选配置 → 获取参数 → 批量转换
python d2c_batch.py --params   # 仅获取相机参数
python d2c_batch.py --convert  # 仅批量转换（使用已有 camera_params.json）
```

---

## D2C 原理详解

本工具提供两种 D2C（Depth-to-Color）对齐算法，核心差异在于**投影方向**。

### 1. 正向投影（Forward Projection）+ Splat

**思路**：从深度图每个像素出发，反投影到 3D 空间，经外参变换后投影到彩色图平面。

```
深度像素 (u_d, v_d, Z)
    ↓  Brown-Conrady 畸变校正 + 反投影
3D 点（深度相机坐标系）[X, Y, Z]
    ↓  外参旋转 R + 平移 t
3D 点（彩色相机坐标系）[X_c, Y_c, Z_c]
    ↓  彩色内参投影
彩色像素 (u_c, v_c)  → 写入对齐深度图
```

**间隙问题**：每个深度像素只写入彩色平面的**单个像素**。由于视角差异，相邻深度像素投影后会出现跳跃，留下空洞（离散小方块）。

**Splat 修复**：对每个投影后的像素，同时写入以其为中心的 N×N 邻域，远→近顺序遍历保证遮挡正确。

```python
def software_d2c(depth_img, params, splat_radius=1):
    depth_intr = params["depth"]["intrinsic"]
    depth_dist = params["depth"]["distortion"]
    color_intr = params["color"]["intrinsic"]
    ext = params["extrinsic"]

    color_w, color_h = color_intr["width"], color_intr["height"]
    fx_c, fy_c = color_intr["fx"], color_intr["fy"]
    cx_c, cy_c = color_intr["cx"], color_intr["cy"]
    dh, dw = depth_img.shape
    depth_scale = params["depth"]["scale"]

    # 1. 像素网格
    u_d, v_d = np.meshgrid(np.arange(dw, dtype=np.float32),
                           np.arange(dh, dtype=np.float32))

    # 2. 深度值（mm）
    Z = depth_img.astype(np.float32) * depth_scale
    valid = Z > 0

    # 3. 去畸变 + 反投影到深度相机 3D 空间
    x_u, y_u = _undistort_points(u_d, v_d, depth_intr, depth_dist)
    X = x_u * Z
    Y = y_u * Z

    # 4. 外参变换：深度相机 → 彩色相机
    R = np.asarray(ext["rot"], dtype=np.float64).reshape(3, 3)
    t = np.asarray(ext["transform"], dtype=np.float64)
    pts = np.stack([X.ravel(), Y.ravel(), Z.ravel()], axis=0)
    pts_c = R @ pts + t[:, np.newaxis]

    Xc = pts_c[0].reshape(dh, dw).astype(np.float32)
    Yc = pts_c[1].reshape(dh, dw).astype(np.float32)
    Zc = pts_c[2].reshape(dh, dw).astype(np.float32)

    # 5. 投影到彩色图平面
    valid_c = valid & (Zc > 0)
    u_c = np.where(valid_c, fx_c * Xc / Zc + cx_c, -1.0)
    v_c = np.where(valid_c, fy_c * Yc / Zc + cy_c, -1.0)
    u_ci = np.round(u_c).astype(np.int32)
    v_ci = np.round(v_c).astype(np.int32)

    in_bounds = (u_ci >= 0) & (u_ci < color_w) & (v_ci >= 0) & (v_ci < color_h)
    mask = valid_c & in_bounds

    u_vals = u_ci[mask]
    v_vals = v_ci[mask]
    z_vals = depth_img[mask]

    # 6. 远→近排序，近处覆盖远处
    order = np.argsort(z_vals)[::-1]
    u_vals = u_vals[order]
    v_vals = v_vals[order]
    z_vals = z_vals[order]

    aligned = np.zeros((color_h, color_w), dtype=np.uint16)

    # 7. Splat 填充（N×N 邻域扩散）
    if splat_radius > 0:
        for dv in range(-splat_radius, splat_radius + 1):
            for du in range(-splat_radius, splat_radius + 1):
                us = np.clip(u_vals + du, 0, color_w - 1)
                vs = np.clip(v_vals + dv, 0, color_h - 1)
                aligned[vs, us] = z_vals
    else:
        aligned[v_vals, u_vals] = z_vals

    return aligned
```

| 特点 | 说明 |
|------|------|
| 优点 | 直观、可控制 Splat 大小、适合理解投影原理 |
| 缺点 | 正向映射天然有间隙，必须配合 Splat；计算量较大 |
| 适用 | 对空洞容忍度有要求的场景、教学/算法验证 |

---

### 2. 逆向投影（Inverse Projection）

**思路**：遍历彩色图每个像素，反投影为射线，变换到深度相机空间后采样深度图。输出天生 dense，无正向映射空洞。

```
彩色像素 (u_c, v_c)
    ↓  彩色内参反投影
射线方向（彩色相机坐标系）[xn, yn, 1]
    ↓  外参逆变换 R^T
射线方向（深度相机坐标系）[ax, ay, az]
    ↓  结合深度图估计 Z_c
深度相机像素 (u_d, v_d)  → 采样深度值
```

采用**双 Pass 策略**：
- **Pass 0**：仅旋转（忽略平移），粗略估计深度坐标
- **Pass 1**：用 Pass 0 的估计值进行完整的 R/t 变换，精确采样

```python
def software_d2c_inv(depth_img, params):
    depth_intr = params["depth"]["intrinsic"]
    color_intr = params["color"]["intrinsic"]
    ext = params["extrinsic"]
    depth_scale = params["depth"]["scale"]

    color_w, color_h = color_intr["width"], color_intr["height"]
    fx_d, fy_d = depth_intr["fx"], depth_intr["fy"]
    cx_d, cy_d = depth_intr["cx"], depth_intr["cy"]
    fx_c, fy_c = color_intr["fx"], color_intr["fy"]
    cx_c, cy_c = color_intr["cx"], color_intr["cy"]

    R = np.array(ext["rot"], dtype=np.float64).reshape(3, 3)
    t = np.array(ext["transform"], dtype=np.float64)
    R_inv = R.T
    b = R_inv @ t

    # 1. 彩色像素网格
    uc, vc = np.meshgrid(np.arange(color_w, dtype=np.float32),
                         np.arange(color_h, dtype=np.float32))
    xn_c = (uc - cx_c) / fx_c
    yn_c = (vc - cy_c) / fy_c

    # 2. 射线方向变换到深度相机空间
    N = color_w * color_h
    dirs = np.stack([xn_c.ravel(), yn_c.ravel(), np.ones(N, np.float32)], axis=0)
    dirs_d = (R_inv @ dirs.astype(np.float64)).astype(np.float32)
    ax = dirs_d[0].reshape(color_h, color_w)
    ay = dirs_d[1].reshape(color_h, color_w)
    az = dirs_d[2].reshape(color_h, color_w)

    depth_f32 = depth_img.astype(np.float32)
    az_safe = np.where(az > 1e-6, az, 1.0)

    # Pass 0：旋转-only 粗略估计
    m0_u = (fx_d * ax / az_safe + cx_d).astype(np.float32)
    m0_v = (fy_d * ay / az_safe + cy_d).astype(np.float32)
    Z_d0 = cv2.remap(depth_f32, m0_u, m0_v,
                     cv2.INTER_NEAREST,
                     borderMode=cv2.BORDER_CONSTANT, borderValue=0)

    # 估计 Z_c（mm）
    bx, by, bz = float(b[0]), float(b[1]), float(b[2])
    Z_c_mm = np.where(az_safe > 1e-6,
                      (Z_d0 * depth_scale + bz) / az_safe,
                      0.0).astype(np.float32)

    # Pass 1：完整变换精确采样
    X_d = Z_c_mm * ax - bx
    Y_d = Z_c_mm * ay - by
    Z_d = Z_c_mm * az - bz

    Z_d_safe = np.where(Z_d > 1e-6, Z_d, 1.0)
    m1_u = (fx_d * X_d / Z_d_safe + cx_d).astype(np.float32)
    m1_v = (fy_d * Y_d / Z_d_safe + cy_d).astype(np.float32)

    valid = (Z_d0 > 0) & (az > 1e-6) & (Z_d > 1e-6)
    m1_u = np.where(valid, m1_u, -1.0)
    m1_v = np.where(valid, m1_v, -1.0)

    aligned = cv2.remap(depth_f32, m1_u, m1_v,
                        cv2.INTER_NEAREST,
                        borderMode=cv2.BORDER_CONSTANT, borderValue=0)
    return aligned.astype(np.uint16)
```

| 特点 | 说明 |
|------|------|
| 优点 | 输出天生 dense 无空洞；与 SDK `AlignFilter` 算法等价；速度快 |
| 缺点 | 原理较抽象；需要深度图配合估计 Z_c |
| 适用 | 生产环境、实时预览、对空洞零容忍的场景 |

---

### 两种方法对比

| 维度 | 正向投影 + Splat | 逆向投影 |
|------|------------------|----------|
| 投影方向 | 深度 → 彩色 | 彩色 → 深度 |
| 空洞 | 有（需 Splat 修复） | 无 |
| 速度 | 较慢（numpy 密集计算） | 较快（cv2.remap 向量化） |
| 与 SDK 一致性 | 独立实现 | 与 `AlignFilter` 等价 |
| Splat 可调 | ✓ 支持（关闭/3×3/5×5/7×7） | ✗ 不需要 |

---

## camera_params.json 格式

```json
{
  "color": {
    "width": 1920, "height": 1080, "fps": 30, "format": "MJPG",
    "intrinsic": { "fx": 1382.5, "fy": 1382.5, "cx": 959.8, "cy": 539.4, "width": 1920, "height": 1080 },
    "distortion": { "k1": -0.055, "k2": 0.071, "k3": 0.0, "k4": 0.0, "k5": 0.0, "k6": 0.0, "p1": 0.0, "p2": 0.0 }
  },
  "depth": {
    "width": 640, "height": 400, "fps": 30, "format": "Y16",
    "scale": 0.001,
    "intrinsic": { "fx": 424.0, "fy": 424.0, "cx": 319.5, "cy": 199.5, "width": 640, "height": 400 },
    "distortion": { "k1": 0.0, ... }
  },
  "extrinsic": {
    "rot": [1,0,0, 0,1,0, 0,0,1],
    "transform": [-14.82, 0.12, 0.03]
  }
}
```

`extrinsic.rot` 为行优先展平的 3×3 旋转矩阵，`extrinsic.transform` 为平移向量（单位 mm），方向为深度相机坐标系 → 彩色相机坐标系。

---

## 输入 / 输出格式

| 项目 | 说明 |
|------|------|
| 输入深度图 | PNG，uint16，单位为传感器原始单位（乘以 `depth_scale` 得 mm） |
| 输出对齐深度图 | PNG，uint16，单位与输入相同，分辨率与所选 RGB 配置一致 |
| 输出伪彩图 | PNG，uint8 BGR，JET colormap，无效像素为纯黑 |

---

## 注意事项

- 获取相机参数时需要相机**实际连接**；批量转换时不需要。
- 更换相机或修改分辨率后需重新获取参数。
- 输入深度图分辨率须与获取参数时选择的 Depth 分辨率一致。
- 对齐深度图的有效区域受深度传感器视场角限制，彩色图边缘区域可能无深度值（值为 0）。
- **正向投影**实时预览帧率较低（纯 numpy 计算），建议确认效果后切回**逆向投影**保持预览流畅。