直接法公式推导

单层直接法

已知两帧图像，估计相机运动

设待估计的目标为$T_{cur,ref}$，在参考帧（前一时刻）中取一组点{pi}\{p_i\}{pi​}的位姿可以通过最小化目标函数求解：
$${\mathbf{T}}_{\mathrm{cur},\mathrm{ref}}=\frac{1}{N}\sum _{i=1}^N\sum _{W_i}{\Vert I_{\mathrm{ref}}\left(\pi \left({\mathbf{p}}_i\right)\right)-I_{\mathrm{cur}}\left(\pi \left({\mathbf{T}}_{\mathrm{cur},\mathrm{ref}}{\mathbf{p}}_i\right)\right)\Vert }_2^2.$$
其中$N$为点数，$\pi$函数为针孔相机的投影函数${\mathbb{R}}^3\mapsto {\mathbb{R}}^2$，$W_i$为第$i$ 个点周围的小窗口。同光流，可用Gauss-Newton函数求解。

1. 该问题中的误差：
   $$e(T)=I_{ref}(\pi (p_i))-I_{cur}(\pi (T_{cur,ref}{p}_i))$$

2. 误差相对于自变量的梯度：

   自变量：相机的位姿$\xi$ （李代数形式，6个自由度），故雅克比矩阵为$1\times 6$的矩阵。

   给误差（其实是作用在T上）施加一个扰动：
   $$\begin{gathered} e(\delta T\oplus T)=I_{ref}(p_1)-I_{cur}(\frac{1}{Z_2}Kexp(\delta {\xi }^{\wedge })TP) \\ \approx I_{ref}(p_1)-I_{cur}(\frac{1}{Z_2}K(1+\delta {\xi }^{\wedge })TP) \\ =I_{ref}(p_1)-I_{cur}(u+\frac{1}{Z_2}K\delta {\xi }^{\wedge }TP) \\ \approx I_{ref}(P_1)-I_{cur}(u)-{I_{cur}}^{\prime }(u)\delta \xi \\ =e(T)-\frac{\partial {\mathbf{I}}_{cur}}{\partial \stackrel{\rightharpoonup }{u}}\frac{\stackrel{\rightharpoonup }{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}\delta \mathbf{\xi } \end{gathered}$$

   完全按照一阶泰勒，上边近似后不应该是$\delta \xi$,应该是上边的$u$后边的那一坨。
   难道是因为两个都是无穷小量，所以相等？

   $$\begin{gathered} q=TP \\ {u}^{\rightharpoonup }=\frac{1}{Z_2}Kq \end{gathered}$$

   • 注：$q$为$P$在第二个相机坐标系下的坐标，$\stackrel{\rightharpoonup }{u}$ 是它的像素坐标，由（4）可知，$q$是$T$的函数，$\stackrel{\rightharpoonup }{u}$ 是$q$的函数，这里的$P$是已知位置的空间点。

   • $$\begin{gathered} \underset{\delta \xi \to 0}{\lim }\frac{e(\delta \xi \oplus \xi )-e(\xi )}{\delta \xi }=-\frac{\partial I_{cur}}{\partial \stackrel{\rightharpoonup }{u}}\frac{\partial \stackrel{\rightharpoonup }{u}}{\partial q}\frac{\partial q}{\partial \delta \xi } \\ =J \end{gathered}$$

   • 梯度矩阵：

   • $$J=-\frac{\partial I_{cur}}{\partial \stackrel{\rightharpoonup }{u}}\frac{\partial \stackrel{\rightharpoonup }{u}}{\partial q}\frac{\partial q}{\partial \delta \xi }$$
     雅克比矩阵的维度为$1\times 6$

   • 其中：

     • $$\frac{\partial \mathbf{u}}{\partial \mathbf{q}}=\left[\begin{array}{ccc}\frac{\partial u}{\partial X}& \frac{\partial u}{\partial Y}& \frac{\partial u}{\partial Z}\\ \frac{\partial v}{\partial X}& \frac{\partial v}{\partial Y}& \frac{\partial v}{\partial Z}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\end{array}\right]$$

     • $$\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}=\left[\mathbf{I},-{\mathbf{q}}^{\wedge }\right]$$

     • $$\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}=\left[\begin{array}{cccccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}& -\frac{f_{x}XY}{Z^2}& f_x+\frac{f_x{X}^2}{Z^2}& -\frac{f_{x}Y}{Z}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}& -f_y-\frac{f_y{Y}^2}{Z^2}& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z}\end{array}\right]$$

   • 若采用中值差分：

     • $$\frac{\partial I_{cur}}{\partial u}=\frac{1}{2}\left[\begin{array}{c}{I}_{cur}(u+1,v)-I_{cur}(u-1,v),I_{cur}(u,v+1-I_{cur}(u,v-1))\end{array}\right]$$

3. 窗口尽量取大一点（上一帧图像中的点可能在投影后跑到下一帧图像对应窗口的外部），但也不能太大，否则计算时间太长。

   窗口不要取单个点（投影之后点就没了，不在窗口范围内）

单层直接法：

• 可使用光流计算视差进而生成稀疏点云