16、3D感知与非平面表面感知技术解析

3D感知与非平面表面感知技术解析

1. 3D感知基础：使用卡尔曼滤波器重建3D线与平面

在3D感知领域，卡尔曼滤波器是一种强大的工具，可用于从3D点重建3D线和3D平面。

1.1 3D线的重建

对于3D线的重建，卡尔曼滤波方程的各组件表达式如下：
- (M_i) 是一个 (2×4) 矩阵，(y_i) 是二维向量，((x_i, y_i, z_i)) 是来自多个源的3D点坐标，作为测量向量。
- (M_i=\begin{bmatrix}z_i & 0 & 1 & 0\0 & z_i & 0 & 1\end{bmatrix})
- (y_i=\begin{bmatrix}x_i\y_i\end{bmatrix})
- 测量误差矩阵 (w_i(2×3)) 可通过特定表达式估计。
- 初始估计向量 (a_0 = [a_0, b_0, p_0, q_0]^T) 可假设为任意值，如 ([0 0 0 0])，也可由前两个点计算得出：
- (a_0=\frac{z_2x_1 - z_1x_2}{z_2 - z_1})
- (b_0=\frac{z_2y_1 - z_1y_2}{z_2 - z_1})
- (p_0=\frac{z_2x_1 - z_1x_2}{z_2 - z_1})
- (q_0=\frac{z_2y_1 - z_1y_2}{z_2 - z_1})

下面是3D线估计过程的前两次迭代示例：
- 第一次迭代 ：
- 初始状态向量 (a_0 = [0 0 0 0]^T)