SLAM在线光度标定总结

Introduction¹

直接法依赖于光度恒定假设，但是多帧之间的曝光时间和辐照衰减都会破坏假设。

光度模型

物体通过反射光线从而被看见，一点反射的光量叫辐射（radiance）$L$L。如果辐射和观测的角度独立，则是朗伯反射（漫反射）。

传感器在$x$x位置接收的能量值叫辐照（Irradiance）$I(x)$I(x).

不同的空间点的辐射与辐照应是一一对映的。但是相机都会存在渐晕，它符合cos的四阶关系，因此用$V:\Omega\to[0,1]$V:Ω→[0,1]建模：
$I(x)=V(x) L \tag{1}$I(x)=V(x)L(1)
传感器在曝光时间$e$e内对辐照进行积分，假设期间辐照恒定，因此建模为：
$I_{a c c}(x)=e I(x) \tag{2}$Iacc​(x)=eI(x)(2)
经过相机响应函数(CRF)$f:\R\to[0,255]$f:R→[0,255]后输出图像强度，CRF限制了动态范围，过曝会设为255，欠曝会设置为0.我们无法得到决对尺度，因此把CRF归一化到单位区间$[0,1]$[0,1]。

因此辐射$L$L映射为图像强度$O$O：
$O=f(e V(x) L) \tag{3}$O=f(eV(x)L)(3)

跟踪前端

使用金字塔KLT跟踪，在此基础上增加帧间的增益比，使用舒尔补来高效的实现。

为了恢复渐晕需要将特征点覆盖整个图像，因此使用网格来使特征均匀分布，使用长特征跟踪来更好的覆盖径向。

为了改善长期跟踪，使用双向光流剔除误匹配。同时还在特征附近使用小patch来一起加入优化，引入低梯度图像区域，降低对于几何误差的敏感。

优化后端

能量函数：
$E=\sum_{p \in P} \sum_{i \in F_{p}} w_{i}^{p}\left\|\underbrace{O_{i}^{p}-f\left(e_{i} V\left(x_{i}^{p}\right) L^{p}\right)}_{r\left(f, V, e_{i}, L^{p}\right)}\right\|_{h} \tag{4}$E=p∈P∑​i∈Fp​∑​wip​∥∥∥∥∥∥∥​r(f,V,ei​,Lp)Oip​−f(ei​V(xip​)Lp)​​∥∥∥∥∥∥∥​h​(4)
假设点都是在朗伯平面上。

CRF建模：
$f_{G}(x)=f_{0}(x)+\sum_{k=1}^{n} c_{k} h_{k}(x) \tag{5}$fG​(x)=f0​(x)+k=1∑n​ck​hk​(x)(5)
选择$n=4$n=4，它满足$f_G(0)=0,f_G(1)= 255$fG​(0)=0,fG​(1)=255。

假设衰减因子是中心对称：
$V(x)=1+v_{1} R(x)^{2}+v_{2} R(x)^{4}+v_{3} R(x)^{6} \tag{6}$V(x)=1+v1​R(x)2+v2​R(x)4+v3​R(x)6(6)
使用LM算法，将辐射的估计与其它参数解耦。

第一步

固定$L^p$Lp，求雅克比
$J=\left(\frac{\partial r}{\partial \boldsymbol{c}}, \frac{\partial r}{\partial \boldsymbol{v}}, \frac{\partial r}{\partial e_{i}}\right) \tag{7}$J=(∂c∂r​,∂v∂r​,∂ei​∂r​)(7)
通过求解公式（8）获得：
$\left(\boldsymbol{J}^{T} \boldsymbol{W} \boldsymbol{J}+\lambda \operatorname{diag}\left(\boldsymbol{J}^{T} \boldsymbol{W} \boldsymbol{J}\right)\right) \Delta \boldsymbol{x}=-\boldsymbol{J}^{T} \boldsymbol{W} \boldsymbol{r} \tag{8}$(JTWJ+λdiag(JTWJ))Δx=−JTWr(8)
权重矩阵由两部分的权重因子组成，第一是Huber函数，第二是降低高梯度图像位置的权重，防止小的不准确导致大的错误，即：
$w_{r}^{(2)}=\frac{\mu}{\mu+\left\|\nabla F_{i}\left(x_{i}^{p}\right)\right\|_{2}^{2}} \tag{9}$wr(2)​=μ+∥∇Fi​(xip​)∥22​μ​(9)
公式（9）会降低图像块中心的权重，增加附近残差的权重。

第二步

固定其他的参数，每个点都是独立更新：
$\boldsymbol{J}^{p}=\left(\frac{\partial r}{\partial L^{p}}\right) \tag{10}$Jp=(∂Lp∂r​)(10)

$\Delta L=\frac{-\boldsymbol{J}_{p}^{T} \boldsymbol{W}_{p} \boldsymbol{r}_{p}}{(1+\lambda) \boldsymbol{J}_{p}^{T} \boldsymbol{W}_{p} \boldsymbol{J}_{p}} \tag{11}$ΔL=(1+λ)JpT​Wp​Jp​−JpT​Wp​rp​​(11)

该算法可以在离线和在线两种模式下工作：

• 离线

所有参数一起优化，大的序列会分块。剔除外点后再进行优化一次。

• 在线

新来一帧先根据当前值，去除渐晕和响应函数的影响：
$E=\sum_{i=1}^{M} \sum_{p \in P_{i}} w_{i}^{p}\left(\underbrace{\frac{f^{-1}\left(O_{i}^{p}\right)}{V\left(x_{i}^{p}\right)}-e_{i} L^{p}}_{r\left(e_{i}, L^{p}\right)}\right)^{2} \tag{12}$E=i=1∑M​p∈Pi​∑​wip​⎝⎜⎜⎜⎛​r(ei​,Lp)V(xip​)f−1(Oip​)​−ei​Lp​​⎠⎟⎟⎟⎞​2(12)

然后优化曝光时间，初始化辐射强度为跟踪点的平均强度。

在后台对渐晕和响应函数进行优化。

---

Introduction²

光圈焦距的变化会影响标定的参数，长时间使用也会变化。和上面的不同，这里没有假设渐晕是中心对称的。

本文²的亮点

• ORB特征附近取patch
• CRF使用薄板样条插值（TPS, Thin Plate Splines）和Gamma曲线建模
• 六阶多项式和TPS建模渐晕
• 通过插值（TPS）稀疏变成稠密估计
• 联合优化辐照度，渐晕和CRF
• CPU实时
• 自然环境和部分动态环境下运行

Method

假设（Assumptions）

• 朗伯反射
• 场景光照不随时间变化
• 衰减矫正系数为[0,1]之间
• 相邻的像素具有相似的衰减

衰减因子估计（Attenuation Factor Estimate）

二维薄板多项式样条为：
$h(\mathbf{u})=p(\mathbf{u})+\sum_{i=1}^{N} c_{i} \cdot \phi\left(\left\|\mathbf{u}-\mathbf{d}_{i}\right\|\right) \tag{13}$h(u)=p(u)+i=1∑N​ci​⋅ϕ(∥u−di​∥)(13)
径向基函数(RBF)
$\phi(r)=r^{2} \cdot \ln (r) \tag{14}$ϕ(r)=r2⋅ln(r)(14)
多项式为：
$p(\mathbf{u})=\mathbf{v}^{\top} \cdot \left( \begin{array}{l}{1} \\ {\mathbf{u}}\end{array}\right) \tag{15}$p(u)=v⊤⋅(1u​)(15)
其中$\bf u$u是数据点，${\bf d}_i$di​是控制点，已知坐标位置$\bf u$u和矫正因子$s_{\rm u}$su​.

在插值的情况下是计算：
$s_{i}=h\left(\mathbf{u}_{i}\right), 1 \leq i \leq M \tag{16}$si​=h(ui​),1≤i≤M(16)
插值需要$N=M$N=M，这是TPS要求的，一个当控制点其他的带入进行拟合，这会无法实现高效在线求解。因此代替方法是，使用包含固定数目$N=P^H\times P^V$N=PH×PV控制点的网格。
$\underset{\mathbf{c}, \mathbf{v}}{\operatorname{argmin}} \sum_{i}^{M}\left\|h\left(\mathbf{u}_{i}\right)-s_{i}\right\|^{2} \tag{17}$c,vargmin​i∑M​∥h(ui​)−si​∥2(17)
每个控制点，RBF固定，选择$P^H,P^V \in \{3,4,5,6,7 \}$PH,PV∈{3,4,5,6,7}，并且需要满足：
$\sum_{i}^{N} c_{i}=0, \sum_{i}^{N} c_{i} \cdot d_{i, x}=0, \sum_{i}^{N} c_{i} \cdot d_{i, y}=0 \tag{18}$i∑N​ci​=0,i∑N​ci​⋅di,x​=0,i∑N​ci​⋅di,y​=0(18)
在渐晕矫正中，使用的是下面的公式：
$p(r)=1+\sum_{i=1}^{3} v_{i} \cdot r^{2 i} \tag{19}$p(r)=1+i=1∑3​vi​⋅r2i(19)

$h(\mathbf{u})=p\left(\sqrt{2}\left\|\mathbf{u}-\mathbf{d}_{m}\right\|\right)+\sum_{i=1}^{N} c_{i} \cdot \phi\left(\left\|\mathbf{u}-\mathbf{d}_{i}\right\|\right) \tag{20}$h(u)=p(2​∥u−dm​∥)+i=1∑N​ci​⋅ϕ(∥u−di​∥)(20)

这里没太看懂，是怎样计算的插值，应该是TPS插值不懂

目前的理解是使用已知点，利用公式（17）（19）（20）来计算它的参数。

NOTE：为了减小计算量，原本应该每个已知点轮流作为控制点，这里控制点选择是均匀分布在图像的网格中。

相机CRF

使用一维的薄板样条，控制点均匀分布在0和1之间，取5~20个点，是GCCM（Generalized Gamma Curve Model），要增加约束强迫$f(0)=0,f(1)=1$f(0)=0,f(1)=1
$p_{f}(x)=\sum_{i=0}^{n} v_{i} \cdot x^{i} \tag{21}$pf​(x)=i=0∑n​vi​⋅xi(21)

$f_{G G C M}(L)=L^{p_{f}(L)} \tag{22}$fGGCM​(L)=Lpf​(L)(22)

$g_{G G C M}(I)=I^{1 / p_{f}(I)} \tag{23}$gGGCM​(I)=I1/pf​(I)(23)

使用公式（21）代替公式（15）是$GGCM+TPS$GGCM+TPS，使用公式（22）代替公式（15）是$GGCM^{TPS}$GGCMTPS

值得注意的是，公式（22）和公式（23）并不是真正意义上的逆，是分别对两个函数进行估计。

图像矫正

使用公式（17）求解，使用公式（20）插值估计。矫正图像公式为：
$I_{c, \mathbf{u}}=f^{-1}\left(I_{\mathbf{u}}\right) /[V(\mathbf{u}) \cdot k] \tag{24}$Ic,u​=f−1(Iu​)/[V(u)⋅k](24)

关键帧光度标定

和上一篇相似，不同的是联合优化了所有的参数，公式如下：
$\underset{\mathbf{c}, \mathbf{v}, \mathbf{k}, \mathbf{L}_{\mathbf{p}}}{\operatorname{argmin}} \sum_{i, j, o=1}^{N, M, O} \rho_{h}\left(\left\|w_{j}\left[I_{\mathbf{u}_{j}}-f\left(k_{i} V\left(\mathbf{u}_{j}\right) L_{\mathbf{p}_{o}}\right)\right]\right\|^{2}\right) \tag{25}$c,v,k,Lp​argmin​i,j,o=1∑N,M,O​ρh​(∥∥​wj​[Iuj​​−f(ki​V(uj​)Lpo​​)]∥∥​2)(25)
其中$N$N是关键帧数目，$M$M是观测数，$O$O是地图点数目

Huber函数的$\alpha = 0.2/255$α=0.2/255，同样使用公式（9）来降低高梯度权重。使用$5\times 5$5×5的patch，255和0的像素直接丢弃。

单帧的曝光时间估计使用已经估计地图点的辐照（radiance）值：
$\underset{k}{\operatorname{argmin}} \sum_{i=1}^{N} \rho_{h}\left(\left\|w_{i} \cdot\left[\frac{f^{-1}\left(I_{\mathbf{u}_{i}}\right)}{V\left(\mathbf{u}_{i}\right)}-k \cdot L_{\mathbf{p}_{i}}\right]\right\|^{2}\right)\tag{26}$kargmin​i=1∑N​ρh​(∥∥∥∥​wi​⋅[V(ui​)f−1(Iui​​)​−k⋅Lpi​​]∥∥∥∥​2)(26)

整体过程：

• 每一帧会根据当前跟踪的点，和当前的光度参数，来计算曝光时间；
• 关键帧创建时，会refine跟踪地图点和新创建地图点的辐照度（radiance）估计，以及帧的曝光时间。
• 一定数目关键帧后，会进行global对所有的光度参数进行优化。
• 稀疏分布的特征使用样条插值来计算，用于后面的跟踪。

---

光照变化的应对

本文³主要针对外部光照变化导致的鲁棒性，与上面两篇不同。

代价函数

$W(\mathbf{x}, d, \mathbf{M})=\pi_{I_{1}}\left(\mathbf{M} \cdot \pi_{I_{2}}^{-1}(\mathbf{x}, d)\right) \tag{27}$W(x,d,M)=πI1​​(M⋅πI2​−1​(x,d))(27)

$C(\mathbf{M})=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathbf{x} \in \Omega_{D}} \rho\left(I\left(\mathbf{x}_{\mathbf{M}}\right)-T(\mathbf{x})\right) \tag{28}$C(M)=∣ΩD​∣1​x∈ΩD​∑​ρ(I(xM​)−T(x))(28)

这里$\mathbf{x}_{\mathbf{M}} :=W(\mathbf{x}, D(\mathbf{x}), \mathbf{M})$xM​:=W(x,D(x),M)

使用了鲁棒核函数来过滤外点：
$\rho_{k}(r)=\left\{\begin{array}{ll}{\frac{r^{2}}{2},} & {\text { if }|r| \leq k} \\ {k\left(|r|-\frac{k}{2}\right),} & {\text { otherwise }}\end{array}\right. \tag{29}$ρk​(r)={2r2​,k(∣r∣−2k​),​ if ∣r∣≤k otherwise ​(29)
使用逆向组合来计算
$C(\Delta \mathrm{M})=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathrm{x} \in \Omega_{D}} \rho\left(I\left(\mathrm{x}_{\mathrm{M}}\right)-T\left(\mathrm{x}_{\Delta \mathrm{M}}\right)\right) \tag{30}$C(ΔM)=∣ΩD​∣1​x∈ΩD​∑​ρ(I(xM​)−T(xΔM​))(30)

描述子

实际就是特征附近取的Patch

第一种如上图中间所示，没有考虑warp
$C(\Delta \mathbf{M})=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathbf{x} \in \Omega_{D}} \rho\left(\left\|\mathbf{D}_{I}\left(\mathbf{x}_{\mathbf{M}}\right)-\mathbf{D}_{T}\left(\mathbf{x}_{\Delta \mathbf{M}}\right)\right\|_{2}\right) \tag{31}$C(ΔM)=∣ΩD​∣1​x∈ΩD​∑​ρ(∥DI​(xM​)−DT​(xΔM​)∥2​)(31)
这种通常是对应不上的。

第二种上图最右侧的
$C(\Delta \mathbf{M})=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathbf{x} \in \Omega_{D}} \rho\left( \left\| \mathbf{d}\left(I, N(\mathbf{x})_{\mathbf{M}}\right)- \mathbf{d}\left(T, N(\mathbf{x})_{\Delta \mathbf{M}}\right)\right\|_{2}\right) \tag{32}$C(ΔM)=∣ΩD​∣1​x∈ΩD​∑​ρ(∥d(I,N(x)M​)−d(T,N(x)ΔM​)∥2​)(32)
这种计算方法会比较慢，可以使用描述子图像梯度来近似，可以参考⁴：
$\frac{\partial}{\partial \epsilon} \mathbf{d}\left(T, N(\mathbf{x})_{\Delta \mathbf{M}}\right) \approx \nabla \mathbf{D}_{T} \frac{\partial}{\partial \epsilon} \mathbf{x}_{\Delta \mathbf{M}} \tag{33}$∂ϵ∂​d(T,N(x)ΔM​)≈∇DT​∂ϵ∂​xΔM​(33)

光照变化鲁棒方程

上面介绍的是基于光度不变假设的（BCA）

全局模型

A. Global median bias normalization (GMedian)

提前计算一个全局光度偏移
$\beta=\operatorname{median}_{\mathbf{x}}\left(I\left(\mathbf{x}_{\mathbf{M}}\right)-T\left(\mathbf{x}_{\Delta \mathbf{M}}\right)\right) \tag{34}$β=medianx​(I(xM​)−T(xΔM​))(34)
带入到公式（30）中得到：
$C(\Delta \mathbf{M})=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathbf{x} \in \Omega_{D}} \rho\left(I\left(\mathbf{x}_{\mathbf{M}}\right)-T\left(\mathbf{x}_{\Delta \mathbf{M}}\right)-\beta\right) \tag{35}$C(ΔM)=∣ΩD​∣1​x∈ΩD​∑​ρ(I(xM​)−T(xΔM​)−β)(35)
B. Global affine model (GAffine)

使用光度仿射变换模型
$C(\Delta \mathbf{M}, \delta \alpha, \delta \beta)=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathbf{x} \in \Omega_{D}} \rho\left((1+\alpha) I\left(\mathbf{x}_{\mathbf{M}}\right)+\beta- (1+\delta \alpha) T\left(\mathbf{x}_{\Delta \mathbf{M}}\right)-\delta \beta \right) \tag{36}$C(ΔM,δα,δβ)=∣ΩD​∣1​x∈ΩD​∑​ρ((1+α)I(xM​)+β−(1+δα)T(xΔM​)−δβ)(36)
仿射参数和位姿一起更新：
$\alpha :=\frac{\alpha-\delta \alpha}{1+\delta \alpha} \quad \beta :=\frac{\beta-\delta \beta}{1+\delta \alpha} \tag{37}$α:=1+δαα−δα​β:=1+δαβ−δβ​(37)
C. Zero-mean normalized cross correlation (ZNCC)
$\mathrm{ZNCC}(\Delta \mathbf{M})=\frac{\sum_{\mathbf{x} \in \Omega_{D}} i_{\mathbf{x}} t_{\mathbf{x}}}{\sqrt{\sum_{\mathbf{x} \in \Omega_{D}} i_{\mathbf{x}}^{2}} \sqrt{\sum_{\mathbf{x} \in \Omega_{D}} t_{\mathbf{x}}^{2}}} \tag{38}$ZNCC(ΔM)=∑x∈ΩD​​ix2​​∑x∈ΩD​​tx2​​∑x∈ΩD​​ix​tx​​(38)
其中$i_{\mathbf{x}}=\left(I\left(\mathbf{x}_{\mathbf{M}}\right)-\overline{I}\right)$ix​=(I(xM​)−I)，$t_{\mathbf{x}}=\left(T\left(\mathbf{x}_{\Delta M}\right)-\overline{T}\right)$tx​=(T(xΔM​)−T)，$\overline{I}=\frac{1}{\left|\Omega_{D}\right|} \sum_{\mathbf{x} \in \Omega_{D}} I(\mathbf{x})$I=∣ΩD​∣1​∑x∈ΩD​​I(x)，最大化公式（39）即可。

D. Mutual information (MI)

互信息
$\operatorname{MI}(\Delta \mathbf{M})=\sum_{r, t \in \Omega_{I}} p_{I T}(r, t, \Delta \mathbf{M}) \log \left(\frac{p_{I T}(r, t, \Delta \mathbf{M})}{p_{I}(r) p_{T}(t, \Delta \mathbf{M})}\right) \tag{39}$MI(ΔM)=r,t∈ΩI​∑​pIT​(r,t,ΔM)log(pI​(r)pT​(t,ΔM)pIT​(r,t,ΔM)​)(39)
计算直方图，然后计算每个像素值的概率，最大化公式（39）

有需要，具体优化过程看论文中的参考文献。

局部偏差

E. Gradient magnitude (GradM)

使用梯度大小来作为描述子。

使用公式（40）代替公式（32）中的值
$\mathbf{d}(I, N(\mathbf{x}))=\|\nabla(I, N(\mathbf{x}))\|_{2} \tag{40}$d(I,N(x))=∥∇(I,N(x))∥2​(40)
F. Gradient (Grad)

直接使用梯度向量来进行匹配
$\mathbf{d}(I, N(\mathbf{x}))=\nabla(I, N(\mathbf{x})) \tag{41}$d(I,N(x))=∇(I,N(x))(41)
G. Local mean bias normalization (LMean)

减去局部均值
$d(I, N(\mathbf{x}))=I(\mathbf{x})-\frac{1}{|N(\mathbf{x})|} \sum_{\mathbf{y} \in N(\mathbf{x})} I(\mathbf{y}) \tag{42}$d(I,N(x))=I(x)−∣N(x)∣1​y∈N(x)∑​I(y)(42)

类似特征描述子

H. Descriptor fields (DF)

计算描述子
$\begin{aligned} \mathbf{d}(I, N(\mathbf{x}))=&\left[\left[\left(\mathbf{f}_{1} * I\right)(\mathbf{x})\right]^{+},\left[\left(\mathbf{f}_{1} * I\right)(\mathbf{x})\right]^{-}, \ldots\right.\left[\left(\mathbf{f}_{n} * I\right)(\mathbf{x})\right]^{+},\left[\left(\mathbf{f}_{n} * I\right)(\mathbf{x})\right]^{T} \end{aligned} \tag{43}$d(I,N(x))=​[[(f1​∗I)(x)]+,[(f1​∗I)(x)]−,…[(fn​∗I)(x)]+,[(fn​∗I)(x)]T​(43)
其中${\mathbf f}_i$fi​表示卷积核，使用标准差为1的高斯核，符号$[\cdot]^+[\cdot]^-$[⋅]+[⋅]−意义如下
$[x]^{+}=\left\{\begin{array}{ll}{x,} & {\text { if } x \geq 0} \\ {0,} & {\text { otherwise }}\end{array}\right. \quad, \text { and } \quad[x]^{-}=[-x]^{+} \tag{44}$[x]+={x,0,​ if x≥0 otherwise ​, and [x]−=[−x]+(44)
I. Census transform (Census)

在双目深度估计中常用，顺序不变的仿射变化都不变。但是精度较低。方法是和周围的像素比较，每次比较就占一位，例如定义如下，大小定义可变
$\mathbf{d}(I, N(\mathbf{x}))_{i}=\left\{\begin{array}{ll}{1,} & {\text { if } I(\mathbf{x})<I\left(\mathbf{x}_{i}\right)} \\ {0,} & {\text { otherwise }}\end{array}\right. \tag{45}$d(I,N(x))i​={1,0,​ if I(x)<I(xi​) otherwise ​(45)
但是这种方法无法使用基于梯度的优化方法，具体解决办法看论文。

下面是几种方法的对比：

实验结果

实验结果表明，VO数据集中 GradM 方法更好，实际数据集中 Census 效果更好。闭环中SIFT特征效果更好，建议先使用feature进行匹配，然后使用直接法来求精位姿。

Reference

TPS: https://blog.****.net/VictoriaW/article/details/70161180

---

1. Bergmann P, Wang R, Cremers D. Online photometric calibration of auto exposure video for realtime visual odometry and SLAM[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 627-634. ↩︎

2. Quenzel, Jan et al. “Keyframe-Based Photometric Online Calibration and Color Correction.” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2018): 1-8. ↩︎ ↩︎

3. Park S , Thomas Schöps, Pollefeys M . Illumination change robustness in direct visual SLAM[C]// 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017. ↩︎

4. E. Antonakos, J. Alabort-i Medina, G. Tzimiropoulos, and S. P.Zafeiriou, “Feature-based Lucas-Kanade and active appearance models,” ITIP, vol. 24, no. 9, pp. 2617–2632, 2015. ↩︎