一、刚性ICP算法
ICP的全称是 iterative closest point —— 迭代最近点。它是一种点云匹配的算法。
不了解刚性ICP配准算法的可以看一下我的这一篇：【ICP算法思想与推导详解】
二、非刚性ICP
在刚性ICP中，目标物体是刚体，那么物体上的每一点在两帧之间的移动都是相同的
所以对于刚性物体，只需要对整个物体估计 1个变换即可.
也就是像 【ICP算法思想与推导详解】 中求
R
\boldsymbol R
R 与
t
\boldsymbol t
t：
R ∈ R 3 × 3 , t ∈ R 3 × 1 \boldsymbol R\in \mathbb R^{3\times 3}, \boldsymbol t\in \mathbb R^{3\times 1} R∈R3×3,t∈R3×1
或者是像 KinectFusion 论文中的公式(20)那样估计一个 6维向量
x
\boldsymbol x
x：
x
=
(
α
,
β
,
γ
,
t
x
,
t
y
,
t
z
)
T
=
(
ϕ
T
,
t
T
)
T
\boldsymbol x = (\alpha, \beta, \gamma, t_x, t_y, t_z)^\mathrm T = (\boldsymbol \phi^\mathrm T, \boldsymbol t^\mathrm T)^\mathrm T
x=(α,β,γ,tx,ty,tz)T=(ϕT,tT)T
而在非刚性ICP中，目标物体是非刚体，物体上每个点在两帧之前的移动就都不同了：
【图】
此时要估计的变量就不止一个 6维向量 了，而是
n
n
n 个 6维向量，
n
n
n 是这个非刚物体表面的节点个数。
我们可以将他们放到同一个向量中：
x = ( α 1 , β 1 , γ 1 , t 1 x , t 1 y , t 1 z , … , α n , β n , γ n , t n x , t n y , t n z ) T = ( ϕ 1 T , t 1 T , ϕ 2 T , t 2 T , … , ϕ n T , t n T ) T ∈ R 6 n × 1 \begin{aligned} \boldsymbol x &= (\alpha_1, \beta_1, \gamma_1, t_{1_x}, t_{1_y}, t_{1_z}, \dots, \alpha_n, \beta_n, \gamma_n, t_{n_x}, t_{n_y}, t_{n_z})^\mathrm T \\ &=(\boldsymbol \phi_1^\mathrm T, \boldsymbol t_1^\mathrm T, \boldsymbol \phi_2^\mathrm T, \boldsymbol t_2^\mathrm T, \dots, \boldsymbol \phi_n^\mathrm T, \boldsymbol t_n^\mathrm T)^\mathrm T \in \mathbb R^{6n\times 1} \end{aligned} x=(α1,β1,γ1,t1x,t1y,t1z,…,αn,βn,γn,tnx,tny,tnz)T=(ϕ1T,t1T,ϕ2T,t2T,…,ϕnT,tnT)T∈R6n×1
三、目标函数
和刚性ICP一样，目标函数仍然可以是 Point-to-Point, Point-to-Plane 等ICP配准函数：
f
=
∥
v
^
−
T
^
v
∥
2
o
r
f
=
(
v
^
−
T
^
v
)
T
n
^
f = \Vert \hat\boldsymbol v - \hat\boldsymbol T\boldsymbol v \Vert^2 \\ or \\ f = (\hat\boldsymbol v - \hat\boldsymbol T\boldsymbol v)^\mathrm T \hat\boldsymbol n
f=∥v^−T^v∥2orf=(v^−T^v)Tn^
DynamicFusion 中对空间定义了 3 种帧：
- canonical frame 标准空间 —— 指 非刚物体固定不动(未发生形变)时的状态（一般使用初始状态来作为标准空间）
- warp frame 曲变场帧 —— 指 将非刚物体由标准空间通过我们估计的曲变函数变化得到的形变形式下的三维状态（预测值）
- live frame 实时帧 —— 指 深度相机实时拍摄到的三维状态（观测值）
下面看 DynamicFusion 的 非刚ICP 公式(7) ：
D a t a ( W , V , D t ) = ∑ u ∈ Ω ψ d a t a ( n ^ u T ( v ^ u − v l u ~ ) ) \mathrm{Data}( \mathcal{W},\mathcal{V},\mathcal{D}_t) = \sum_{u\in\Omega} \psi_{\mathrm{data}}\big(\hat{\boldsymbol n}_u^{\mathrm T}(\hat{\boldsymbol v}_u-\boldsymbol {vl}_{\widetilde{u}})\big) Data(W,V,Dt)=u∈Ω∑ψdata(n^uT(v^u−vlu ))
先说一下这里字母的含义：
Ω
~~~\Omega
Ω 表示非刚物体上所有 Node 全集，以下假设它含有
N
N
N 个 Node
u
~~~u
u 表示非刚物体上的某一个 Node 节点
v
^
u
~~~\hat{\boldsymbol v}_u
v^u 表示 从canonical空间warp之后的 三维空间坐标
n
^
u
~~~\hat{\boldsymbol n}_u
n^u 表示 从canonical空间warp之后的 三维空间法向量
u
~~~u
u 表示非刚物体上的某一个 Node 节点
v
l
u
~
~~~\boldsymbol {vl}_{\widetilde{u}}
vlu
表示
v
^
u
\hat{\boldsymbol v}_u
v^u 投影到成像平面后再从深度图中计算得到的 三维空间坐标(live frame)
ψ
d
a
t
a
(
⋅
)
~~~\psi_{\mathrm{data}}(\cdot)
ψdata(⋅) 是一个 Tukey penalty fucntion ，鲁棒统计学领域的损失函数。
我们先去掉
ψ
d
a
t
a
\psi_{\mathrm{data}}
ψdata 和
∑
\sum
∑ 求和符号，看看里面的东西：
n
^
u
T
(
v
^
u
−
v
l
u
~
)
(I)
\tag {I} \hat{\boldsymbol n}_u^{\mathrm T}(\hat{\boldsymbol v}_u-\boldsymbol {vl}_{\widetilde{u}})
n^uT(v^u−vlu
)(I)
很容易看出，DynamicFusion使用了 Point-to-Plane 形式的 ICP
它做的是 warp frame 与 live frame 之间的非刚配准。
四、求解
现在我们先对上一节里说到的函数 ( I ) (\mathrm I) (I) 进行变换
只对其中一个节点
u
u
u 进行分析
（因为只讨论一个点，我把下标
u
u
u 去掉了，然后我把后面那个
v
l
\boldsymbol {vl}
vl 写成
l
\boldsymbol l
l 了，因为俩字母叠一起怪怪的总会看成乘法…），
记：
f
=
n
^
T
(
v
^
−
l
)
f = \hat{\boldsymbol n}^{\mathrm T}(\hat{\boldsymbol v}-\boldsymbol l)
f=n^T(v^−l)
将其表示为 canonical空间
(
v
,
n
)
(\boldsymbol v, \boldsymbol n)
(v,n) 和 warp场
T
^
\hat\boldsymbol T
T^ 的形式：
v
^
=
T
^
v
n
^
=
T
^
n
T
^
=
W
t
(
x
c
)
}
⟹
f
=
n
T
T
^
T
T
^
v
−
n
T
T
^
T
l
\left. \begin{aligned} \hat{\boldsymbol v} = \hat\boldsymbol T\boldsymbol v \\ \hat{\boldsymbol n} = \hat\boldsymbol T\boldsymbol n \\ \hat\boldsymbol T = \mathcal W_t(\boldsymbol x_c) \end{aligned} \right\} ~~\Longrightarrow~~~ f=\boldsymbol n^{\mathrm T}\hat\boldsymbol T^{\mathrm T}\hat\boldsymbol T\boldsymbol v - \boldsymbol n^{\mathrm T}\hat\boldsymbol T^{\mathrm T}\boldsymbol l
v^=T^vn^=T^nT^=Wt(xc)⎭⎪⎪⎬⎪⎪⎫ ⟹ f=nTT^TT^v−nTT^Tl
我们需要优化 节点
u
u
u 的 warp场 变换，即优化
T
^
\hat\boldsymbol T
T^：
T
^
=
arg min
T
^
f
\hat\boldsymbol T = \argmin_{\hat\boldsymbol T} f
T^=T^argminf
根据高斯-牛顿法，我们的目标就是求雅克比矩阵：
J
=
∂
D
a
t
a
∂
f
∂
f
∂
x
=
∂
D
a
t
a
∂
f
(
∂
f
∂
ϕ
1
,
∂
f
∂
t
1
,
…
,
∂
f
∂
ϕ
2
,
∂
f
∂
ϕ
n
)
=
∂
D
a
t
a
∂
f
(
∂
f
∂
α
1
,
∂
f
∂
β
1
,
∂
f
∂
γ
1
,
∂
f
∂
t
1
x
,
∂
f
∂
t
1
y
,
∂
f
∂
t
1
z
,
…
)
\begin{aligned} \boldsymbol J &= \frac{\partial \mathrm{Data}}{\partial f}\frac{\partial f}{\partial \boldsymbol x} = \frac{\partial \mathrm{Data}}{\partial f}\Big(\frac{\partial f}{\partial \boldsymbol \phi_1}, \frac{\partial f}{\partial \boldsymbol t_1},\dots, \frac{\partial f}{\partial \boldsymbol \phi_2}, \frac{\partial f}{\partial \boldsymbol \phi_n}\Big)\\ &= \frac{\partial \mathrm{Data}}{\partial f}\Big(\frac{\partial f}{\partial \alpha_1},\frac{\partial f}{\partial \beta_1},\frac{\partial f}{\partial \gamma_1},\frac{\partial f}{\partial t_{1_x}},\frac{\partial f}{\partial t_{1_y}},\frac{\partial f}{\partial t_{1_z}},\dots\Big) \end{aligned}
J=∂f∂Data∂x∂f=∂f∂Data(∂ϕ1∂f,∂t1∂f,…,∂ϕ2∂f,∂ϕn∂f)=∂f∂Data(∂α1∂f,∂β1∂f,∂γ1∂f,∂t1x∂f,∂t1y∂f,∂t1z∂f,…)
现在就是要表示出
∂
f
∂
x
k
\displaystyle\frac{\partial f}{\partial \boldsymbol x_k}
∂xk∂f 所对应的 6 个元素，就能把整个 Jacobian 矩阵表示出来：
∂
f
∂
α
k
,
∂
f
∂
β
k
,
∂
f
∂
γ
k
,
∂
f
∂
t
k
x
,
∂
f
∂
t
k
y
,
∂
f
∂
t
k
z
\frac{\partial f}{\partial \alpha_k},\frac{\partial f}{\partial \beta_k},\frac{\partial f}{\partial \gamma_k},\frac{\partial f}{\partial t_{k_x}},\frac{\partial f}{\partial t_{k_y}},\frac{\partial f}{\partial t_{k_z}}
∂αk∂f,∂βk∂f,∂γk∂f,∂tkx∂f,∂tky∂f,∂tkz∂f
下面介绍第1项 ∂ f ∂ α k \displaystyle\frac{\partial f}{\partial \alpha_k} ∂αk∂f的求法，后面 5 项求法类似
记
T
^
=
[
t
^
i
j
]
\hat\boldsymbol T = [ \hat t_{ij} ]
T^=[t^ij]
根据矩阵求导链式法则：
∂
f
∂
α
k
=
∑
i
j
∂
f
∂
t
^
i
j
∂
t
^
i
j
∂
α
k
=
[
v
e
c
(
∂
f
∂
T
^
)
]
T
⋅
v
e
c
(
∂
T
^
∂
α
k
)
=
t
r
(
(
∂
f
∂
T
^
)
T
⋅
∂
T
^
∂
α
k
)
(II)
\tag {II} \frac{\partial f}{\partial \alpha_k} = \sum_{ij} \frac{\partial f}{\partial \hat t_{ij}} \frac{\partial \hat t_{ij}}{\partial \alpha_k}=[\bold{vec}(\frac{\partial f}{\partial \hat\boldsymbol T}) ]^\mathrm T \cdot \bold{vec}(\frac{\partial \hat\boldsymbol T}{\partial \alpha_k}) = tr\Big(( \textcolor{#FF0000}{\frac{\partial f}{\partial \hat\boldsymbol T}})^\mathrm T\cdot \textcolor{#0000FF}{\frac{\partial \hat\boldsymbol T}{\partial \alpha_k}} \Big)
∂αk∂f=ij∑∂t^ij∂f∂αk∂t^ij=[vec(∂T^∂f)]T⋅vec(∂αk∂T^)=tr((∂T^∂f)T⋅∂αk∂T^)(II)
其中
∂
f
∂
T
^
=
T
^
(
n
v
T
+
v
n
T
)
−
l
n
T
\textcolor{#FF0000}{\frac{\partial f}{\partial \hat\boldsymbol T}} =\hat\boldsymbol T(\boldsymbol n\boldsymbol v^\mathrm T+ \boldsymbol v \boldsymbol n^\mathrm T) - \boldsymbol l\boldsymbol n^\mathrm T
∂T^∂f=T^(nvT+vnT)−lnT
比较麻烦一点的是 ∂ T ^ ∂ α k \displaystyle \textcolor{#0000FF}{\frac{\partial \hat\boldsymbol T}{\partial \alpha_k}} ∂αk∂T^
DynamicFusion中，warp场的插值使用的是对偶四元数混合线插：
T
^
=
W
t
(
x
c
)
=
T
l
w
S
E
3
(
D
Q
B
(
x
c
)
)
=
T
l
w
S
E
3
(
q
^
)
\begin{aligned} \hat\boldsymbol T &= \mathcal W_t(\boldsymbol x_c) \\ &= \boldsymbol T_{lw}SE3\big(\bold{DQB}(\boldsymbol x_c)\big) \\ &= \boldsymbol T_{lw}SE3\big( \hat\boldsymbol q\big) \end{aligned}
T^=Wt(xc)=TlwSE3(DQB(xc))=TlwSE3(q^)
对偶四元数我写过两篇，不了解的可以瞅一眼：
【四元数，对偶四元数】
【对偶四元数线性混合插值】
而我们知道，对偶四元数可以表示为一个 8自由度的向量： q ^ = ( s r , x r , y r , z r , s ε , x ε , y ε , z ε ) \hat\boldsymbol q=(s_r, x_r, y_r, z_r, s_\varepsilon, x_\varepsilon, y_\varepsilon, z_\varepsilon) q^=(sr,xr,yr,zr,sε,xε,yε,zε)
那么根据链式法则：
∂
T
^
∂
α
k
=
T
l
w
(
∂
S
E
3
(
q
^
)
∂
s
r
∂
s
r
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
x
r
∂
x
r
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
y
r
∂
y
r
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
z
r
∂
z
r
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
s
ε
∂
s
ε
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
x
ε
∂
x
ε
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
y
ε
∂
y
ε
∂
α
k
+
∂
S
E
3
(
q
^
)
∂
z
ε
∂
z
ε
∂
α
k
)
(III)
\tag{III} \begin{aligned} \textcolor{#0000FF}{\frac{\partial \hat\boldsymbol T}{\partial \alpha_k}} =\boldsymbol T_{lw}\Big(& \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial s_r}} \textcolor{#22BB00}{\frac{\partial s_r}{\partial \alpha _k}} +\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial x_r}} \textcolor{#22BB00}{\frac{\partial x_r}{\partial \alpha _k}} +\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial y_r}} \textcolor{#22BB00}{\frac{\partial y_r}{\partial \alpha _k}} +\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial z_r}} \textcolor{#22BB00}{\frac{\partial z_r}{\partial \alpha _k}} \\ +&\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial s_\varepsilon}} \textcolor{#22BB00}{\frac{\partial s_\varepsilon}{\partial \alpha _k}} +\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial x_\varepsilon}} \textcolor{#22BB00}{\frac{\partial x_\varepsilon}{\partial \alpha _k}} +\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial y_\varepsilon}} \textcolor{#22BB00}{\frac{\partial y_\varepsilon}{\partial \alpha _k}} +\textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial z_\varepsilon}} \textcolor{#22BB00}{\frac{\partial z_\varepsilon}{\partial \alpha _k}} \Big) \end{aligned}
∂αk∂T^=Tlw(+∂sr∂SE3(q^)∂αk∂sr+∂xr∂SE3(q^)∂αk∂xr+∂yr∂SE3(q^)∂αk∂yr+∂zr∂SE3(q^)∂αk∂zr∂sε∂SE3(q^)∂αk∂sε+∂xε∂SE3(q^)∂αk∂xε+∂yε∂SE3(q^)∂αk∂yε+∂zε∂SE3(q^)∂αk∂zε)(III)
(
I
I
I
)
\mathrm{(III)}
(III) 式结果为 8个链式法则元素 的和，每个元素的 前一半 可以由 对偶四元数 与 变换矩阵的关系得出：
S
E
3
(
q
^
)
=
(
1
−
2
(
y
r
2
+
z
r
2
)
2
(
x
r
y
r
−
z
r
s
r
)
2
(
x
r
z
r
+
y
r
s
r
)
2
(
−
s
ε
x
r
+
x
ε
s
r
−
y
ε
z
r
+
z
ε
y
r
)
2
(
x
r
y
r
+
s
r
z
r
)
1
−
2
(
x
r
2
+
z
r
2
)
2
(
y
r
z
r
−
x
r
s
r
)
2
(
−
s
ε
y
r
+
x
ε
z
r
+
y
ε
s
r
−
z
ε
x
r
)
2
(
x
r
z
r
+
y
r
s
r
)
2
(
y
r
z
r
+
x
r
s
r
)
1
−
2
(
x
r
2
+
y
r
2
)
2
(
−
s
ε
z
r
−
x
ε
y
r
+
y
ε
x
r
+
z
ε
s
r
)
0
0
0
1
)
SE3(\hat\boldsymbol q)= \begin{pmatrix} 1-2(y_r^2+z_r^2) & 2(x_ry_r-z_rs_r) & 2(x_rz_r+y_rs_r) & 2(-s_\varepsilon x_r+x_\varepsilon s_r-y_\varepsilon z_r+z_\varepsilon y_r) \\ 2(x_ry_r+s_rz_r) & 1-2(x_r^2+z_r^2) & 2(y_rz_r-x_rs_r) & 2(-s_\varepsilon y_r+x_\varepsilon z_r+y_\varepsilon s_r-z_\varepsilon x_r) \\ 2(x_rz_r+y_rs_r) & 2(y_rz_r+x_rs_r) & 1-2(x_r^2+y_r^2) & 2(-s_\varepsilon z_r-x_\varepsilon y_r+y_\varepsilon x_r+z_\varepsilon s_r) \\ 0 & 0 & 0 & 1 \end{pmatrix}
SE3(q^)=⎝⎜⎜⎛1−2(yr2+zr2)2(xryr+srzr)2(xrzr+yrsr)02(xryr−zrsr)1−2(xr2+zr2)2(yrzr+xrsr)02(xrzr+yrsr)2(yrzr−xrsr)1−2(xr2+yr2)02(−sεxr+xεsr−yεzr+zεyr)2(−sεyr+xεzr+yεsr−zεxr)2(−sεzr−xεyr+yεxr+zεsr)1⎠⎟⎟⎞
∂ S E 3 ( q ^ ) ∂ s r = 2 ( 0 − z r y r x ε z r 0 − x r y ε − y r x r 0 z ε 0 0 0 0 ) , ∂ S E 3 ( q ^ ) ∂ x r = 2 ( 0 y r z r − s ε y r − 2 x r − s r − z ε z r s r − 2 x r y ε 0 0 0 0 ) ∂ S E 3 ( q ^ ) ∂ y r = 2 ( − 2 y r x r s r z ε x r 0 z r − s ε − s r z r − 2 y r − x ε 0 0 0 0 ) , ∂ S E 3 ( q ^ ) ∂ z r = 2 ( − 2 z r − s r x r − y ε s r − 2 z r y r x ε x r y r 0 − s ε 0 0 0 0 ) ∂ S E 3 ( q ^ ) ∂ s ε = 2 ( 0 − x ε − y ε − z ε 0 ) , ∂ S E 3 ( q ^ ) ∂ x ε = 2 ( 0 s ε z ε − y ε 0 ) ∂ S E 3 ( q ^ ) ∂ y ε = 2 ( 0 − z ε s ε x ε 0 ) , ∂ S E 3 ( q ^ ) ∂ z ε = 2 ( 0 y ε − x ε s ε 0 ) \begin{aligned} \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial s_r}} &= 2 \begin{pmatrix} 0 & -z_r & y_r & x_\varepsilon \\ z_r & 0 & -x_r & y_\varepsilon \\ -y_r & x_r & 0 & z_\varepsilon \\ 0 & 0 & 0 & 0 \end{pmatrix}, & \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial x_r}} &= 2 \begin{pmatrix} 0 & y_r & z_r & -s_\varepsilon \\ y_r & -2x_r & -s_r & -z_\varepsilon \\ z_r & s_r & -2x_r & y_\varepsilon \\ 0 & 0 & 0 & 0 \end{pmatrix} \\ \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial y_r}} &= 2 \begin{pmatrix} -2y_r & x_r & s_r & z_\varepsilon \\ x_r & 0 & z_r & -s_\varepsilon \\ -s_r & z_r & -2y_r & -x_\varepsilon \\ 0 & 0 & 0 & 0 \end{pmatrix}, & \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial z_r}} &= 2 \begin{pmatrix} -2z_r & -s_r & x_r & -y_\varepsilon \\ s_r & -2z_r & y_r & x_\varepsilon \\ x_r & y_r & 0 & -s_\varepsilon \\ 0 & 0 & 0 & 0 \end{pmatrix} \\ \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial s_\varepsilon}} &= 2 \begin{pmatrix} & & \begin{matrix} \boldsymbol 0 \end{matrix} & & & \begin{matrix} -x_\varepsilon \\ -y_\varepsilon \\ -z_\varepsilon \\ 0 \end{matrix} \end{pmatrix}, & \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial x_\varepsilon}} &= 2 \begin{pmatrix} & & \begin{matrix} \boldsymbol 0 \end{matrix} & & & \begin{matrix} s_\varepsilon \\ z_\varepsilon \\ -y_\varepsilon \\ 0 \end{matrix} \end{pmatrix} \\ \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial y_\varepsilon}} &= 2 \begin{pmatrix} & & \begin{matrix} \boldsymbol 0 \end{matrix} & & & \begin{matrix} -z_\varepsilon \\ s_\varepsilon \\ x_\varepsilon \\ 0 \end{matrix} \end{pmatrix}, & \textcolor{#DD8800}{\frac{\partial SE3(\hat\boldsymbol q)}{\partial z_\varepsilon}} &= 2 \begin{pmatrix} & & \begin{matrix} \boldsymbol 0 \end{matrix} & & & \begin{matrix} y_\varepsilon \\ -x_\varepsilon \\ s_\varepsilon \\ 0 \end{matrix} \end{pmatrix} \\ \end{aligned} ∂sr∂SE3(q^)∂yr∂SE3(q^)∂sε∂SE3(q^)∂yε∂SE3(q^)=2⎝⎜⎜⎛0zr−yr0−zr0xr0yr−xr00xεyεzε0⎠⎟⎟⎞,=2⎝⎜⎜⎛−2yrxr−sr0xr0zr0srzr−2yr0zε−sε−xε0⎠⎟⎟⎞,=2⎝⎜⎜⎛0−xε−yε−zε0⎠⎟⎟⎞,=2⎝⎜⎜⎛0−zεsεxε0⎠⎟⎟⎞,∂xr∂SE3(q^)∂zr∂SE3(q^)∂xε∂SE3(q^)∂zε∂SE3(q^)=2⎝⎜⎜⎛0yrzr0yr−2xrsr0zr−sr−2xr0−sε−zεyε0⎠⎟⎟⎞=2⎝⎜⎜⎛−2zrsrxr0−sr−2zryr0xryr00−yεxε−sε0⎠⎟⎟⎞=2⎝⎜⎜⎛0sεzε−yε0⎠⎟⎟⎞=2⎝⎜⎜⎛0yε−xεsε0⎠⎟⎟⎞
(
I
I
I
)
\mathrm{(III)}
(III) 式结果每个元素的 后一半 则又需要使用链式法则，
∂
s
r
∂
α
k
=
∂
s
r
∂
q
^
k
∂
q
^
k
∂
α
k
=
∂
s
r
∂
s
k
r
∂
s
k
r
∂
α
k
+
∂
s
r
∂
x
k
r
∂
x
k
r
∂
α
k
+
∂
s
r
∂
y
k
r
∂
y
k
r
∂
α
k
+
∂
s
r
∂
z
k
r
∂
z
k
r
∂
α
k
+
∂
s
r
∂
s
k
ε
∂
s
k
ε
∂
α
k
+
∂
s
r
∂
x
k
ε
∂
x
k
ε
∂
α
k
+
∂
s
r
∂
y
k
ε
∂
y
k
ε
∂
α
k
+
∂
s
r
∂
z
k
ε
∂
z
k
ε
∂
α
k
(IV)
\tag{IV} \begin{aligned} \textcolor{#22BB00}{\frac{\partial s_r}{\partial \alpha _k}} &= \frac{\partial s_r}{\partial \hat{\boldsymbol q}_k}\frac{\partial \hat{\boldsymbol q}_k}{\partial \alpha_k} \\ &= \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {s_k}_r}}\textcolor{#0088AA}{\frac{\partial {s_k}_r}{\partial \alpha_k}} + \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {x_k}_r}}\textcolor{#0088AA}{\frac{\partial {x_k}_r}{\partial \alpha_k}} + \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {y_k}_r}}\textcolor{#0088AA}{\frac{\partial {y_k}_r}{\partial \alpha_k}} + \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {z_k}_r}}\textcolor{#0088AA}{\frac{\partial {z_k}_r}{\partial \alpha_k}} \\ &+\textcolor{#CC00CC}{\frac{\partial s_r}{\partial {s_k}_\varepsilon}}\textcolor{#0088AA}{\frac{\partial {s_k}_\varepsilon}{\partial \alpha_k}} + \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {x_k}_\varepsilon}}\textcolor{#0088AA}{\frac{\partial {x_k}_\varepsilon}{\partial \alpha_k}} + \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {y_k}_\varepsilon}}\textcolor{#0088AA}{\frac{\partial {y_k}_\varepsilon}{\partial \alpha_k}} + \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {z_k}_\varepsilon}}\textcolor{#0088AA}{\frac{\partial {z_k}_\varepsilon}{\partial \alpha_k}} \end{aligned}
∂αk∂sr=∂q^k∂sr∂αk∂q^k=∂skr∂sr∂αk∂skr+∂xkr∂sr∂αk∂xkr+∂ykr∂sr∂αk∂ykr+∂zkr∂sr∂αk∂zkr+∂skε∂sr∂αk∂skε+∂xkε∂sr∂αk∂xkε+∂ykε∂sr∂αk∂ykε+∂zkε∂sr∂αk∂zkε(IV)
也是链式法则把整个
q
^
k
\hat{\boldsymbol q}_k
q^k 的8个元素都链了一遍。
(
I
V
)
\mathrm{(IV)}
(IV) 式结果8个链式求导的每个元素 前一半 是该节点的对偶四元数 与 别的节点的对偶四元数的偏导关系，因为：
q
^
=
∑
i
∈
K
(
x
c
)
w
i
q
^
i
∥
∑
i
∈
K
(
x
c
)
w
i
q
^
i
∥
\hat\boldsymbol q = \frac{\displaystyle\sum_{i\in K(\boldsymbol x_c)}w_i\hat{\boldsymbol q}_i}{\bigg\Vert \displaystyle\sum_{i\in K(\boldsymbol x_c)}w_i\hat{\boldsymbol q}_i \bigg\Vert}
q^=∥∥∥∥i∈K(xc)∑wiq^i∥∥∥∥i∈K(xc)∑wiq^i
其中 K ( x c ) K(\boldsymbol x_c) K(xc) 表示 节点 u u u 在 canonical 空间中 k-nearest 节点编号的合集。
记
∑
i
∈
K
(
x
c
)
w
i
q
^
i
=
q
~
\displaystyle\sum_{i\in K(\boldsymbol x_c)}w_i\hat{\boldsymbol q}_i= \widetilde {\boldsymbol q}
i∈K(xc)∑wiq^i=q
， 则
q
^
=
q
~
∥
q
~
∥
\hat\boldsymbol q = \frac{\displaystyle \widetilde {\boldsymbol q}}{\displaystyle \Vert \widetilde {\boldsymbol q} \Vert}
q^=∥q
∥q
(
q
~
=
(
s
~
r
,
x
~
r
,
y
~
r
,
z
~
r
,
s
~
ε
,
x
~
ε
,
y
~
ε
,
z
~
ε
)
,
∥
q
~
∥
=
s
~
r
2
+
x
~
r
2
+
y
~
r
2
+
z
~
r
2
\widetilde {\boldsymbol q} = (\widetilde s_r, \widetilde x_r, \widetilde y_r, \widetilde z_r, \widetilde s_\varepsilon, \widetilde x_\varepsilon, \widetilde y_\varepsilon, \widetilde z_\varepsilon), ~~\Vert \widetilde {\boldsymbol q} \Vert = \sqrt{\displaystyle \widetilde s_r^2 + \widetilde x_r^2 + \widetilde y_r^2 + \widetilde z_r^2}
q
=(s
r,x
r,y
r,z
r,s
ε,x
ε,y
ε,z
ε), ∥q
∥=s
r2+x
r2+y
r2+z
r2 )
那么：
s
r
=
s
~
r
∥
q
~
∥
=
∑
i
∈
K
(
x
c
)
w
i
s
i
r
(
∑
i
∈
K
(
x
c
)
w
i
s
i
r
)
2
+
x
~
r
2
+
y
~
r
2
+
z
~
r
2
=
w
k
s
k
r
+
∑
i
∈
K
(
x
c
)
,
i
≠
k
w
i
s
i
r
(
w
k
s
k
r
+
∑
i
∈
K
(
x
c
)
,
i
≠
k
w
i
s
i
r
)
2
+
x
~
r
2
+
y
~
r
2
+
z
~
r
2
\begin{aligned} \textcolor{#CC00CC}{s_r} &= \frac{\widetilde s_r}{\Vert \widetilde {\boldsymbol q} \Vert} = \frac{\displaystyle\sum_{i\in K(\boldsymbol x_c)}w_i {s_i}_r}{\sqrt{\displaystyle \Big(\displaystyle\sum_{i\in K(\boldsymbol x_c)}w_i {s_i}_r\Big)^2 + \widetilde x_r^2 + \widetilde y_r^2 + \widetilde z_r^2}} \\ &= \frac{w_k\textcolor{#CC00CC}{{s_k}_r} +\displaystyle\sum_{i\in K(\boldsymbol x_c), i\neq k}w_i {s_i}_r}{\sqrt{\displaystyle \Big(w_k\textcolor{#CC00CC}{{s_k}_r} +\displaystyle\sum_{i\in K(\boldsymbol x_c), i\neq k}w_i {s_i}_r\Big)^2 + \widetilde x_r^2 + \widetilde y_r^2 + \widetilde z_r^2}} \end{aligned}
sr=∥q
∥s
r=(i∈K(xc)∑wisir)2+x
r2+y
r2+z
r2i∈K(xc)∑wisir=(wkskr+i∈K(xc),i=k∑wisir)2+x
r2+y
r2+z
r2wkskr+i∈K(xc),i=k∑wisir
所以
∂ s r ∂ s k r = w k ∥ q ~ ∥ − s ~ r w k s ~ r ∥ q ~ ∥ ∥ q ~ ∥ 2 = w k ∥ q ~ ∥ − w k s ~ r 2 ∥ q ~ ∥ 3 \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {s_k}_r}} = \frac{w_k\Vert \widetilde {\boldsymbol q} \Vert - \widetilde s_r\displaystyle\frac{w_k\widetilde s_r}{\Vert \widetilde {\boldsymbol q} \Vert}}{\Vert \widetilde {\boldsymbol q} \Vert ^2} = \frac{w_k}{\Vert \widetilde {\boldsymbol q} \Vert} - \frac{w_k\widetilde s_r^2}{\Vert \widetilde {\boldsymbol q} \Vert^3} ∂skr∂sr=∥q ∥2wk∥q ∥−s r∥q ∥wks r=∥q ∥wk−∥q ∥3wks r2
同法算出其他几项：
∂
s
r
∂
x
k
r
=
−
w
k
s
~
r
x
~
r
∥
q
~
∥
3
,
∂
s
r
∂
y
k
r
=
−
w
k
s
~
r
y
~
r
∥
q
~
∥
3
,
∂
s
r
∂
z
k
r
=
−
w
k
s
~
r
z
~
r
∥
q
~
∥
3
,
∂
s
r
∂
s
k
ε
=
∂
s
r
∂
x
k
ε
=
∂
s
r
∂
y
k
ε
=
∂
s
r
∂
z
k
ε
=
0
\begin{aligned} \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {x_k}_r}} &= - \frac{w_k\widetilde s_r\widetilde x_r}{\Vert \widetilde {\boldsymbol q} \Vert^3}, ~~\textcolor{#CC00CC}{\frac{\partial s_r}{\partial {y_k}_r}} = - \frac{w_k\widetilde s_r\widetilde y_r}{\Vert \widetilde {\boldsymbol q} \Vert^3}, ~~\textcolor{#CC00CC}{\frac{\partial s_r}{\partial {z_k}_r}} = - \frac{w_k\widetilde s_r\widetilde z_r}{\Vert \widetilde {\boldsymbol q} \Vert^3}, \\ \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {s_k}_\varepsilon}} &= \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {x_k}_\varepsilon}}= \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {y_k}_\varepsilon}}= \textcolor{#CC00CC}{\frac{\partial s_r}{\partial {z_k}_\varepsilon}} = 0 \end{aligned}
∂xkr∂sr∂skε∂sr=−∥q
∥3wks
rx
r, ∂ykr∂sr=−∥q
∥3wks
ry
r, ∂zkr∂sr=−∥q
∥3wks
rz
r,=∂xkε∂sr=∂ykε∂sr=∂zkε∂sr=0
(
I
V
)
\mathrm{(IV)}
(IV) 式结果每个元素的 后一半 可以由 对偶四元数 与 变换矩阵的关系 ：
{
s
r
=
cos
∥
ϕ
∥
2
x
r
=
α
∥
ϕ
∥
sin
∥
ϕ
∥
2
y
r
=
β
∥
ϕ
∥
sin
∥
ϕ
∥
2
z
r
=
γ
∥
ϕ
∥
sin
∥
ϕ
∥
2
s
ε
=
−
1
2
(
t
x
x
r
+
t
y
y
r
+
t
z
z
r
)
x
ε
=
1
2
(
t
x
x
r
+
t
y
z
r
−
t
z
z
r
)
y
ε
=
1
2
(
−
t
x
z
r
+
t
y
s
r
+
t
z
x
r
)
z
ε
=
1
2
(
t
x
y
r
−
t
y
x
r
+
t
z
s
r
)
\left\{ \begin{aligned} s_r &= \cos\frac{\Vert \boldsymbol \phi \Vert}{2} & x_r &= \frac{\alpha}{\Vert \boldsymbol \phi \Vert} \sin\frac{\Vert \boldsymbol \phi \Vert}{2} \\ y_r &= \frac{\beta}{\Vert \boldsymbol \phi \Vert} \sin\frac{\Vert \boldsymbol \phi \Vert}{2} & z_r &= \frac{\gamma}{\Vert \boldsymbol \phi \Vert} \sin\frac{\Vert \boldsymbol \phi \Vert}{2} \\ s_\varepsilon &= -\frac{1}{2}(t_xx_r+t_yy_r+t_zz_r) & x_\varepsilon &= \frac{1}{2}(t_xx_r+t_yz_r-t_zz_r) \\ y_\varepsilon &= \frac{1}{2}(-t_xz_r+t_ys_r+t_zx_r) &z_\varepsilon &= \frac{1}{2}(t_xy_r-t_yx_r+t_zs_r) \end{aligned} \right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧sryrsεyε=cos2∥ϕ∥=∥ϕ∥βsin2∥ϕ∥=−21(txxr+tyyr+tzzr)=21(−txzr+tysr+tzxr)xrzrxεzε=∥ϕ∥αsin2∥ϕ∥=∥ϕ∥γsin2∥ϕ∥=21(txxr+tyzr−tzzr)=21(txyr−tyxr+tzsr)
则：
∂
s
k
r
∂
α
k
=
−
α
k
2
∥
ϕ
k
∥
sin
∥
ϕ
k
∥
2
,
\begin{aligned} \textcolor{#0088AA}{\frac{\partial {s_k}_r}{\partial \alpha _k}} &= -\frac{\alpha_k}{2\Vert \boldsymbol \phi_k \Vert}\sin \frac{\Vert \boldsymbol \phi_k \Vert}{2}, \end{aligned}
∂αk∂skr=−2∥ϕk∥αksin2∥ϕk∥,
后面7项不想写了，然后
(
I
V
)
\mathrm{(IV)}
(IV) 式中的
∂
s
r
∂
α
k
\displaystyle\textcolor{#22BB00}{\frac{\partial s_r}{\partial \alpha _k}}
∂αk∂sr 就给求出来了，
(
I
I
I
)
\mathrm{(III)}
(III) 式中后7项 我也不写了，然后就求出了
(
I
I
)
\mathrm{(II)}
(II) 式中
∂
T
^
∂
α
k
\displaystyle \textcolor{#0000FF}{\frac{\partial \hat\boldsymbol T}{\partial \alpha_k}}
∂αk∂T^，
(
I
I
)
\mathrm{(II)}
(II) 式中后5项 我也不写了，最终求出
∂
f
∂
x
k
\displaystyle\frac{\partial f}{\partial \boldsymbol x_k}
∂xk∂f，这里的
k
∈
K
(
x
c
)
k\in K(\boldsymbol x_c)
k∈K(xc)
也就是说明，对于节点
u
u
u，只有他的 k-临近 点的偏导才有值，其他的
k
∉
K
(
x
c
)
k\notin K(\boldsymbol x_c)
k∈/K(xc) 的偏导全为 0：
J
u
=
∂
f
∂
x
=
(
0
,
…
,
0
,
∂
f
∂
x
k
1
,
0
,
…
,
0
,
∂
f
∂
x
k
2
,
0
,
…
,
0
,
∂
f
∂
x
k
K
,
0
,
…
,
0
)
T
\boldsymbol J_u = \frac{\partial f}{\partial \boldsymbol x}=\Big(\boldsymbol 0,\dots,\boldsymbol 0,\frac{\partial f}{\partial \boldsymbol x_{k_1}},\boldsymbol 0,\dots,\boldsymbol 0,\frac{\partial f}{\partial \boldsymbol x_{k_2}},\boldsymbol 0,\dots,\boldsymbol 0,\frac{\partial f}{\partial \boldsymbol x_{k_K}},\boldsymbol 0,\dots,\boldsymbol 0 \Big)^\mathrm T
Ju=∂x∂f=(0,…,0,∂xk1∂f,0,…,0,∂xk2∂f,0,…,0,∂xkK∂f,0,…,0)T
然后再根据 DynamicFusion 公式(7) 对
u
u
u 求个和，
后面再就是高斯牛顿求解的步骤了，这里就不写了。。。
快疯了。。。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
