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})
(q≈Rp+t) - 颜色一致:
(
d
≈
c
)
(\\mathbf{d} \\approx \\mathbf{c})
(d≈c)
关键难点:
- 如何定义颜色残差
- 如何与几何残差统一优化
- 如何保持数值稳定性与收敛性
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(u−cx),d fy(v−cy),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
p∈P 引入一个虚拟正交相机(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
u⋅np=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)+dp⊤u,(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′)∣p′∈Np 做最小二乘拟合来估计。
具体地,设
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)=s−np(s−p)⊤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)=p′∈Np∑(Cp(f(p′)−p)−C(p′))2≈p′∈Np∑(C(p)+dp⊤(f(p′)−p)−C(p′))2,(10)
同时约束
d
p
⊤
n
p
=
0
d_p^\\top n_p = 0
dp⊤np=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)≈(op−p)⊤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′)−(op−s(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)∈K∑rC(p,q)(T)2+σ(p,q)∈K∑rG(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}
Jr⊤Jrξ=−Jr⊤r,(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∂(Cp∘f∘s)=∇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)=np⊤Js(ξ),(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∣Σg−12+∣rc,i∣Σc−12)
这是一个加权最小二乘,但权重来自噪声模型,而非拍脑袋。
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. 参考文献
相关公式推导材料参考: 点云配准之-ColorICP中公式详细推导





