光流追踪KLT方法技巧
Inverse Compositional
KLT方法中对齐操作有Forward Additive Image Alignment（前向加法），Forward Compositional Image Alignment（前向组合），Inverse Compositional Image Alignment（逆向组合）等方法。
对于图像对齐算法表达如下：
\[
\min\limits_{\boldsymbol{p}}\sum_{\boldsymbol{x}}\left[I(\mathbf{W}(\boldsymbol{x};\boldsymbol{p}))-T(\boldsymbol{x})\right]^2
\]
利用图像I和模板T光度不变假设求取参数。其中\(W\)可以参数化旋转平移或者其他。
前向加法方法对p追加扰动，如下：
\[
\sum_{\boldsymbol{x}}\left[I(\mathbf{W}(\boldsymbol{x};\boldsymbol{p}+\Delta p))-T(\boldsymbol{x})\right]^2
\]
一阶泰勒展开：
\[
\begin{align}
I({\mathbf W}({\boldsymbol x};{\boldsymbol p}+{\boldsymbol\Delta}{\boldsymbol p}))&\approx I({\mathbf W}(\boldsymbol x;{\boldsymbol p}))+\dfrac{\partial I(\mathbf W({\mathbf W};{\boldsymbol p})}{\partial{\boldsymbol p}}\Delta{\boldsymbol p}\quad \\
&=I(\mathbf{W}(\boldsymbol{x};\boldsymbol{p}))+{\frac{\partial I(\mathbf{W}({\boldsymbol{x}};\boldsymbol{p})}{\partial{\boldsymbol{x}}^{\prime}}}{\frac{\partial{\mathbf{W}}({\boldsymbol{x;p}})}{\partial\boldsymbol{p}}}\Delta\boldsymbol{p}\quad \\
&=I(\mathbf W(\boldsymbol x;\boldsymbol p))+\nabla I\dfrac{\partial\mathbf W}{\partial\boldsymbol p}\Delta\boldsymbol p
\end{align}
\]
最终变为：
\[
\sum_\mathbf{x}\Big[I(\mathbf{W}(\mathbf{x};\mathbf{p}))+\mathbf{\nabla}I\dfrac{\partial\mathbf{W}}{\partial\mathbf{p}}\Delta\mathbf{p}-T(\mathbf{x})\Big]^2.
\]
其在迭代过程中反复计算雅克比，时间复杂度为\(O(n^2 N + n^3)\).
逆向组合方法对模板追加变换扰动如下：
\[
\sum_{\boldsymbol{x}}\left[T(\mathbf{W}(\boldsymbol{x};\Delta\boldsymbol{p})-I(\mathbf{W}({\boldsymbol{x}};\boldsymbol{p}))\right]^2
\]
一阶泰勒展开：
\[
\sum\limits_{\mathbf{x}}\bigg[T(\mathbf{W}(\mathbf{x},\mathbf{0}))+\mathbf{\nabla}T\dfrac{\partial\mathbf{W}}{\partial\mathbf{p}}\Delta\mathbf{p}-I({\mathbf{W}}(\mathbf{x};\mathbf{p}))\bigg]^2.
\]
求解如下：
\[
H=\sum_{\mathbf{x}}{\left[ \Delta T{\frac{\partial\mathbf{W}}{\partial\mathbf{p}}}\right]}^{T} {\left[{\boldsymbol{\nabla T}}{\frac{\partial\mathbf{\mathbf{W}}}{\partial\mathbf{p}}}\right]}
\]
\[
\Delta\mathbf{p}=\color{red}{ H^{-1}\sum_{\mathbf{x}}\bigg[\nabla T\dfrac{\partial\mathbf{W}}{\partial\mathbf{p}}\bigg]^T } \bigg[I\big(\mathbf{W}\big(\mathbf{x};\mathbf{p}\big)\big)-T\big(\mathbf{x}\big)\bigg]
\]
其中红色部分都可以提前计算好，时间复杂度为\(O(nN + n^3)\).
LSSD
locally-scaled sum of squared differences(LSSD) 主要思想是通过patch像素除以均值消除广度变化导致的跟踪失效问题。
\[
r_i(\boldsymbol{\xi})=\dfrac{I_{t+1}(\mathbf{T}\mathbf{x}_i)}{\overline{I_{t+1}}}-\dfrac{I_t(\mathbf{x}_t)}{\overline{I_t}}\quad\forall\mathbf{x}_i\in\Omega.
\]
特征点参数化
特征点参数化有如下几种方法：
- global xyz： [x, y, z]
-
anchored inverse depth: 只优化逆深度
\[
\frac{1}{d}[x,y,z]=\frac{1}{d}[u,v,1],d = \frac{1}{Z}
\]
-
Anchored Bearing Vector + Inverse Depth: 3D点的方位角在球坐标系下表示
\[
\dfrac1d[x,y,z]=\dfrac1d[\cos\phi\sin\psi,\sin\phi,\cos\phi\text{cos}\psi] \\
d=\frac{1}{\sqrt{X^2+Y^2+Z^2}}
\]
-
Anchored Bearing Vector + Inverse Depth: 3D点的方位角使用极射赤平投影表示
\[
\frac{1}{d}[x,y,z]=\dfrac{1}{d}[\eta u,\eta v,\eta-1],\quad\eta=\dfrac{2}{1+u^2+v^2}, \quad d=\dfrac{1}{\sqrt{X^2+Y^2+Z^2}}
\]
Stereographic projection(极射赤平投影):
- 投影球：黑色曲线的表示的大圆；
- 赤平面： 过球心的水平面(蓝色直线所示的X-Y plane)；
- 极射点：球上两极发射点，图中的黄色和黑圆点所示，这里我们选择的极射点是黑色圆点[0, 0, -1]。
投影：连接空间3D点的Unit bear vector末端点与极射点[0, 0, -1]得到的直线，与赤平面的交点即为空间
3D点Unit bear vector在赤平面的投影。
反投影：为了获得空间3D点的Unit bear vector，可以将过极射点和3D点Unit bear vector在赤平面的投影点的直线与投影球面相交获得。
设3D点\(P=[X,Y,Z]\)，其单位向量\([X/d, Y/d, Z/d]\)在赤平面的投影点为\([u,v,0]\)，其满足的直线方程：
\[
\frac{X/d-0}{u-0}=\frac{Y/d-0}{v-0}=\frac{Z/d+1}{0+1} \\
d=\sqrt{X^2+Y^2+Z^2}
\]
该直线经过\(\left[\frac{X}{d},\frac{Y}{d},\frac{Z}{d}\right],[u,v,0],[0,0,-1]\)
由直线方程可得：
\[
u=\frac{X}{d+Z}\quad v=\frac{Y}{d+Z}
\]
令：
\[
[x,y,z]=\left[\frac Xd,\frac Yd,\frac Zd\right]
\]
得投影方程：
\[
u=\dfrac{x}{1+z} \quad v=\dfrac{y}{1+z}
\]
由上式得：
\[
[x,y,z]=[(1+z)u,(1+z)v,z]
\]
令：\(\eta = 1 + z\)
得：
\[
[x,y,z]=[\eta u,\eta v,\eta-1]
\]
由单位向量模长可得：
\[
(\eta u)^2 + (\eta v)^2 + (\eta-1)^2 = 1
\]
最终得到：
\[
\eta=\dfrac{2}{1+u^2+v^2}
\]
Bundle Adjustment
Batch Bundle Adjustment
Sliding Window optimization
Reference
[0] Lucas-Kanade 20 Years On: A Unifying Framework
[1] Visual-Inertial Mapping with Non-Linear Factor Recovery