点云配准算法之- GICP算法点云配准概率模型推导和最大似然求解（MLE）

一、GICP算法拓展

下面我们对文章：点云配准算法之-Generalized-ICP（ GICP）中点云配准模型进行拓展，推导如公式内容，

d i ( T ∗ ) ∼ N ( b ^ i − T ∗ a ^ i , C i B + T ∗ C i A ( T ∗ ) T ) d_i(T^\ast) \sim \mathcal{N}\big( \hat{b}_i - T^\ast \hat{a}_i, C_i^B + T^\ast C_i^A (T^\ast)^T \big) di(T∗)∼N(b^i−T∗a^i,CiB+T∗CiA(T∗)T)

并解释其来源和意义。

1 前提

设有真实（潜在）点集：

A ^ = a ^ i , B ^ = b ^ i \hat{A} = {\hat{a}_i}, \quad \hat{B} = {\hat{b}_i} A^=a^i,B^=b^i

观测点集：

A = a i , B = b i A = {a_i}, \quad B = {b_i} A=ai,B=bi

点的观测模型为：

a i = a ^ i + ϵ i A , b i = b ^ i + ϵ i B a_i = \hat{a}_i + \epsilon_i^A, \quad b_i = \hat{b}_i + \epsilon_i^B ai=a^i+ϵiA,bi=b^i+ϵiB

其中

ϵ i A ∼ N ( 0 , C i A ) \epsilon_i^A \sim \mathcal{N}(0, C_i^A) ϵiA∼N(0,CiA)
ϵ i B ∼ N ( 0 , C i B ) \epsilon_i^B \sim \mathcal{N}(0, C_i^B) ϵiB∼N(0,CiB)

即每个观测点是其潜在点加上独立高斯噪声。

同时假设存在刚体变换 T ∗ T^\ast T∗ 使得：

b ^ i = T ∗ a ^ i \hat{b}_i = T^\ast \hat{a}_i b^i=T∗a^i

2 定义误差向量

定义点对误差：

d i ( T ) = b i − T a i d_i(T) = b_i - T a_i di(T)=bi−Tai

代入观测模型：

d i ( T ∗ ) = b i − T ∗ a i = ( b ^ i + ϵ i B ) − T ∗ ( a ^ i + ϵ i A ) = ( b ^ i − T ∗ a ^ i ) + ( ϵ i B − T ∗ ϵ i A ) \begin{aligned} d_i(T^\ast) &= b_i - T^\ast a_i \ &= (\hat{b}_i + \epsilon_i^B) - T^\ast (\hat{a}_i + \epsilon_i^A) \ &= (\hat{b}_i - T^\ast \hat{a}_i) + (\epsilon_i^B - T^\ast \epsilon_i^A) \end{aligned} di(T∗)=bi−T∗ai =(b^i+ϵiB)−T∗(a^i+ϵiA) =(b^i−T∗a^i)+(ϵiB−T∗ϵiA)

注意到 b ^ i − T ∗ a ^ i = 0 \hat{b}_i - T^\ast \hat{a}_i = 0 b^i−T∗a^i=0（由理想变换关系成立），因此：

d i ( T ∗ ) = ϵ i B − T ∗ ϵ i A d_i(T^\ast) = \epsilon_i^B - T^\ast \epsilon_i^A di(T∗)=ϵiB−T∗ϵiA

3 误差分布

由于 ϵ i A \epsilon_i^A ϵiA 和 ϵ i B \epsilon_i^B ϵiB 独立，且都是高斯分布，利用高斯分布线性变换性质：

若 x ∼ N ( 0 , Σ x ) x \sim \mathcal{N}(0, \Sigma_x) x∼N(0,Σx)， y = A x ⟹ y ∼ N ( 0 , A Σ x A T ) y = A x \implies y \sim \mathcal{N}(0, A \Sigma_x A^T) y=Ax⟹y∼N(0,AΣxAT)
两个独立高斯相加： x ∼ N ( 0 , Σ x ) , y ∼ N ( 0 , Σ y ) ⟹ x + y ∼ N ( 0 , Σ x + Σ y ) x \sim \mathcal{N}(0, \Sigma_x), y \sim \mathcal{N}(0, \Sigma_y) \implies x + y \sim \mathcal{N}(0, \Sigma_x + \Sigma_y) x∼N(0,Σx),y∼N(0,Σy)⟹x+y∼N(0,Σx+Σy)

因此：

d i ( T ∗ ) = ϵ i B − T ∗ ϵ i A ∼ N ( 0 , C i B + T ∗ C i A ( T ∗ ) T ) \begin{aligned} d_i(T^\ast) &= \epsilon_i^B - T^\ast \epsilon_i^A \ &\sim \mathcal{N}(0, C_i^B + T^\ast C_i^A (T^\ast)^T) \end{aligned} di(T∗)=ϵiB−T∗ϵiA ∼N(0,CiB+T∗CiA(T∗)T)

这里 ϵ i B \epsilon_i^B ϵiB 对应 C i B C_i^B CiB， − T ∗ ϵ i A -T^\ast \epsilon_i^A −T∗ϵiA 对应 T ∗ C i A ( T ∗ ) T T^\ast C_i^A (T^\ast)^T T∗CiA(T∗)T，二者独立，协方差直接相加。

4 小结

公式

可以分为两部分理解：

均值部分 ： b ^ i − T ∗ a ^ i \hat{b}_i - T^\ast \hat{a}_i b^i−T∗a^i 表示理想刚体变换下的误差，理论上为零。
协方差部分 ： C i B + T ∗ C i A ( T ∗ ) T C_i^B + T^\ast C_i^A (T^\ast)^T CiB+T∗CiA(T∗)T 表示观测噪声累积后的不确定性，用于加权马氏距离。

这也是 Generalized-ICP 与普通 ICP 的本质区别：误差被建模为概率分布，并使用协方差加权代价函数。

二、最大似然（MLE）到线性化求解增量

1. 问题回顾（从 MLE 出发）

我们以第 i 个匹配对为单位，定义当前变换下的残差：

d i ( T ) = b i − T a i , d_i(T) = b_i - T a_i, di(T)=bi−Tai,

概率模型给出（忽略均值项，视当前 T T T 为待估）代价为马氏距离之和（MLE 等价于最小化负对数似然）：

E ( T ) = ∑ i d i ( T ) T S i − 1 d i ( T ) , E(T) = \sum_i d_i(T)^T S_i^{-1} d_i(T), E(T)=i∑di(T)TSi−1di(T),

其中通常取

S i = C i B + R C i A R T S_i = C_i^B + R C_i^A R^T Si=CiB+RCiART

（用当前 T = ( R , t ) T=(R,t) T=(R,t) 来构造 S i S_i Si）。

我们要在当前估计 T T T 处线性化 d i ( T ) d_i(T) di(T)，得到关于增量参数 ξ ∈ R 6 \xi\in\mathbb{R}^6 ξ∈R6 的线性最小二乘问题并求解增量。

2. 参数化变换与微扰模型（左乘扰动，常用）

选择常见的左扰动参数化（也可以用右扰动，但雅可比会不同）：

令当前估计为 T = ( R , t ) T=(R,t) T=(R,t)，用 ξ = ( ϕ , ρ ) \xi=(\phi,\rho) ξ=(ϕ,ρ)，其中 ϕ ∈ R 3 \phi\in\mathbb{R}^3 ϕ∈R3（小旋转向量）， ρ ∈ R 3 \rho\in\mathbb{R}^3 ρ∈R3（小平移向量）。定义扰动

T ( ξ ) = exp ⁡ ( ξ ^ ) T , T(\xi) = \exp(\hat\xi) T, T(ξ)=exp(ξ^)T,

其中
ξ ^ = [ ϕ ^ ρ 0 T 0 ] \hat{\xi} = \begin{bmatrix} \hat{\boldsymbol\phi} & \boldsymbol\rho \\ \mathbf{0}^T & 0 \end{bmatrix} ξ^=[ϕ^0Tρ0]

（ ϕ ^ \hat\phi ϕ^ 为 3 × 3 3\times3 3×3 的反对称矩阵）。对小 ξ \xi ξ 有一阶近似：

exp ⁡ ( ξ ^ ) ≈ [ I + ϕ ^ ρ 0 1 ] . \exp(\hat\xi) \approx \begin{bmatrix} I + \hat\phi & \rho \\ 0 & 1\end{bmatrix}. exp(ξ^)≈[I+ϕ^0ρ1].

因此作用在点 a i a_i ai 上：

T ( ξ ) a i ≈ ( I + ϕ ^ ) R a i + t + ρ = R a i + t + ϕ ^ ( R a i ) + ρ . \begin{aligned} T(\xi) a_i &\approx (I + \hat\phi) R a_i + t + \rho \ &= R a_i + t + \hat\phi (R a_i) + \rho. \end{aligned} T(ξ)ai≈(I+ϕ^)Rai+t+ρ =Rai+t+ϕ^(Rai)+ρ.

于是残差：

d i ( ξ ) = b i − T ( ξ ) a i ≈ b i − ( R a i + t ) ⏟ e i − ϕ ^ ( R a i ) − ρ , \begin{aligned} d_i(\xi) &= b_i - T(\xi) a_i \ &\approx \underbrace{b_i - (R a_i + t)}_{e_i} - \hat\phi (R a_i) - \rho, \end{aligned} di(ξ)=bi−T(ξ)ai ≈ei bi−(Rai+t)−ϕ^(Rai)−ρ,

其中我们定义当前残差（在当前 T T T 下）为

e i = b i − ( R a i + t ) . e_i = b_i - (R a_i + t). ei=bi−(Rai+t).

把 ϕ ^ ( R a i ) \hat\phi (R a_i) ϕ^(Rai) 写成更方便的雅可比形式。记向量 p i = R a i p_i = R a_i pi=Rai，则 ϕ ^ p i = ϕ × p i \hat\phi p_i = \phi \times p_i ϕ^pi=ϕ×pi，并可表示为矩阵乘法：

ϕ ^ p i = − ( p i ) ∧ ϕ , \hat\phi p_i = - (p_i)^\wedge \phi, ϕ^pi=−(pi)∧ϕ,

所以

ϕ ^ p i − ρ = ( p i ) ∧ ϕ − ρ . \hat\phi p_i - \rho = (p_i)^\wedge \phi - \rho. ϕ^pi−ρ=(pi)∧ϕ−ρ.

因此一阶线性化写成矩阵形式为

d i ( ξ ) ≈ e i + J i ξ , d_i(\xi) \approx e_i + J_i \xi, di(ξ)≈ei+Jiξ,

其中雅可比为（ 3 × 6 3\times6 3×6）：

J i = [ ( p i ) ∧ − I 3 ] , p i = R a i . J_i = \begin{bmatrix} (p_i)^\wedge & -I_3 \end{bmatrix},\qquad p_i = R a_i. Ji=[(pi)∧−I3],pi=Rai.

（检验： J i ξ = ( p i ) ∧ ϕ − ρ J_i \xi = (p_i)^\wedge \phi - \rho Jiξ=(pi)∧ϕ−ρ，与上式一致。）

说明：如果采用右乘扰动 T ( ξ ) = T exp ⁡ ( ξ ^ ) T(\xi)=T\exp(\hat\xi) T(ξ)=Texp(ξ^)，则 J i J_i Ji 将不同（具体形式可查 se(3) 推导），但实现时保持一致的扰动约定即可。

3. 线性化代价与法方程

将线性化代入代价：

E ( ξ ) ≈ ∑ i ( e i + J i ξ ) T S i − 1 ( e i + J i ξ ) = ∑ i ( e i T S i − 1 e i + 2 e i T S i − 1 J i ξ + ξ T J i T S i − 1 J i ξ ) . \begin{aligned} E(\xi) &\approx \sum_i (e_i + J_i \xi)^T S_i^{-1} (e_i + J_i \xi) \ &= \sum_i \big( e_i^T S_i^{-1} e_i + 2 e_i^T S_i^{-1} J_i \xi + \xi^T J_i^T S_i^{-1} J_i \xi \big). \end{aligned} E(ξ)≈i∑(ei+Jiξ)TSi−1(ei+Jiξ) =i∑(eiTSi−1ei+2eiTSi−1Jiξ+ξTJiTSi−1Jiξ).

去掉常数项并对 ξ \xi ξ 求导（令梯度为 0）得到线性方程组（Gauss--Newton normal equations）：

( ∑ i J i T S i − 1 J i ) ξ = − ∑ i J i T S i − 1 e i . \left( \sum_i J_i^T S_i^{-1} J_i \right) \xi = - \sum_i J_i^T S_i^{-1} e_i. (i∑JiTSi−1Ji)ξ=−i∑JiTSi−1ei.

写成简洁矩阵形式：

A ξ = b , A \xi = b, Aξ=b,

其中

A = ∑ i J i T S i − 1 J i ( 6 × 6 ) , b = − ∑ i J i T S i − 1 e i ( 6 × 1 ) . A = \sum_i J_i^T S_i^{-1} J_i\quad(6\times6),\qquad b = -\sum_i J_i^T S_i^{-1} e_i\quad(6\times1). A=i∑JiTSi−1Ji(6×6),b=−i∑JiTSi−1ei(6×1).

解出 ξ \xi ξ 后，按扰动模型更新变换：

T ← exp ⁡ ( ξ ^ ) , T . T \leftarrow \exp(\hat\xi), T. T←exp(ξ^),T.

（若用于 LM，可在 A A A 对角上加阻尼项 λ I \lambda I λI。）

4. 展开 J i T S − 1 J i J_i^T S^{-1} J_i JiTS−1Ji 的分块形式（

将 J i = [ ( p i ) ∧ , − I ] J_i = [ (p_i)^\wedge, \ -I ] Ji=[(pi)∧, −I]，则

J i T S − 1 J i [ ( ( p i ) ∧ ) T S − 1 ( p i ) ∧ − ( ( p i ) ∧ ) T S − 1 S − 1 ( p i ) ∧ S − 1 ] . J_i^T S^{-1} J_i \begin{bmatrix} ((p_i)^\wedge)^T S^{-1} (p_i)^\wedge & -((p_i)^\wedge)^T S^{-1} \ S^{-1} (p_i)^\wedge & S^{-1} \end{bmatrix}. JiTS−1Ji[((pi)∧)TS−1(pi)∧−((pi)∧)TS−1 S−1(pi)∧S−1].

而单项 J i T S − 1 e i J_i^T S^{-1} e_i JiTS−1ei 为

J i T S − 1 e i = [ ( ( p i ) ∧ ) T S − 1 e i S − 1 e i ] . J_i^T S^{-1} e_i = \begin{bmatrix} ((p_i)^\wedge)^T S^{-1} e_i \ S^{-1} e_i \end{bmatrix}. JiTS−1ei=[((pi)∧)TS−1ei S−1ei].

注意： ( ( p i ) ∧ ) T = − ( p i ) ∧ ((p_i)^\wedge)^T = - (p_i)^\wedge ((pi)∧)T=−(pi)∧（反对称矩阵的转置等于其负值），因此在实际代码中可根据数值稳定性直接计算乘积形式以避免构造不必要的中间矩阵。

5. 实现要点

5.1 伪代码（单次 GN 迭代，基于当前 T = ( R , t ) T=(R,t) T=(R,t)）

text 复制代码

A = zeros(6,6)
b = zeros(6,1)

for each matched pair (a_i, b_i):
    p = R * a_i                 # 3x1
    e = b_i - (p + t)           # 3x1

    S = Cb_i + R * Ca_i * R^T   # 3x3
    # 稳定性：若 S 条件不佳，可做正则： S += eps * I
    S_inv = inverse(S)          # 3x3  (或使用 Cholesky solve 而非直接逆)

    # 构造 J (3x6): [ p^  | -I ]
    p_hat = skew(p)             # 3x3
    J = [ p_hat, -I3 ]          # 3x6

    # 累加
    A += J^T * S_inv * J        # 6x6
    b += - J^T * S_inv * e      # 6x1

# 求解 A xi = b
# 在数值上对 A 做对称化和正则化
A = 0.5*(A + A^T)
A += lambda * I6  # LM 阻尼或小正则 eps

xi = solve(A, b)    # 用 LDLT / Cholesky / robust solver

# 更新变换（左扰动）
T = expmap_se3(xi) * T

5.2 关于 S − 1 S^{-1} S−1 的计算

直接求逆开销小（3×3），但更稳妥的是用 Cholesky 分解对秩良好的 S S S 求解 S − 1 v S^{-1} v S−1v，即在乘法 S_inv * vec 中用线性求解避免构造逆矩阵。
若 S S S 接近奇异，先做正则 S ← S + ϵ I S \leftarrow S + \epsilon I S←S+ϵI。

5.3 关于 `skew(p)`（ p ∧ p^\wedge p∧）

定义：

p ∧ = [ 0 − p z p y p z 0 − p x − p y p x 0 ] p^\wedge = \begin{bmatrix} 0 & -p_z & p_y\\ p_z & 0 & -p_x\\ -p_y & p_x & 0 \end{bmatrix} p∧= 0pz−py−pz0pxpy−px0

在代码中直接构造或用手写一小函数。

5.4 关于扰动方向（左扰动 vs 右扰动）

上面推导假设左扰动： T ← exp ⁡ ( ξ ^ ) T T \leftarrow \exp(\hat\xi) T T←exp(ξ^)T。这在多数学术实现中常见。
若你用右扰动（ T ← T exp ⁡ ( ξ ^ ) T \leftarrow T \exp(\hat\xi) T←Texp(ξ^)），则 J i J_i Ji 为 [ − R a i ∧ , − I ] [-R a_i^\wedge, -I] [−Rai∧,−I] 或其它形式 ------ 要确保 J i J_i Ji 与更新方式一致，否则会出现奇怪偏差。

6. 雅可比的清晰表达

令 p i = R a i p_i = R a_i pi=Rai， e i = b i − ( p i + t ) e_i = b_i - (p_i + t) ei=bi−(pi+t)， S i = C i B + R C i A R T S_i = C_i^B + R C_i^A R^T Si=CiB+RCiART。则：

J i = [ ( p i ) ∧ , − I 3 ] . J_i = \begin{bmatrix} (p_i)^\wedge &\ ,-I_3 \end{bmatrix}. Ji=[(pi)∧ ,−I3].

线性方程汇总：

A = ∑ i J i T S i − 1 J i , b = − ∑ i J i T S i − 1 e i . A = \sum_i J_i^T S_i^{-1} J_i,\qquad b = -\sum_i J_i^T S_i^{-1} e_i. A=i∑JiTSi−1Ji,b=−i∑JiTSi−1ei.

求解 A ξ = b A \xi = b Aξ=b，更新：

T ← exp ⁡ ( ξ ^ ) T . T \leftarrow \exp(\hat\xi) T. T←exp(ξ^)T.

7. 工程实践建议

鲁棒核 ：在累加代价前对每个残差使用 Huber 或 Tukey 权重 w ( e i ) w(e_i) w(ei)，即替换项为 w i J i T S − 1 J i w_i J_i^T S^{-1} J_i wiJiTS−1Ji 与 w i J i T S − 1 e i w_i J_i^T S^{-1} e_i wiJiTS−1ei，对外点更稳健。
预条件 / 归一化 ：若点云尺度或数量差异大，适当标准化或对 A A A 做对角预条件。
S 的近似 ：为加速可以只用协方差的主方向近似或对 S S S 做快速 Cholesky 重用。
并行化 ：A 与 b 的累加可并行（每对计算局部贡献再归约）。
增量步长控制 ：如果 ∣ ξ ∣ |\xi| ∣ξ∣ 很大，可限制步长或用 LM 阻尼避免发散。
更新顺序：若多层/多尺度配准，可在粗层多次迭代后下采样到细层。

8. 小结

残差： d i ( T ) = b i − ( R a i + t ) d_i(T)=b_i - (R a_i + t) di(T)=bi−(Rai+t)，当前残差 e i e_i ei.
线性化增量： d i ( ξ ) ≈ e i + J i ξ d_i(\xi) \approx e_i + J_i \xi di(ξ)≈ei+Jiξ， J i = [ ( R a i ) ∧ , − I ] J_i=[(R a_i)^\wedge, -I] Ji=[(Rai)∧,−I].
正规方程： ( ∑ J i T S i − 1 J i ) ξ = − ∑ J i T S i − 1 e i (\sum J_i^T S_i^{-1} J_i)\xi = -\sum J_i^T S_i^{-1} e_i (∑JiTSi−1Ji)ξ=−∑JiTSi−1ei.
更新： T ← exp ⁡ ( ξ ^ ) T T \leftarrow \exp(\hat\xi) T T←exp(ξ^)T。