阶段1：初始位姿估计

EPnP计算位姿：

• 核心思想是将任意个3D点表示为4个虚拟的控制点的加权组合，然后在控制点坐标系下表达投影关系，最后求解相机的初始位姿。

流程：用少数控制点（4个）表示所有3D点；通过求解控制点的相机坐标，间接得到位姿

展开代码
输入：
  - N个3D点（世界坐标系）
  - N个2D点（图像坐标）
  - 相机内参（f_x, f_y, c_x, c_y）

步骤1：构建4个控制点（世界坐标系）
  ↓
步骤2：将3D点表示为控制点的线性组合
  ↓
步骤3：建立投影方程（关于控制点相机坐标的线性方程）
  ↓
步骤4：构建线性方程组 A·β = 0
  ↓
步骤5：SVD求解，得到控制点的相机坐标
  ↓
步骤6：从控制点恢复位姿 R, t
  ↓
输出：相机位姿（旋转矩阵R和平移向量t）

计算步骤

1：构建控制点（世界坐标系）——方法：基于特征分解的均匀分布选择方法

输入：N个3D点（世界坐标系，已知）

1. 计算质心
2. 中心化处理
3. 计算协方差矩阵并特征分解
4. 定义4个控制点$C_1, C_2, C_3, C_4$

输出：4个控制点在世界坐标系的位置

2：将3D点表示为控制点的线性组合

对每个3D点 $P_i$：

$P_i=\sum_{j=1}^4 \alpha_{i j} C_j$

计算权重 $\alpha_{i j}$（这些权重是固定的，只依赖于3D点的几何关系）

3：建立投影方程

$C_j^c=R C_j+t \Rightarrow P_i^c=\sum_{j=1}^4 \alpha_{i j} C_j^c$

• 控制点在世界坐标系$C_j$经过R、t变换到相机坐标系$C_j^c$，3D点在相机坐标系的位置是： $P_i^c = α_{i1}×C_1^c + α_{i2}×C_2^c + α_{i3}×C_3^c + α_{i4}×C_4^c$

将$P_i^c$ 投影到图像：

代入 $P_i^c$ 的表达式，得到关于 $C_j^c$ 的线性方程：

$X_i^c=\sum_j^4 \alpha_{i j} X_j^c, \quad Y_i^c=\sum_j^4 \alpha_{i j} Y_j^c, \quad Z_i^c=\sum_j^4 \alpha_{i j} Z_j^c$

4：构建线性方程组

最终的投影关系是控制点在相机坐标系下的线性组合。联合上述公式，对每个点建立方程组:

$\begin{aligned} & \sum_{j=1}^4\left(f_x \alpha_{i j} X_j^c-\left(u_i-c_x\right) \alpha_{i j} Z_j^c\right)=0 \\ & \sum_{j=1}^4\left(f_y \alpha_{i j} Y_j^c-\left(v_i-c_y\right) \alpha_{i j} Z_j^c\right)=0\end{aligned}$

这些方程最终构成一个的线性方程组： $A \cdot \beta=0$

其中： A: 系数矩阵（2N × 12） β: 未知向量（12 × 1），包含4个控制点的相机坐标

5：SVD求解

使用SVD分解：$A = U × Σ × V^T$

β 的解 = V 的最后一列（对应最小奇异值）

得到：4个控制点在相机坐标系的位置 $C_1^c, C_2^c, C_3^c, C_4^c$

6：从控制点恢复位姿

已知：

• 控制点在世界坐标系：$C_1, C_2, C_3, C_4$
• 控制点在相机坐标系：$C_1^c, C_2^c, C_3^c, C_4^c$

求解：

• 旋转矩阵 R 和平移向量 t
• 使得：$C_j^c = R × C_j + t$

通过最小二乘或直接计算得到 R 和 t，即求解得到相机的初始外参矩阵。

获取2D-3D对应关系

• 问题：图像中检测到LED标记的2D坐标，但不知道每个2D点对应哪个3D标记点。 例：

展开代码
目标上有5个LED标记（编号：LED1, LED2, LED3, LED4, LED5）
图像中检测到5个光点（检测点：D1, D2, D3, D4, D5）

问题：图像中的D1对应哪个LED？D2对应哪个LED？...

而位姿估计需要建立2D-3D对应关系：

展开代码
已知：
  - 3D点：LED_A在(0.0, 0.0, 0.0)
  - 2D点：图像中某点在(100, 150)
  
求解：
  - 相机的位姿（旋转R和平移t）
  
条件：
  - 必须知道(100, 150)这个2D点就是LED_A的投影

• 解决：需要匹配所有检测点与标记点

步骤1：枚举所有可能匹配

$N = C(n_d, 5) × P(n_l, 5)$

• 从检测点中选择5个
• 记点中选择5个进行排列
• 所有可能的匹配组合

步骤2：用EPnP验证每个匹配

对每个匹配组合：

1、PnP计算位姿

展开代码
匹配组合1：
  检测点1(100, 150) → LED_A(0.0, 0.0, 0.0)
  检测点2(200, 150) → LED_B(0.1, 0.0, 0.0)
  检测点3(150, 250) → LED_C(0.05, 0.1, 0.0)
  检测点4(180, 200) → LED_D(0.08, 0.05, 0.0)
  检测点5(120, 180) → LED_E(0.02, 0.08, 0.0)

1.1、选择4个点用于EPnP计算

从5个匹配中随机选择4个

展开代码
用于EPnP的4个点：
  点1: (100, 150) → (0.0, 0.0, 0.0)
  点2: (200, 150) → (0.1, 0.0, 0.0)
  点3: (150, 250) → (0.05, 0.1, 0.0)
  点4: (180, 200) → (0.08, 0.05, 0.0)

1.2 构建控制点

• 计算质心
• 中心化处理
• 计算协方差矩阵并特征分解
• 定义4个控制点
• 将3D点表示为控制点的线性组合

1.3 建立投影方程

对每个2D-3D对应点，建立两个方程： 对点1: (100, 150) → (0.0, 0.0, 0.0)

$Σ_{j=1}^4 (f_x × α_{1j} × X_j^c - (100 - c_x) × α_{1j} × Z_j^c) = 0$

$Σ_{j=1}^4 (f_y × α_{1j} × Y_j^c - (150 - c_y) × α_{1j} × Z_j^c) = 0$

对点2, 3, 4 类似...

1.4 构建线性方程组

将所有方程组合：A · β = 0

其中：

• A: 8×12 矩阵（4个点 × 2个方程/点 = 8个方程）
• β: 12×1 向量（4个控制点 × 3个坐标 = 12个未知数） = $[X_1^c, Y_1^c, Z_1^c, X_2^c, Y_2^c, Z_2^c, X_3^c, Y_3^c, Z_3^c, X_4^c, Y_4^c, Z_4^c]^T$

1.5 SVD求解

1.6 从控制点恢复位姿

已知：

• 控制点在世界坐标系：$C_1, C_2, C_3, C_4$
• 控制点在相机坐标系：$C_1^c, C_2^c, C_3^c, C_4^c$（从β得到）

求解：

• 旋转矩阵 R 和平移向量 t
• 使得：$C_j^c = R × C_j + t$

通过最小二乘或直接计算得到 R 和 t，即结果：得到位姿 (R, t)

2、位姿将剩余3D点投影到图像

假设剩余的点是第5个点：

展开代码
剩余点：LED_E(0.02, 0.08, 0.0) 在世界坐标系

2.1：转换到相机坐标系

$P_E^c = R × P_E + t= R × (0.02, 0.08, 0.0) + t= (X_E^c, Y_E^c, Z_E^c)$

2.2：投影到图像平面

$u_E' = f_x × (X_E^c / Z_E^c) + c_x$

$v_E' = f_y × (Y_E^c / Z_E^c) + c_y$

结果：投影点 $(u_E', v_E')$ = (125, 185)假设

3、投影误差

• 实际检测点：检测点5 = (120, 180)
• 投影点：$(u_E', v_E')$= (125, 185)

投影误差 = √[(120-125)² + (180-185)²] = √[25 + 25] = √50 ≈ 7.07 像素

4、判断是否可能是正确匹配

如果误差小 → 可能是正确匹配

• 如果投影误差 < 阈值（例如5像素）：
  • 该匹配可能是正确的，记录该匹配关系
• 如果投影误差 ≥ 阈值：
  • 该匹配可能是错误的，丢弃

步骤3：频率与概率直方图筛选

问题：

• 即使重投影误差小，仍可能因噪声导致错误匹配
• 需要更可靠的筛选机制

思路：

• 统计每个匹配关系出现的频率

• 频率高的匹配更可能是正确的

计算匹配频率

假设有600个候选匹配组合，对每个组合：

• 用EPnP计算位姿
• 计算剩余点的重投影误差
• 如果误差 < 5像素 → 记录该匹配关系
• 记录统计次数

展开代码
匹配关系统计：
  LED_A ↔ 检测点1: 出现了 50 次
  LED_A ↔ 检测点2: 出现了 10 次
  LED_A ↔ 检测点3: 出现了 5 次
  LED_A ↔ 检测点4: 出现了 2 次
  
  LED_B ↔ 检测点1: 出现了 8 次
  LED_B ↔ 检测点2: 出现了 45 次  ← 这个匹配出现最频繁！
  LED_B ↔ 检测点3: 出现了 12 次
  
  LED_C ↔ 检测点3: 出现了 48 次
  LED_C ↔ 检测点4: 出现了 15 次
  
  ...（其他匹配关系）

构建频率直方图 H(L_i, D_j)

计算匹配概率

$H_p\left(L_i, D_j\right)=\frac{H\left(L_i, D_j\right)}{\sum_{i=1}^{n_l} H\left(L_i\right)} \cdot \frac{H\left(L_i, D_j\right)}{\sum_{j=1}^{n_d} H\left(D_j\right)}$

上述公式实际上是计算两个条件概率的乘积：

$H_p(L_i, D_j) = P(D_j | L_i) × P(L_i | D_j)$

其中： $P(D_j | L_i) = H(L_i, D_j) / Σ_{k=1}^{n_l} H(L_k)$ = 给定$LED_i$，观察到检测点_j的概率

$P(L_i | D_j) = H(L_i, D_j) / Σ_{m=1}^{n_d} H(D_m)$ = 给定检测点_j，它来自$LED_i$的概率

• 双向验证：同时考虑“LED→检测点”和“检测点→LED”的概率
• 归一化：消除某些LED或检测点频繁匹配的影响
• 置信度：概率越高，匹配越可靠

选择最优匹配

1：按概率排序

展开代码
将所有匹配按概率从高到低排序：

1. LED_A ↔ 检测点1: 概率 = 0.0230  ← 最高！
2. LED_C ↔ 检测点3: 概率 = 0.0212
3. LED_B ↔ 检测点2: 概率 = 0.0186
4. ...

2：构建最优匹配组合

展开代码
步骤1：选择概率最高的：LED_A ↔ 检测点1
步骤2：在剩余匹配中，选择概率最高的：LED_C ↔ 检测点3
步骤3：继续选择，直到所有LED都有匹配

3：计算匹配组合的总置信度

展开代码
对每个完整的匹配组合：
  总置信度 = 所有匹配概率的乘积（或加权和）
  
选择总置信度最高的组合作为最优匹配

最终实现的目标

1. 相机拍摄图像 → 检测到5个LED光点
2. 不知道每个光点对应哪个LED标记
3. 运行本流程：
   • 枚举600个可能匹配
   • EPnP验证每个匹配
   • 频率概率筛选
   • 得到最优匹配 + 初始位姿
4. 输出：目标在相机前方1.2米，偏右15度，得到目标在3D空间中的准确位置和姿态

解决了2D-3D匹配问题：自动确定图像中的每个检测点对应目标上的哪个LED标记

计算了初始位姿：得到相机（或目标）在3D空间中的位置和姿态

展开代码
旋转矩阵 R = [r11, r12, r13;
              r21, r22, r23;
              r31, r32, r33]

平移向量 t = [tx, ty, tz]^T

R 和 t 描述了相机相对于目标（或目标相对于相机）的位置和朝向，例如：目标在相机前方1米，向右偏30度

提供了准确的初始位姿也为后续的粒子滤波跟踪提供准确的起点

展开代码
重投影误差 = 实际检测点与投影点的距离
步骤1：用位姿将3D点投影到图像
  LED_A (0.0, 0.0, 0.0) → 用位姿投影 → (105, 155)

步骤2：与实际检测点比较
  实际检测点：(100, 150)
  投影点：(105, 155)
  
步骤3：计算距离
  误差 = √[(100-105)² + (150-155)²] = 7.07像素

---------
最优位姿（最小重投影误差）：
  R_optimal, t_optimal  ← 作为基准位姿

其他可信位姿（误差在容忍范围内，范围假设为5像素）：
  R_1, t_1  ← 粒子1
  R_2, t_2  ← 粒子2
  R_3, t_3  ← 粒子3
  ...
  
 例子：
 最优位姿（基准）：
  R_optimal, t_optimal
  重投影误差 = 2.1像素
  匹配：LED_A↔D1, LED_B↔D2, LED_C↔D3, LED_D↔D4, LED_E↔D5
  
其他可信位姿1：
  R_1, t_1
  重投影误差 = 3.5像素
  匹配：LED_A↔D1, LED_B↔D2, LED_C↔D3, LED_D↔D5, LED_E↔D4

其他可信位姿2：
  R_2, t_2
  重投影误差 = 4.2像素
  匹配：LED_A↔D2, LED_B↔D1, LED_C↔D3, LED_D↔D4, LED_E↔D5

其他可信位姿3：
  R_3, t_3
  重投影误差 = 4.8像素
  匹配：LED_A↔D1, LED_B↔D3, LED_C↔D2, LED_D↔D4, LED_E↔D5

• 基准位姿：最可能是正确的位姿
  • 作为粒子滤波的"中心粒子"
  • 作为其他粒子的参考点
  • 如果其他粒子都失效，至少有一个可靠的基准
• 其他可信位姿
  • 作为粒子滤波的初始粒子
  • 覆盖不确定性范围
  • 提供多个假设，提高鲁棒性

阶段2：粒子滤波逐帧跟踪

粒子滤波初始化， 使用阶段1的输出：

• 最优位姿 → 基准粒子
• 其他可信位姿 → 初始粒子
• 在基准周围生成更多粒子

展开代码
粒子1：R_optimal, t_optimal（基准，误差2.1）← 最重要
粒子2：R_1, t_1（误差3.5）
粒子3：R_2, t_2（误差4.2）
粒子4：R_3, t_3（误差4.8）
粒子5：R_optimal + 小噪声（在基准周围）
粒子6：R_optimal + 小噪声（在基准周围）
...
粒子100：R_optimal + 小噪声（在基准周围）

初始化完成：100个粒子就绪

预测的信息：目标的位姿（位置+姿态）

位置（Position）：

• X坐标：目标在3D空间中的X位置（米）
• Y坐标：目标在3D空间中的Y位置（米）
• Z坐标：目标在3D空间中的Z位置（米）

姿态（Orientation）：

• 旋转角度：目标相对于某个坐标系的旋转
• 通常用旋转矩阵R或欧拉角表示

具体预测过程

基于常速度模型预测：

$\begin{gathered}\hat{\xi}_{k+1}=\xi_k+\Delta t\left(\xi_k-\xi_{k-1}\right) \\ \Delta t=\left\{\begin{array}{c}0, n_p=1 \\ \left(t_{k+1}-t_k\right) /\left(t_k-t_{k-1}\right), n_p \geq 2\end{array}\right.\end{gathered}$

• 第k-1帧：位姿 $ξ_{k-1} = (位置, 姿态)$
• 第k帧：位姿 $ξ_k = (位置, 姿态)$

计算速度： 速度 = $(ξ_k - ξ_{k-1})$ / 时间差

预测第k+1帧： $ξ̂_{k+1} = ξ_k + Δt × (ξ_k - ξ_{k-1})$

展开代码
第1帧（t=0秒）：
  目标位置：(1.0, 2.0, 3.0) 米
  目标姿态：旋转10度

第2帧（t=0.1秒）：
  目标位置：(1.1, 2.1, 3.1) 米
  目标姿态：旋转12度

计算速度：
  位置速度 = (0.1, 0.1, 0.1) 米/0.1秒 = (1.0, 1.0, 1.0) 米/秒
  角度速度 = 2度/0.1秒 = 20度/秒

预测第3帧（t=0.2秒）：
  预测位置 = (1.1, 2.1, 3.1) + 0.1秒 × (1.0, 1.0, 1.0)
           = (1.2, 2.2, 3.2) 米
  预测姿态 = 12度 + 0.1秒 × 20度/秒 = 14度

对每个粒子独立预测

$P^u=T_k \cdot P_k \cdot W_k, W_k \in\left[\begin{array}{cc}R & t \\ 0 & 1\end{array}\right]$

• 对粒子1：

  • 当前位姿：P_1
  • 预测位姿：$P_1' = T_k × P_1 × W_k$ （T_k是预测变换，W_k是随机噪声）

• 对粒子2：

  • 当前位姿：P_2
  • 预测位姿：$P_2' = T_k × P_2 × W_k$ （独立预测，可能得到不同结果）

  ...

每个粒子代表一个可能的位姿，独立预测保持多样性，即使部分粒子预测错误，其他粒子可能正确

投影点与检测点的比较

位姿 = 旋转 + 平移

在齐次坐标下，位姿用4×4矩阵表示：

展开代码
P'' = [R  t]  = [r11  r12  r13  tx]
      [0  1]    [r21  r22  r23  ty]     R：3×3旋转矩阵（描述姿态）
                [r31  r32  r33  tz]
                [0    0    0    1 ]     t：3×1平移向量（描述位置

对每个投影点：

1. 计算投影点到所有检测点的距离

• 检测点（实际观测）
  • 来源：从实际拍摄的图像中检测得到，直接在图片上进行计算得到LED像素点
  • 方法：图像处理（阈值分割、轮廓检测等）
  • 结果：LED标记在图像中的实际像素位置
• 投影点（理论预测）
  • 来源：粒子估计的位姿，将真实世界坐标的LED标记投影得到
  • 方法：位姿变换（世界系到相机系） + 相机投影（像素平面）
  • 结果：如果粒子位姿正确，投影点应该接近检测点

展开代码
粒子的独立估计结果
已知：LED_A在世界坐标系 (0.0, 0.0, 0.0) 米

粒子1假设位姿：P_1''
  → 用P_1''投影LED_A → (400, 400)  ← 这是理论预测

粒子2假设位姿：P_2''
  → 用P_2''投影LED_A → (421, 392)  ← 不同的假设，不同的预测
---------  
  独立匹配结果
  粒子1（位姿较准确）：
  投影点：(400, 400)
  检测点：(402, 398)
  距离：d_min = 2.83 像素  ← 很小！
  → 匹配很好 → 权重高 ✓

 粒子2（位姿有误差）：
  投影点：(421, 392)
  检测点：(402, 398)
  距离：d_min = 19.9 像素  ← 很大！
  → 匹配很差 → 权重低 ✗

2. 选择最小距离 d_min

展开代码
图像中检测到的LED点：
  检测点1: (100, 150) 像素
  检测点2: (200, 150) 像素
  检测点3: (150, 250) 像素
  检测点4: (180, 200) 像素
  检测点5: (120, 180) 像素

投影点LED_A: (105, 155)
  到检测点1的距离 = √[(105-100)² + (155-150)²] = √50 ≈ 7.07 像素
  到检测点2的距离 = √[(105-200)² + (155-150)²] = √9026 ≈ 95.0 像素
  到检测点3的距离 = √[(105-150)² + (155-250)²] = √10906 ≈ 104.4 像素
  ...
  
  最小距离 d_min = 7.07 像素（对应检测点1）

3. 判断 d_min ≤ λ_rPF（匹配阈值）

• d_min ≤ λ_rPF： → 该投影点匹配成功，计入 n_p

• 否则： → 该投影点匹配失败，不计入 n_p

展开代码
假设 λ_rPF = 5 像素（匹配半径阈值）

投影点LED_A: d_min = 7.07 像素
  7.07 > 5 → 匹配失败 

投影点LED_B: d_min = 3.2 像素
  3.2 ≤ 5 → 匹配成功 

投影点LED_C: d_min = 4.8 像素
  4.8 ≤ 5 → 匹配成功 

过滤明显错误的匹配，只考虑合理的投影点

4、统计有效投影数量 n_p

n_p = 所有满足 d_min ≤ λ_rPF 的投影点数量

反映该粒子位姿能匹配到多少个LED，n_p 越大，位姿假设越可能正确

5、计算粒子权重 w

$w = n_p · n_l + Σ_{i=1}^{n_p} ((λ_rPF - d_min,i) / λ_rPF)² - 3 · n_so$

6、检查权重是否大于阈值 λ_w

• 如果至少有一个粒子权重 > λ_w： → 进行重采样
• 否则： → 重复更新粒子权重

7、权重归一化

$w̃_t^(i) = w_t^(i) / Σ_{j=1}^N w_t^(j)$

将权重转换为概率，便于重采样时按概率选择

原始权重：
展开代码
  粒子1: w_1 = 20.48
  粒子2: w_2 = 7.01
  粒子3: w_3 = 12.5
  粒子4: w_4 = 18.2
  总和 = 58.19

归一化后：
  粒子1: w̃_1 = 20.48 / 58.19 = 0.352
  粒子2: w̃_2 = 7.01 / 58.19 = 0.120
  粒子3: w̃_3 = 12.5 / 58.19 = 0.215
  粒子4: w̃_4 = 18.2 / 58.19 = 0.313
  总和 = 1.0

8、重采样

高权重粒子 → 被复制多份，低权重粒子 → 被淘汰

展开代码
假设有100个粒子需要重采样：

粒子1：权重0.352 → 被复制35份
粒子2：权重0.120 → 被复制12份
粒子3：权重0.215 → 被复制22份
粒子4：权重0.313 → 被复制31份
（总共100份）

结果：下一帧的100个粒子中，大部分来自高权重粒子

意义：

• 去除不可能的粒子（低权重）
• 聚焦到更可能的状态（高权重）
• 防止粒子退化（保持多样性）

实现了什么，估计出位姿可以用来干什么