1. 李群与李代数
参考博客:视觉 SLAM 十四讲 - 李群与李代数。
2. 相机运动模糊建模
运动模糊产生的原因是:相机在曝光期间捕捉到了移动的物体或自身发生了移动,导致场景中某些像素在成像过程中不是来自单一点,而是多个位置的光线的混合。
假设在时间 [ t 0 , t 0 + T ] [t_0, t_0 + T] [t0,t0+T],我们拍摄了一组图像序列 { I k } k = 1 n \{I_k\}_{k = 1}^{n} {Ik}k=1n,则模糊图像可以使用下述公式建模:
I blur = 1 T ∫ t 0 t 0 + T I k ( x , y ) d t ≈ ∑ k = 1 n w k ⋅ I k ( x , y ) , ∑ k = 1 n w k = 1 I_{\text{blur}} = \frac{1}{T} \int_{t_0}^{t_0 + T} I_k(x, y) \, dt \approx \sum_{k = 1}^{n} w_k \cdot I_k(x, y), \quad \sum_{k = 1}^n w_k = 1 Iblur=T1∫t0t0+TIk(x,y)dt≈k=1∑nwk⋅Ik(x,y),k=1∑nwk=1真实模糊图很难获取其对应的清晰中间帧。因此,我们通过模拟真实相机在曝光时间内的光线积分过程,对图像序列按时间进行加权平均,得到一张模糊图像,即合成模糊图 = 模拟曝光下图像形成过程。
3. 相机运动轨迹近似
相机在曝光期间可沿任意光滑路径移动。为近似这一运动,论文分别采用线性插值、三次样条插值和K阶贝塞尔曲线进行建模。
3.1. 线性插值
鉴于快门时间通常较短且相机运动的幅度相对较小,线性插值在大多数情况下通常足够适用。假设相机曝光时间为 τ \tau τ,曝光开始时的位姿为 T 0 \boldsymbol{T}_0 T0,曝光结束时的位姿为 T 1 \boldsymbol{T}_1 T1,则时刻 t ∈ [ 0 , τ ] t \in [0, \tau] t∈[0,τ] 的位姿如下:
T t = T 0 ⋅ exp ( t τ log ( T 0 − 1 ⋅ T 1 ) ) \boldsymbol{T}_t = \boldsymbol{T}_0 \cdot \exp(\dfrac{t}{\tau} \log{(\boldsymbol{T}_0^{-1} \cdot \boldsymbol{T}_1)}) Tt=T0⋅exp(τtlog(T0−1⋅T1))上述公式描述了从初始位姿 T 0 \boldsymbol{T}_0 T0 到目标位姿 T 1 \boldsymbol{T}_1 T1 的平滑插值路径。
在 S E ( 3 ) SE(3) SE(3) 群中,直接对位姿矩阵 T \boldsymbol{T} T 进行欧几里得线性插值(例如, T t = ( 1 − t τ ) T 0 + t τ T 1 \boldsymbol{T}_t = (1 - \dfrac{t}{\tau}) \boldsymbol{T}_0 + \dfrac{t}{\tau} \boldsymbol{T}_1 Tt=(1−τt)T0+τtT1)会破坏群结构(例如,旋转矩阵可能不再正交)。为此,实际应用中需要把位姿矩阵转换为李代数元素,进行线性插值后,再通过指数映射回李群,以确保结果始终是有效的位姿矩阵。
首先,计算位姿 T 0 \boldsymbol{T}_0 T0 到 T 1 \boldsymbol{T}_1 T1 的相对位姿为 Δ T = T 0 − 1 T 1 ∈ S E ( 3 ) \Delta \boldsymbol{T} = \boldsymbol{T}_0^{-1} \boldsymbol{T}_1 \in SE(3) ΔT=T0−1T1∈SE(3)。值得注意的是,增量变换 Δ T \Delta \boldsymbol{T} ΔT 在相机处于位姿 T 0 \boldsymbol{T}_0 T0 时的局部坐标系中定义,因此新位姿 T 1 \boldsymbol{T}_1 T1 的计算需使用右乘法则: T 1 = T 0 Δ T \boldsymbol{T}_1 = \boldsymbol{T}_0 \Delta \boldsymbol{T} T1=T0ΔT,参考博客:机器人位姿变换的坐标系相对性:左乘法则与右乘法则解析。
其次,对 Δ T \Delta \boldsymbol{T} ΔT 取对数映射到李代数元素,即有 ξ = log ( Δ T ) ∈ s e ( 3 ) \boldsymbol{\xi} = \log(\Delta \boldsymbol{T}) \in se(3) ξ=log(ΔT)∈se(3)。
然后,在李代数中进行线性插值。对于任意时间 t t t,线性插值为: ξ t = t τ ξ \boldsymbol{\xi}_t = \dfrac{t}{\tau} \boldsymbol{\xi} ξt=τtξ。
最后,将插值结果通过指数映射回李群,即有 exp ( ξ t ) = exp ( t τ log ( T 0 − 1 T 1 ) ) \exp(\boldsymbol{\xi}_t) = \exp(\dfrac{t}{\tau} \log(\boldsymbol{T}_0^{-1} \boldsymbol{T}_1)) exp(ξt)=exp(τtlog(T0−1T1)),最终的时刻 t t t 的位姿矩阵 T t \boldsymbol{T}_t Tt 为:
T t = T 0 ⋅ exp ( t τ log ( T 0 − 1 ⋅ T 1 ) ) \boldsymbol{T}_t = \boldsymbol{T}_0 \cdot \exp(\dfrac{t}{\tau} \log{(\boldsymbol{T}_0^{-1} \cdot \boldsymbol{T}_1)}) Tt=T0⋅exp(τtlog(T0−1⋅T1))
3.2. 三次样条插值
三次样条插值(Cubic Spline Interpolation)是一种常用的数值插值方法,用于在一组数据点之间构造一条平滑曲线。它的核心思想是:在每两个相邻的数据点之间用一个三次多项式段来拟合,保证整条曲线在每个节点处一阶和二阶导数连续,从而构建一条光滑的整体曲线。
给定 n + 1 n+1 n+1 个数据点:
( x 0 , y 0 ) , ( x 1 , y 1 ) , ⋯ , ( x n , y n ) , x 0 < x 1 < ⋯ < x n (x_0, y_0), (x_1, y_1), \cdots, (x_n, y_n), \quad x_0 < x_1 < \cdots < x_n (x0,y0),(x1,y1),⋯,(xn,yn),x0<x1<⋯<xn我们希望在每个区间 [ x i , x i + 1 ] [x_i, x_{i+1}] [xi,xi+1] 上,构造一个三次多项式 S i ( x ) S_i(x) Si(x):
S i ( x ) = a i + b i ( x − x i ) + c i ( x − x i ) 2 + d i ( x − x i ) 3 S_i(x) = a_i + b_i (x - x_i) + c_i (x - x_i)^2 + d_i (x - x_i)^3 Si(x)=ai+bi(x−xi)+ci(x−xi)2+di(x−xi)3使得这 n n n 个多项式段拼接成一条"光滑"的插值曲线。
在论文中 u = t τ ∈ [ 0 , 1 ] u = \dfrac{t}{\tau} \in [0, 1] u=τt∈[0,1],相当于上述的 x − x i x - x_i x−xi,那么三次样条插值可以表述为:
{ B ~ ( u ) = C [ 1 u u 2 u 3 ] , C = 1 6 [ 6 0 0 0 5 3 − 3 1 1 3 3 − 2 0 0 0 1 ] T ( u ( t ) ) = T 0 ∏ j = 0 2 exp ( B ~ j + 1 ( u ( t ) ) ⋅ Ω j ) Ω i = log ( T i − 1 − 1 ⋅ T i ) \begin{cases} \tilde{\boldsymbol{B}}(u) = \boldsymbol{C} \begin{bmatrix} 1 \\ u \\ u^{2} \\ u^{3} \end{bmatrix}, \quad \boldsymbol{C} = \dfrac{1}{6} \begin{bmatrix} 6 & 0 & 0 & 0 \\ 5 & 3 & -3 & 1 \\ 1 & 3 & 3 & -2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \\ \boldsymbol{T}(u(t)) = \boldsymbol{T}_0 \prod_{j = 0}^{2} \exp \left( \tilde{\boldsymbol{B}}_{j+1}(u(t)) \cdot \boldsymbol{\Omega}_j \right) \\ \\ \boldsymbol{\Omega}_i = \log\left(\boldsymbol{T}_{i-1}^{-1} \cdot \boldsymbol{T}_i\right) \end{cases} ⎩
⎨
⎧B~(u)=C
1uu2u3
,C=61
651003300−33001−21
T(u(t))=T0∏j=02exp(B~j+1(u(t))⋅Ωj)Ωi=log(Ti−1−1⋅Ti)其中:
- T ( u ( t ) ) \boldsymbol{T}(u(t)) T(u(t)) 表示 t t t 时刻的相机位姿的插值结果
- Ω i \boldsymbol{\Omega}_i Ωi 表示位姿 T i − 1 \boldsymbol{T}_{i-1} Ti−1 到 T i \boldsymbol{T}_{i} Ti 的相对位姿,并且通过对数映射将李群元素转换到李代数空间
B ~ ( u ) = [ B ~ 0 ( u ) , B ~ 1 ( u ) , B ~ 2 ( u ) , B ~ 3 ( u ) ] T \tilde{\boldsymbol{B}}(u) = \left[ \tilde{B}_{0}(u), \tilde{B}_{1}(u), \tilde{B}_{2}(u), \tilde{B}_{3}(u) \right]^T B~(u)=[B~0(u),B~1(u),B~2(u),B~3(u)]T 为 4 维向量,具体而言: { B ~ 0 ( u ) = 1 B ~ 1 ( u ) = 1 6 ( 5 + 3 u − 3 u 2 + u 3 ) B ~ 2 ( u ) = 1 6 ( 1 + 3 u + 3 u 2 − 2 u 3 ) B ~ 3 ( u ) = 1 6 u 3 \begin{cases} \tilde{B}_{0}(u) = 1 \\ \\ \tilde{B}_{1}(u) = \dfrac{1}{6}(5 + 3u - 3u^{2} + u^{3}) \\ \\ \tilde{B}_{2}(u) = \dfrac{1}{6}(1 + 3u + 3u^{2} - 2u^{3}) \\ \\ \tilde{B}_{3}(u) = \dfrac{1}{6}u^{3} \end{cases} ⎩
⎨
⎧B~0(u)=1B~1(u)=61(5+3u−3u2+u3)B~2(u)=61(1+3u+3u2−2u3)B~3(u)=61u3 T ( u ) \boldsymbol{T}(u) T(u) 的数学表达式如下: T ( u ) = T 0 ⋅ exp ( B ~ 1 Ω 0 ) ⏟ T 0 → T 1 路径 ⋅ exp ( B ~ 2 Ω 1 ) ⏟ T 1 → T 2 路径 ⋅ exp ( B ~ 3 Ω 2 ) ⏟ T 2 → T 3 路径 \begin{equation*} \boldsymbol{T}(u) = \boldsymbol{T}_{0} \cdot \underbrace{\exp (\tilde{B}_{1} \boldsymbol{\Omega}_{0})}_{\text{$\boldsymbol{T}_{0} \rightarrow \boldsymbol{T}_{1}$ 路径}} \cdot \underbrace{\exp (\tilde{B}_{2} \boldsymbol{\Omega}_{1})}_{\text{$\boldsymbol{T}_{1} \rightarrow \boldsymbol{T}_{2}$ 路径}} \cdot \underbrace{\exp (\tilde{B}_{3} \boldsymbol{\Omega}_{2})}_{\text{$\boldsymbol{T}_{2} \rightarrow \boldsymbol{T}_{3}$ 路径}} \end{equation*} T(u)=T0⋅T0→T1 路径
exp(B~1Ω0)⋅T1→T2 路径
exp(B~2Ω1)⋅T2→T3 路径
exp(B~3Ω2)其中:
Ω 0 = log ( T 0 − 1 T 1 ) Ω 1 = log ( T 1 − 1 T 2 ) Ω 2 = log ( T 2 − 1 T 3 ) \begin{align*} \boldsymbol{\Omega}_0 &= \log(\boldsymbol{T}_0^{-1}\boldsymbol{T}_1) \\ \boldsymbol{\Omega}_1 &= \log(\boldsymbol{T}_1^{-1}\boldsymbol{T}_2) \\ \boldsymbol{\Omega}_2 &= \log(\boldsymbol{T}_2^{-1}\boldsymbol{T}_3) \end{align*} Ω0Ω1Ω2=log(T0−1T1)=log(T1−1T2)=log(T2−1T3)值得注意的是: B ~ 0 \tilde{B}_{0} B~0 无对应运动:因为控制点序列从 T 0 \boldsymbol{T}_0 T0 开始,没有 T − 1 → T 0 \boldsymbol{T}_{-1} \rightarrow \boldsymbol{T}_0 T−1→T0 的运动可编码。
3.3. K 阶贝塞尔曲线插值
对于 K K K 阶贝塞尔曲线,我们需要 K + 1 K + 1 K+1 个控制点 T i ( i = 1 , 2 , ⋯ , K + 1 ) \boldsymbol{T}_i \ (i = 1, 2, \cdots, K + 1) Ti (i=1,2,⋯,K+1)。
在欧氏空间 R n \Bbb{R}^n Rn 中, K K K 阶 Bézier 曲线为:
x ( u ) = ∑ i = 0 K B i K ( u ) ⋅ x i = ∑ i = 0 K ( K i ) ( 1 − u ) K − i u i ⋅ x i x(u) = \sum_{i = 0}^{K} B_i^K(u) \cdot x_i = \sum_{i = 0}^{K} \binom{K}{i} (1 - u)^{K - i} u^i \cdot x_i x(u)=i=0∑KBiK(u)⋅xi=i=0∑K(iK)(1−u)K−iui⋅xi为了将 Bézier 插值推广到 S E ( 3 ) SE(3) SE(3),我们需要对每个控制点 T i ∈ S E ( 3 ) \boldsymbol{T}_i \in SE(3) Ti∈SE(3) 映射到李代数:
ξ i = log ( T i ) ∈ s e ( 3 ) \boldsymbol{\xi}_i = \log(\boldsymbol{T}_i) \in se(3) ξi=log(Ti)∈se(3)使用 Bézier 权重在李代数空间中构造组合项:
η i = B i K ( u ) ⋅ ξ i = ( K i ) ( 1 − u ) K − i u i ⋅ log ( T i ) \boldsymbol{\eta}_i = B_i^K(u) \cdot \boldsymbol{\xi}_i = \binom{K}{i} (1 - u)^{K - i} u^i \cdot \log(\boldsymbol{T}_i) ηi=BiK(u)⋅ξi=(iK)(1−u)K−iui⋅log(Ti)则 K K K 阶贝塞尔曲线插值的结果为:
η = ∑ i = 0 K η i = ∑ i = 0 K ( K i ) ( 1 − u ) K − i u i ⋅ log ( T i ) \boldsymbol{\eta} = \sum_{i = 0}^{K} \boldsymbol{\eta}_i = \sum_{i = 0}^{K} \binom{K}{i} (1 - u)^{K - i} u^i \cdot \log(\boldsymbol{T}_i) η=i=0∑Kηi=i=0∑K(iK)(1−u)K−iui⋅log(Ti)将插值结果通过指数映射回 S E ( 3 ) SE(3) SE(3) 有:
T t = exp ( η ) = exp ( ∑ i = 0 K ( K i ) ( 1 − u ) K − i u i ⋅ log ( T i ) ) = ∏ i = 0 K exp ( ( K i ) ( 1 − u ) K − i u i ⋅ log ( T i ) ) \boldsymbol{T}_t = \exp(\boldsymbol{\eta}) = \exp \left(\sum_{i = 0}^{K} \binom{K}{i} (1 - u)^{K - i} u^i \cdot \log(\boldsymbol{T}_i)\right) = \prod_{i = 0}^{K} \exp\left( \binom{K}{i} (1 - u)^{K - i} u^i \cdot \log(\boldsymbol{T}_i) \right) Tt=exp(η)=exp(i=0∑K(iK)(1−u)K−iui⋅log(Ti))=i=0∏Kexp((iK)(1−u)K−iui⋅log(Ti))值得注意的是: T t \boldsymbol{T}_t Tt 相对于 T i \boldsymbol{T}_i Ti 是可微分的,从而可以通过优化 T i \boldsymbol{T}_i Ti 来优化其运动模糊效果。