欢迎光临
我们一直在努力

点云配准之-Colored Point Cloud Registration(ColorICP)

1. 简介

Colored Point Cloud Registration Revisited 重新审视并系统化了融合颜色(photometric / color)信息的点云配准问题。论文的核心目标是:

在存在噪声、遮挡、视角变化和几何退化的情况下,通过联合几何与颜色一致性,提高点云配准的稳定性、精度和收敛域。

与传统仅依赖几何(point-to-point / point-to-plane ICP)的方法不同,本文强调:

  • 颜色信息是强约束而非装饰信息
  • 合理建模颜色残差,可以在几何约束不足时(如平面、对称结构)避免错误收敛
  • 颜色与几何的融合应当体现在统一的优化目标中,而非简单加权拼接

论文在理论、建模和实验层面对 Colored ICP(CICP) 进行了“再推导(Revisited)”,澄清其数学本质、假设条件与适用边界。 在这里插入图片描述


2. 算法背景

2.1 纯几何配准的局限

传统 ICP 族方法存在根本问题:

  • 几何退化

    • 大平面、圆柱、对称结构 → 多解或弱约束
  • 局部最优严重依赖初值

  • 遮挡 / 稀疏 / 低纹理场景 下匹配不稳定

即使是 point-to-plane ICP,本质仍然只使用几何法向信息。


2.2 颜色信息的潜力与挑战

颜色能提供:

  • 对称结构的歧义破除
  • 平面内的额外约束
  • 纹理丰富区域的更强对齐信号

但挑战在于:

  • 颜色受 光照变化、曝光、白平衡 影响
  • RGB 空间并非欧氏意义下的“物理一致”
  • 若建模不当,颜色会引入系统性偏差

本文的意义就在于: 以正确的统计建模方式,把颜色真正变成“约束”,而不是噪声。


3. 文章贡献

核心贡献可以总结为 5 点:

  • 统一的几何–颜色联合误差模型 将点云配准建模为一个联合最小二乘问题,而非两阶段或启发式加权。

  • 重新推导 Colored ICP 的目标函数 明确 CICP 实际是在最小化一个带协方差的 MAP(最大后验)估计问题。

  • 颜色残差的正确线性化方式 论文指出:颜色约束必须在光度一致性假设下线性化,避免错误 Jacobian。

  • 几何–颜色权重的物理解释 权重不再是“经验超参数”,而是与噪声协方差直接相关。

  • 实验性地证明:颜色显著扩大收敛域 在弱几何约束场景中,CICP 明显优于纯几何 ICP。


  • 4. 需解决的关键问题

    给定两帧彩色点云:

    P

    =

    (

    p

    i

    ,

    c

    i

    )

    ,

    Q

    =

    (

    q

    j

    ,

    d

    j

    )

    \\mathcal{P} = {(\\mathbf{p}_i, \\mathbf{c}_i)}, \\quad \\mathcal{Q} = {(\\mathbf{q}_j, \\mathbf{d}_j)}

    P=(pi,ci),Q=(qj,dj)

    目标是估计刚体变换:

    T

    =

    (

    R

    ,

    t

    )

    S

    E

    (

    3

    )

    \\mathbf{T} = (\\mathbf{R}, \\mathbf{t}) \\in SE(3)

    T=(R,t)SE(3)

    使得:

    • 几何一致:

      (

      q

      R

      p

      +

      t

      )

      (\\mathbf{q} \\approx \\mathbf{R}\\mathbf{p} + \\mathbf{t})

      (qRp+t)

    • 颜色一致:

      (

      d

      c

      )

      (\\mathbf{d} \\approx \\mathbf{c})

      (dc)

    关键难点:

    • 如何定义颜色残差
    • 如何与几何残差统一优化
    • 如何保持数值稳定性与收敛性

    5. 论文提出的算法流程

    整体流程可视为 Colored ICP 的“理论化版本”:

  • 建立对应关系(通常仍是最近邻)

  • 构建联合残差

    • 几何残差(point-to-plane)
    • 颜色残差(photometric)
  • 统一最小二乘目标

  • 线性化 + 高斯–牛顿迭代

  • 更新位姿

  • 迭代至收敛

  • 与传统 ICP 的区别: 残差维度更高、信息矩阵不再是单位阵


    6. 核心方法细节

    6.1 RGB-D 图像对齐理论

    本节先回顾用于 RGB-D 图像对齐的光度目标函数(photometric objective)[35, 25],并将其与定义在同一图像平面上的几何目标函数相结合。这一节将引入相关符号,并为第 6.2节中介绍的彩色点云对齐奠定基础。

    一幅 RGB-D 图像由一张彩色图像

    I

    I

    I 和一张与其配准到同一坐标系下的深度图像

    D

    D

    D 组成。为简化起见,这里我们使用强度图像(intensity images)。给定一对 RGB-D 图像

    (

    I

    i

    ,

    D

    i

    )

    (I_i, D_i)

    (Ii,Di)

    (

    I

    j

    ,

    D

    j

    )

    (I_j, D_j)

    (Ij,Dj),以及一个能够将

    (

    I

    j

    ,

    D

    j

    )

    (I_j, D_j)

    (Ij,Dj) 粗略对齐到

    (

    I

    i

    ,

    D

    i

    )

    (I_i, D_i)

    (Ii,Di) 的初始变换

    T

    0

    T_0

    T0,目标是求解一个最优变换,使得这两幅 RGB-D 图像在稠密意义下对齐。


    光度目标函数

    光度目标函数

    E

    I

    E_I

    EI 定义为像素强度差的平方和 [35, 25]:

    E

    I

    (

    T

    )

    =

    x

    (

    I

    i

    (

    x

    )

    I

    j

    (

    x

    )

    )

    2

    ,

    (1)

    E_I(T) = \\sum_x \\bigl(I_i(x') – I_j(x)\\bigr)^2, \\tag{1}

    EI(T)=x(Ii(x)Ij(x))2,(1) 其中

    x

    =

    (

    u

    ,

    v

    )

    x = (u, v)^\\top

    x=(u,v) 表示图像

    (

    I

    j

    ,

    D

    j

    )

    (I_j, D_j)

    (Ij,Dj) 中的一个像素,而

    x

    =

    (

    u

    ,

    v

    )

    x' = (u', v')^\\top

    x=(u,v) 是其在图像

    (

    I

    i

    ,

    D

    i

    )

    (I_i, D_i)

    (Ii,Di) 中的对应像素。

    该对应关系的建立过程如下:首先将深度像素

    (

    x

    ,

    D

    j

    (

    x

    )

    )

    (x, D_j(x))

    (x,Dj(x)) 转换为

    (

    I

    j

    ,

    D

    j

    )

    (I_j, D_j)

    (Ij,Dj) 相机坐标系下的三维点;然后利用变换

    T

    T

    T 对该三维点进行变换;最后将其投影到

    (

    I

    i

    ,

    D

    i

    )

    (I_i, D_i)

    (Ii,Di) 的图像平面上。形式化地,有

    x

    =

    g

    u

    v

    (

    s

    (

    h

    (

    x

    ,

    D

    j

    (

    x

    )

    )

    ,

    T

    )

    )

    .

    (2)

    x' = g_{uv}\\bigl(s(h(x, D_j(x)), T)\\bigr). \\tag{2}

    x=guv(s(h(x,Dj(x)),T)).(2)

    其中,

    h

    h

    h 表示从深度像素到齐次坐标下三维点的映射:

    h

    (

    u

    ,

    v

    ,

    d

    )

    =

    (

    (

    u

    c

    x

    )

    ,

    d

    f

    x

     

    (

    v

    c

    y

    )

    ,

    d

    f

    y

     

    d

     

    1

    )

    ,

    (3)

    h(u, v, d) = \\begin{pmatrix} \\dfrac{(u – c_x), d}{f_x} \\ \\dfrac{(v – c_y), d}{f_y} \\ d \\ 1 \\end{pmatrix}, \\tag{3}

    h(u,v,d)=(fx(ucx),d fy(vcy),d d 1),(3) 这里

    f

    x

    f_x

    fx

    f

    y

    f_y

    fy 是相机的焦距,

    (

    c

    x

    ,

    c

    y

    )

    (c_x, c_y)

    (cx,cy) 是主点坐标。

    s

    s

    s 表示刚体变换:

    s

    (

    h

    ,

    T

    )

    =

    T

    h

    .

    (4)

    s(h, T) = T h. \\tag{4}

    s(h,T)=Th.(4)

    g

    g

    g

    h

    h

    h 的逆映射,用于将三维点映射回深度像素:

    g

    (

    s

    x

    ,

    s

    y

    ,

    s

    z

    ,

    1

    )

    =

    (

    s

    x

    f

    x

    s

    z

    +

    c

    x

     

    s

    y

    f

    y

    s

    z

    +

    c

    y

     

    s

    z

    )

    .

    (5)

    g(s_x, s_y, s_z, 1) = \\begin{pmatrix} \\dfrac{s_x f_x}{s_z} + c_x \\ \\dfrac{s_y f_y}{s_z} + c_y \\ s_z \\end{pmatrix}. \\tag{5}

    g(sx,sy,sz,1)=(szsxfx+cx szsyfy+cy sz).(5) 其中

    g

    g

    g 的前两个分量,记为

    g

    u

    v

    g_{uv}

    guv,构成了图像

    (

    I

    i

    ,

    D

    i

    )

    (I_i, D_i)

    (Ii,Di) 平面上的对应像素

    x

    x'

    x


    几何目标函数

    类似地,我们可以定义一个几何目标函数

    E

    D

    E_D

    ED,用于比较像素

    x

    x

    x

    x

    x'

    x 处的深度值。需要注意的是,直接比较

    D

    i

    (

    x

    )

    D_i(x')

    Di(x)

    D

    j

    (

    x

    )

    D_j(x)

    Dj(x) 会产生错误结果,因为这两个深度值是在不同的相机坐标系中测量得到的。因此,我们将

    D

    i

    (

    x

    )

    D_i(x')

    Di(x) 与变换后的深度

    g

    d

    g_d

    gd 进行比较,其中

    g

    d

    g_d

    gd 是式 (5) 中

    g

    g

    g 的第三个分量:

    E

    D

    (

    T

    )

    =

    x

    (

    D

    i

    (

    x

    )

    g

    d

    (

    s

    (

    h

    (

    x

    ,

    D

    j

    (

    x

    )

    )

    ,

    T

    )

    )

    )

    2

    .

    (6)

    E_D(T) = \\sum_x \\bigl(D_i(x') – g_d(s(h(x, D_j(x)), T))\\bigr)^2. \\tag{6}

    ED(T)=x(Di(x)gd(s(h(x,Dj(x)),T)))2.(6)

    需要强调的是,光度目标函数

    E

    I

    E_I

    EI 和几何目标函数

    E

    D

    E_D

    ED 都定义在同一个参数化域上。在下一节中,我们将展示,通过改变参数化域,可以将这些目标函数推广到非结构化点云的情形。


    联合光度与几何目标

    联合的光度—几何目标函数可以通过将

    E

    I

    E_I

    EI

    E

    D

    E_D

    ED 组合得到:

    E

    (

    T

    )

    =

    (

    1

    σ

    )

    E

    I

    (

    T

    )

    +

    σ

    E

    D

    (

    T

    )

    ,

    (7)

    E(T) = (1 – \\sigma) E_I(T) + \\sigma E_D(T), \\tag{7}

    E(T)=(1σ)EI(T)+σED(T),(7) 其中

    σ

    [

    0

    ,

    1

    ]

    \\sigma \\in [0, 1]

    σ[0,1] 是一个权重系数,用于平衡光度项与几何项的相对贡献。


    6.2 彩色点云配准(Colored Point Cloud Registration)

    在本节中,我们将联合优化目标函数(公式 7)推广到彩色点云对齐问题。


    6.2.1 参数化(Parameterization)

    P

    P

    P 为彩色点云,

    C

    (

    p

    )

    C(p)

    C(p) 为一个离散函数,用于获取每个点

    p

    p

    p 的颜色(强度)。为了在优化中使用颜色信息,我们需要将

    C

    (

    p

    )

    C(p)

    C(p) 推广为连续函数,以便计算其梯度。

    概念上,我们为每个点

    p

    P

    p \\in P

    pP 引入一个虚拟正交相机(virtual orthogonal camera),相机沿点的法向量

    n

    p

    n_p

    np 观察该点。该虚拟相机的图像平面为点

    p

    p

    p 的切平面。平面上定义一个虚拟图像,可表示为连续颜色函数

    C

    p

    (

    u

    )

    C_p(u)

    Cp(u),其中

    u

    u

    u 为沿切平面发出的向量,满足

    u

    n

    p

    =

    0

    u \\cdot n_p = 0

    unp=0

    C

    p

    (

    u

    )

    C_p(u)

    Cp(u) 可用一阶近似表示为:

    C

    p

    (

    u

    )

    C

    (

    p

    )

    +

    d

    p

    u

    ,

    (8)

    C_p(u) \\approx C(p) + d_p^\\top u, \\tag{8}

    Cp(u)C(p)+dpu,(8)

    其中

    d

    p

    d_p

    dp

    C

    p

    (

    u

    )

    C_p(u)

    Cp(u) 的梯度,通过对点

    p

    p

    p 的局部邻域

    N

    p

    N_p

    Np 内的颜色数据

    C

    (

    p

    )

    p

    N

    p

    {C(p') \\mid p' \\in N_p}

    C(p)pNp 做最小二乘拟合来估计。

    具体地,设

    f

    (

    s

    )

    f(s)

    f(s) 为将三维点

    s

    s

    s 投影到点

    p

    p

    p 的切平面的函数:

    f

    (

    s

    )

    =

    s

    n

    p

    (

    s

    p

    )

    n

    p

    ,

    (9)

    f(s) = s – n_p (s – p)^\\top n_p, \\tag{9}

    f(s)=snp(sp)np,(9)

    则计算梯度

    d

    p

    d_p

    dp 的最小二乘目标函数为:

    L

    (

    d

    p

    )

    =

    p

    N

    p

    (

    C

    p

    (

    f

    (

    p

    )

    p

    )

    C

    (

    p

    )

    )

    2

    p

    N

    p

    (

    C

    (

    p

    )

    +

    d

    p

    (

    f

    (

    p

    )

    p

    )

    C

    (

    p

    )

    )

    2

    ,

    (10)

    L(d_p) = \\sum_{p' \\in N_p} \\big(C_p(f(p') – p) – C(p')\\big)^2 \\approx \\sum_{p' \\in N_p} \\big(C(p) + d_p^\\top(f(p') – p) – C(p')\\big)^2, \\tag{10}

    L(dp)=pNp(Cp(f(p)p)C(p))2pNp(C(p)+dp(f(p)p)C(p))2,(10)

    同时约束

    d

    p

    n

    p

    =

    0

    d_p^\\top n_p = 0

    dpnp=0。这是一个线性最小二乘问题,可以在预处理阶段高效求解。相关公式推导材料参考:点云配准之-ColorICP中公式详细推导

    同样地,我们可以假设虚拟相机具有深度通道,并定义连续深度函数

    G

    p

    (

    u

    )

    G_p(u)

    Gp(u)。由于梯度在原点为 0,其一阶近似可表示为常数函数:

    G

    p

    (

    u

    )

    (

    o

    p

    p

    )

    n

    p

    ,

    (11)

    G_p(u) \\approx (o_p – p)^\\top n_p, \\tag{11}

    Gp(u)(opp)np,(11)

    其中

    o

    p

    o_p

    op 是虚拟相机的原点。


    6.2.2 目标函数(Objective)

    P

    P

    P

    Q

    Q

    Q 为两组彩色点云,

    T

    0

    T_0

    T0 为粗略初始对齐。我们的目标是找到最优变换

    T

    T

    T

    Q

    Q

    Q 对齐到

    P

    P

    P

    联合优化目标函数定义为:

    E

    (

    T

    )

    =

    (

    1

    σ

    )

    E

    C

    (

    T

    )

    +

    σ

    E

    G

    (

    T

    )

    ,

    (12)

    E(T) = (1 – \\sigma) E_C(T) + \\sigma E_G(T), \\tag{12}

    E(T)=(1σ)EC(T)+σEG(T),(12)

    其中

    E

    C

    E_C

    EC 为光度(Photometric)项,

    E

    G

    E_G

    EG 为几何(Geometric)项,

    σ

    [

    0

    ,

    1

    ]

    \\sigma \\in [0, 1]

    σ[0,1] 为平衡二者的权重。

    光度项

    E

    C

    E_C

    EC 通过推广公式 1 中的光度残差

    E

    I

    E_I

    EI 定义。这里的修改是基于对应集

    K

    =

    (

    p

    ,

    q

    )

    K = {(p, q)}

    K=(p,q) 而非像素集

    x

    {x}

    x。对应集

    K

    K

    K 通过 ICP 算法生成:在每次优化迭代中,

    K

    K

    K 被重新计算为

    P

    P

    P 与当前变换下的

    Q

    Q

    Q

    T

    k

    Q

    T_k Q

    TkQ)之间距离小于阈值

    ϵ

    \\epsilon

    ϵ 的点对。

    利用第 6.2.1 节中的虚拟相机,点

    q

    q

    q 被投影到

    p

    p

    p 的切平面上的

    q

    q'

    q

    q

    =

    f

    (

    s

    (

    q

    ,

    T

    )

    )

    ,

    (13)

    q' = f(s(q, T)), \\tag{13}

    q=f(s(q,T)),(13)

    其中

    s

    s

    s 为刚性变换(公式 4),

    f

    f

    f 为投影函数(公式 9)。使用局部颜色函数

    C

    p

    C_p

    Cp(公式 8)和投影点

    q

    q'

    q(公式 13),光度项定义为:

    E

    C

    (

    T

    )

    =

    (

    p

    ,

    q

    )

    K

    (

    C

    p

    (

    q

    )

    C

    (

    q

    )

    )

    2

    .

    (14)

    E_C(T) = \\sum_{(p,q) \\in K} \\big(C_p(q') – C(q)\\big)^2. \\tag{14}

    EC(T)=(p,q)K(Cp(q)C(q))2.(14)

    几何项

    E

    G

    E_G

    EG 同样是对公式 6 的推广:

    E

    G

    (

    T

    )

    =

    (

    p

    ,

    q

    )

    K

    (

    G

    p

    (

    q

    )

    (

    o

    p

    s

    (

    q

    ,

    T

    )

    )

    n

    p

    )

    2

    .

    (15)

    E_G(T) = \\sum_{(p,q) \\in K} \\big(G_p(q') – (o_p – s(q,T))^\\top n_p\\big)^2. \\tag{15}

    EG(T)=(p,q)K(Gp(q)(ops(q,T))np)2.(15)

    代入公式 11 消去

    o

    p

    o_p

    op,得到:

    E

    G

    (

    T

    )

    =

    (

    p

    ,

    q

    )

    K

    (

    (

    s

    (

    q

    ,

    T

    )

    p

    )

    n

    p

    )

    2

    .

    (16)

    E_G(T) = \\sum_{(p,q) \\in K} \\big((s(q,T) – p)^\\top n_p\\big)^2. \\tag{16}

    EG(T)=(p,q)K((s(q,T)p)np)2.(16)

    该函数等价于 ICP 算法中的点到平面目标函数(point-to-plane ICP)。当仅使用几何项(

    σ

    =

    1

    \\sigma=1

    σ=1)时,本算法退化为传统点到平面 ICP。

    综上,联合优化目标函数可写为:

    E

    (

    T

    )

    =

    (

    1

    σ

    )

    (

    p

    ,

    q

    )

    K

    r

    C

    (

    p

    ,

    q

    )

    (

    T

    )

    2

    +

    σ

    (

    p

    ,

    q

    )

    K

    r

    G

    (

    p

    ,

    q

    )

    (

    T

    )

    2

    ,

    (17)

    E(T) = (1-\\sigma) \\sum_{(p,q) \\in K} r_C^{(p,q)}(T)^2 + \\sigma \\sum_{(p,q) \\in K} r_G^{(p,q)}(T)^2, \\tag{17}

    E(T)=(1σ)(p,q)KrC(p,q)(T)2+σ(p,q)KrG(p,q)(T)2,(17)

    其中光度残差

    r

    C

    r_C

    rC 和几何残差

    r

    G

    r_G

    rG 定义为:

    r

    C

    (

    p

    ,

    q

    )

    (

    T

    )

    =

    C

    p

    (

    f

    (

    s

    (

    q

    ,

    T

    )

    )

    )

    C

    (

    q

    )

    ,

    (18)

    r_C^{(p,q)}(T) = C_p(f(s(q,T))) – C(q), \\tag{18}

    rC(p,q)(T)=Cp(f(s(q,T)))C(q),(18)

    r

    G

    (

    p

    ,

    q

    )

    (

    T

    )

    =

    (

    s

    (

    q

    ,

    T

    )

    p

    )

    n

    p

    .

    (19)

    r_G^{(p,q)}(T) = (s(q,T) – p)^\\top n_p. \\tag{19}

    rG(p,q)(T)=(s(q,T)p)np.(19)


    6.2.3 优化(Optimization)

    使用 Gauss-Newton 方法最小化非线性最小二乘目标

    E

    (

    T

    )

    E(T)

    E(T)。每次迭代中,将

    T

    T

    T 局部线性化为 6 维向量

    ξ

    =

    (

    α

    ,

    β

    ,

    γ

    ,

    a

    ,

    b

    ,

    c

    )

    ,

    \\xi = (\\alpha, \\beta, \\gamma, a, b, c),

    ξ=(α,β,γ,a,b,c),

    其中包含旋转分量

    ω

    \\omega

    ω 和平移

    t

    t

    t

    T

    T

    T 可线性近似为:

    T

    (

    1

    γ

    β

    a

    γ

    1

    α

    b

    β

    α

    1

    c

    0

    0

    0

    1

    )

    T

    k

    ,

    (20)

    T \\approx \\begin{pmatrix} 1 & -\\gamma & \\beta & a \\\\ \\gamma & 1 & -\\alpha & b \\\\ -\\beta & \\alpha & 1 & c \\\\ 0 & 0 & 0 & 1 \\end{pmatrix} T_k, \\tag{20}

    T

    1γβ0γ1α0βα10abc1

    Tk,(20)

    其中

    T

    k

    T_k

    Tk 为上一迭代得到的变换。按照 Gauss-Newton 方法,解线性系统:

    J

    r

    J

    r

    ξ

    =

    J

    r

    r

    ,

    (21)

    J_r^\\top J_r \\xi = – J_r^\\top r, \\tag{21}

    JrJrξ=Jrr,(21)

    其中

    r

    r

    r 为残差向量,

    J

    r

    J_r

    Jr 为雅可比矩阵,在

    T

    k

    T_k

    Tk 处计算:

    r

    =

    (

    1

    σ

    r

    C

     

    σ

    r

    G

    )

    ,

    (22)

    r = \\begin{pmatrix} \\sqrt{1-\\sigma} r_C \\ \\sqrt{\\sigma} r_G \\end{pmatrix}, \\tag{22}

    r=(1σ

    rC σ

    rG),(22) 在这里插入图片描述

    残差的偏导数由链式法则计算:

    r

    C

    (

    p

    ,

    q

    )

    (

    T

    )

    =

    (

    C

    p

    f

    s

    )

    ξ

    i

    =

    C

    p

    (

    f

    )

    J

    f

    (

    s

    )

    J

    s

    (

    ξ

    )

    ,

    (28-29)

    \\nabla r_C^{(p,q)}(T) = \\frac{\\partial (C_p \\circ f \\circ s)}{\\partial \\xi_i} \\\\= \\nabla C_p(f) J_f(s) J_s(\\xi), \\tag{28-29}

    rC(p,q)(T)=ξi(Cpfs)=Cp(f)Jf(s)Js(ξ),(28-29)

    r

    G

    (

    p

    ,

    q

    )

    (

    T

    )

    =

    n

    p

    J

    s

    (

    ξ

    )

    ,

    (30)

    \\nabla r_G^{(p,q)}(T) = n_p^\\top J_s(\\xi), \\tag{30}

    rG(p,q)(T)=npJs(ξ),(30)

    其中

    C

    p

    =

    d

    p

    \\nabla C_p = d_p

    Cp=dp 为预先计算好的梯度,

    J

    f

    (

    s

    )

    J_f(s)

    Jf(s) 为投影函数

    f

    f

    f 的雅可比,

    J

    s

    (

    ξ

    )

    J_s(\\xi)

    Js(ξ) 为刚性变换

    s

    s

    s

    ξ

    \\xi

    ξ 的雅可比。

    每次迭代中,计算残差

    r

    r

    r 和雅可比矩阵

    J

    r

    J_r

    Jr,解线性系统(公式 21),更新

    T

    T

    T,并映射到

    S

    E

    (

    3

    )

    SE(3)

    SE(3)。下一迭代以

    T

    k

    +

    1

    T_{k+1}

    Tk+1 重新参数化。

    相关公式推导材料参考:点云配准之-ColorICP中公式详细推导


    6.2.4 粗到细处理(Coarse-to-fine processing)

    目标函数(公式 12)为非凸函数,优化可能陷入局部最小值。为缓解此问题,我们采用 粗到细策略:

    • 构建点云金字塔(point cloud pyramid),通过体素网格下采样(voxel grid)生成不同尺度点云。
    • 每个体素内的点用其质心近似。粗尺度的残差是细尺度多个残差的组合,使目标函数更平滑,有助于 Gauss-Newton 收敛到更优解。
    • 优化从粗到细逐层进行,每层优化结果作为下一层的初始化。

    算法总结(Algorithm 1):彩色点云配准通过联合优化光度与几何残差,结合虚拟相机局部连续颜色函数、Gauss-Newton 优化与粗到细策略,实现高精度彩色点云对齐。 在这里插入图片描述


    6.3 联合最小二乘目标

    最终目标函数:

    min

    ξ

    i

    (

    r

    g

    ,

    i

    Σ

    g

    1

    2

    +

    r

    c

    ,

    i

    Σ

    c

    1

    2

    )

    \\min_{\\boldsymbol{\\xi}} \\sum_i \\left( | r_{g,i} |^2_{\\Sigma_g^{-1}} + | r_{c,i} |^2_{\\Sigma_c^{-1}} \\right)

    ξmini(rg,iΣg12+rc,iΣc12)

    这是一个加权最小二乘,但权重来自噪声模型,而非拍脑袋。


    6.4 与 MAP 的关系

    论文指出: Colored ICP 等价于:

    在高斯噪声假设下,对位姿进行 MAP 估计

    这使得:

    • 权重 = 噪声协方差的逆
    • 颜色 / 几何可自然融合
    • 可扩展到信息滤波 / 图优化框架

    7. 算法优势

  • 显著扩大收敛域 在几何弱约束场景尤其明显(墙面、地面、室内)

  • 减少错误配准 颜色破除了几何对称性导致的多解问题

  • 与 ICP 框架高度兼容 可直接嵌入现有 SLAM / 配准系统

  • 有明确统计解释 不再是 heuristic trick,而是 MAP 推导结果


  • 8. 局限与注意事项

    • 对光照变化敏感
    • 颜色梯度噪声大时可能误导优化
    • 计算量增加(Jacobian 维度更高)
    • 不适合 无纹理 / 强光变化 场景

    9. 与 TEASER / 鲁棒方法的关系

    方法对应关系鲁棒性颜色全局最优
    ICP / CICP 已知 可用
    RANSAC 未知 概率
    TEASER 已知 极高
    Colored ICP 已知 低–中

    它们解决的是不同层级的问题:

    • TEASER:解决“外点爆炸”
    • CICP:解决“几何退化 + 收敛域”

    10. 参考文献

  • Park et al., Colored Point Cloud Registration Revisited, ICCV
  • Besl & McKay, A Method for Registration of 3-D Shapes, PAMI
  • Newcombe et al., Dense Visual SLAM, ISMAR
  • Yang & Carlone, TEASER, RSS

  • 相关公式推导材料参考: 点云配准之-ColorICP中公式详细推导

    赞(0)
    未经允许不得转载:171主机测评 » 点云配准之-Colored Point Cloud Registration(ColorICP)
    分享到: 更多 (0)

    评论 抢沙发

    • 昵称 (必填)
    • 邮箱 (必填)
    • 网址