LM算法计算单应矩阵

slam

---

单应矩阵

单应矩阵的定义

什么是单应矩阵呢？其实简单来说，就是两个图像之间的变换矩阵。什么意思呢，可以考虑这样一个情形：
你有一个相机，拍摄一个建筑物，首先在某一个视角拍了一张，然后又走到另一个角度再拍一张，这样你就得到了对同一个场景进行拍摄的两张不同的图像，容易理解的是，既然是刻画同一个场景，那么这两图之间必然存在着一定的联系(映射)，这个联系如果用一个矩阵来刻画，那么得到的这个矩阵，我们就把它叫做单应矩阵了(hemography)，如果用一个公式来刻画，那么自然而然可以吧公式写成：

$$x' = Hx$$
其中$x$是齐次坐标，即说$x = (u, v, 1)^T$，u,v为图像坐标，$x'$是新的图像坐标下，对原来图像坐标下同一点的观测结果。值得注意的是，对于其次坐标，我们可以任意乘以一个常数，这个坐标的意义并不改变，或者可以不那么严格的写这样一个算式$x = kx$, 因此，对于映射来说，H矩阵让x释放出一个尺度上的不确定度，因此H矩阵的自由度等于8,或者说，计算H矩阵，只用计算出8个数，然后令最后一个数等于1就行了，或者也可以给出一个其他的约束，确定这个矩阵

单应矩阵的作用

那么单应矩阵有什么用呢？
最直接的应用当然是图像拼接了，可以理解的是，当你对一个大场景进行观测时，或者说你拍了很多照片，每个照片有重叠部分又有新的不分，假如我们选择其中某一个图作为基准，所有的图像都像基准图进行坐标变换，然后再把重叠部分叠再一起，那么多出来的图像信息也就自然而然地加到基准图上了。
其他的应用，包括图像增稳，透视纠正，视图合成等，它在图像领域是一个很好很强大的工具，大多数应用的第一步而且是最核心的一步都是计算这个矩阵，之后再进行其他处理，这个矩阵的算法很多，接下来就介绍一个超级强大，应用超级广泛的算法，即基于LM的迭代优化算法

---

单应矩阵的计算

基本原理

首先想要通过优化得到这个矩阵，自然而然需要建立一个基于优化的数学模型，我们从它的定义$x' = Hx$开始

$$x'_i = Hx_i => x'_i \times Hx_i = 0$$
把那个叉积方程整理之后有:
$$\left [ \begin{array} 1 0^T && -w_i'x_i^T && y'_ix_i^T \\ w'_ix_i^T && 0^T && -x_i'x_i^T \\ -y_i'x_i^T && x_i'x_i^T && 0^T \end{array} \right ] \left ( \begin{array} 1 h^1 \\ h^2 \\ h^3 \end{array} \right ) = 0$$

其中$x'_i = (x'_i, y'_i, w'_i)^T$, $H = (h^1, h^2, h^3)$
由于坐标是其次坐标(第三个坐标可以理解为尺度，但是同一个点不同尺度表达同一个点)，这三个方程实际上是相容的，用两个方程就足以刻画所有的约束，即

$$\left [ \begin{array} 1 0^T && -w_i'x_i^T && y'_ix_i^T \\ w'_ix_i^T && 0^T && -x_i'x_i^T \end{array} \right ] \left ( \begin{array} 1 h^1 \\ h^2 \\ h^3 \end{array} \right ) = 0$$

简单记为$A_ih = 0$

正如先前讲的，这个方程看似9个未知数，但其中有一个是用来控制比例的，可以通过它随意缩放，矩阵，因此实际上只有8个未知数，一个点有两个方向的约束，可以提供两个方程，所以理论上只需要4个点就能够算出一个H矩阵来，这样的解叫做最小配置解，但实际上你的到的图像上对应点的数一般总是多于8个的，而且可能存在各种各样的噪声，最小配置解总是不那么鲁棒的，比较好的策略是尽量多使用正确的匹配点，得到一个使得全局误差最小的H矩阵，比如基本DLT算法，即计算$A^TA$的最小特征值所对应的特征向量和A的零空间等价...
在这里，我们介绍的是一个强大的迭代优化算法，它总是在实际应用中用得更加广泛

LM算法

Sampson近似的Newton迭代

定义点$X = (x, y, x', y')^T$满足等式$Ah = 0$，我们认为这个点$X$受到h的约束，使得它根据某种规则定义的矩阵A有$Ah = 0$的结果，这种关系我们将他记为$C_H(X) = 0$

现在我们约定一些符号:
$d_{\bot}(x_1, x_2)$ :表达$x_1，x_2$之间的几何距离
$\hat{X}$ :表示最优估计
$|| \bullet ||$ : 表示二范数

记：$\delta_x = \hat{X} - X$，且我们希望最优解正好满足约束$C_H(\hat{X}) = 0$
那么可以得到：

$$C_H(X + \delta_x) = C_H(X) + \frac{\partial C_H}{\partial X} \delta_x = 0$$
记:$J = \frac{\partial C_H}{\partial X}$,可得:$J \delta_x = -C_H(X) = -\epsilon$ ,
最后我们的问题简化成了:求在满足$J \delta_x = -\epsilon$条件下，使得$||\delta_x ||$最小的向量$\delta_x$,由此可以算出
$$\delta_x = -(J^TJ)^{-1}J^T\epsilon$$

那么现在算法是比较显然的
对于某一次迭代得到$X_i$，然后求出$X_i$对应的雅可比矩阵$J_i$,之后算出新的增量$\delta_{X_i}$,由此

$$X_{i+1} = X_i + \delta_{X_i}$$

Levenberg-Marquardt迭代

在Newton迭代的基础上，我们对它进行改进，将$N\delta = J^TJ\delta = -J^T\epsilon$用$N'\delta = -J^T\epsilon$代替,其中

$$N'(i,j) = \left \{ \begin{array} 1 (1+\lambda)N(i,j) && i = j \\ N(i, j) && i \neq j \end{array} \right .$$

之后我们需要判断得到的$\delta$是否使得误差减小，如果变小，那么接受它，并且将$\lambda$除以10,否则将$\lambda$乘以10，重新解方程，直到误差减小为止

这个算法其实就这么简单，比Newton迭代多了几步，但实际上会减少一些迭代次数，效率更高，但是，实际上这个算法就直接这样套用去计算单应矩阵，这会是一件无比坑爹的事情，因为假设有n个匹配点，那么就会出现2n+9个参数，然而核心计算的时间复杂度是可怕的$N^3$(解线性方程)，而且这个核心计算会运行无比多次，这会直接导致你算到个矩阵算到明年。
但是你大可不必担心，接下来，隆重推出稀疏算法，这样将会无比愉快地得到一个时间复杂度为n的超高效算法，这个算法的优越性在大量实际应用中得到体现

---

稀疏LM求解2D单应

现在让我们忘掉那些该死的抽象的数学吧，重新从方程$x' = Hx$入手(实际上你可以发现，由于这个方程是线性的，所以sampson近似的误差其实和代数误差等价的，所以我们有理由回到最初的方程),由于这个问题的特殊化，可以得到一个十分简洁快速的算法

基本问题描述

给定两幅图中的对应点集$x_i\leftrightarrow x'_i$,我们使用代数误差来进行优化计算，定义测量向量$X=(x^T_i, x^{'T}_i)^T$，在当前情况下，参数向量$P = (h^T, \hat{x}_1^T, \hat{x}_2^T,\ ...\ , \hat{x}_n^T)^T$，其中$\hat{x}_i$是第一幅图的估计值，h是单应矩阵H的元素组成的向量，函数$f$把$P$映射到$(\hat{X}_1^T, \hat{X}_2^T,\ ...\ , \hat{X}_n^T)^T$，对于每一个$\hat{X}_i = (\hat{x}_i^T, H\hat{x}_i^T)^T = (\hat{x}_i^T, \hat{x}_i'^T)^T$
在这种情况下，每个$\hat{X}_i$仅仅由$h$和$\hat{x}_i$确定，因此必然会有

$$\frac{\partial \hat{X}_i}{\partial b_j} = 0 \quad if \ i \neq j$$
于是就能有一个很强大的形式摆在眼前

算法步骤

定义：
$A_i = \frac{\partial \hat{X}_i}{\partial h} = \left [ \begin{array} 0 \ \ 0 \\ \frac{\partial \hat{x}_i'}{\partial h} \end{array} \right ]$

$B_i = \frac{\partial \hat{X}_i}{\partial \hat{x}_i} = \left [ \begin{array} I \ \ I \\ \frac{\partial \hat{x}'_i }{\partial \hat{x}_i} \end{array} \right ]$

$\epsilon = (\epsilon_1^T, \epsilon_2^T,\ ...\ ,\epsilon_n^T)^T$

其中
$\frac{\partial \hat{x}_i'}{\partial h} = \left [ \begin{array} 0 x & y & z & & & & & \\ & & & x & y & z & & \\ & & & & & & x & y & z \end{array} \right ]$
$\frac{\partial \hat{x}'_i }{\partial \hat{x}_i} = H$

那么方程可以化简为如下形式

$$J\delta = \left [ \begin{array} 0 A_1 && B_1 && && ... &&\\ A_2 && && B_2 && ... \\ \\... && ... && ... && ... \\ A_n && && && ... && B_n \end{array} \right ] \left [ \begin{array} 0 \delta_a \\ \delta_{b1} \\ \delta_{b2} \\ \ \ \vdots \\ \delta_{bn} \end{array} \right ] = \left [ \begin{array} 0 \epsilon_1 \\ \ \vdots \\ \epsilon_n \end{array} \right ]$$

之后，我们可以得到这样一系列的矩阵

$$A = [A_1^T, A_2^T, \ ... \ ,A_n^T]^T \\ B = diag(B_1, B_2, \ ... \ . B_n) \\ \Sigma_x = diag(\Sigma_{x_1}, \Sigma_{x_2}, \ ... \ , \Sigma_{x_n} ) \\ \delta_b = (\delta_{b_1}^T, \delta_{b_2}^T, \ ... \ , \delta_{b_n}^T ) \\ \epsilon = (\epsilon_1^T, \epsilon_2^T, \ ... \ \epsilon_n^T)^T$$

其中$\Sigma_x$代表协方差矩阵(如果给出了协方差矩阵，可以考虑马氏距离，这样得到的算法会更鲁棒，结果也会更好)，$delta$代表需要估计的参数值，$\epsilon$代表误差

最后，让我们来列一下算法详细流程:

已知 　 测量矢量 $X$ 以及它的协方差矩阵 $\Sigma_x$, 参数集合$:P = (h^T, \hat{x}_1^T, \hat{x}_2^T, \ ... \ , \hat{x}_n^T )^T$,测量集合 $X = (X_1^T, X_2^T, \ ... \ , X_n^T)^T$, 对任意的$i \neq j$, $\frac{\partial \hat{X}_i}{\hat{x}_j } = 0$

目标　求最小化$\epsilon^T \Sigma_x^{-1}\epsilon$的参数集合$P$，其中$\epsilon = X - \hat{X}$

算法 　

　1. 初始化常数$\lambda$(一般取0.001),参数集合$P_0$
　2. 计算微分矩阵$A_i = \frac{\partial \hat{X}_i}{\partial h}$以及$B_i = \frac{\partial \hat{X}_i}{\partial \hat{x}_i}$ 以及误差矢量$\epsilon = X_i - \hat{X}_i$
　3. 计算中间值
　　　$U = \sum\limits_i A_i^T\Sigma^{-1}_{x_i}A_i$
$\qquad \ V = diag(V_1, V_2, \ ... \ , V_n)$,其中$V_i = B^T_i \Sigma_{x_1}^{-1} B_i$
$\qquad \ W = [W_1, W_2, \ ... \ ,W_n]$,其中$W_i = A_i^T\Sigma_{x_i}^{-1}B_i$
$\qquad \ \epsilon_A = \sum\limits_i A_i^T \Sigma_{x_i}^{-1}\epsilon_i$
$\qquad \ \epsilon_B = (\epsilon_{B_1}^T, \epsilon_{B_2}^T, \ ... \ , \epsilon_{B_n}^T)^T$, 其中$\epsilon_{B_i} = B_i^T\Sigma_{x_i}^{-1}\epsilon_i$
$\qquad \ U^* = U + (1 + \lambda)I, V^* = V + (1 + \lambda)I$
$\qquad \ Y_i = W_iV^{*-1}$

$\ \ \ $ 4. 下列方程计算$\delta a$

$$( U^* - \sum \limits_i Y_i W_i^T) \delta_a = \epsilon_A - \sum \limits_i Y_i \epsilon_B)$$

$\ \ \ $ 5. 由方程 $\delta_{b_i} = V^{*-1} (\epsilon_{B_i} - W_i^T \delta_a)$计算每个$\delta_{b_i}$
$\ \ \ $ 6. 通过向量$(\delta_a^T, \delta_b^T)^T$更新$P$，并计算新的误差
$\ \ \ $ 7. 判断误差是否减小，如果减小，那么接受更新的参数值，并把$\lambda$减小至之前的10分之１，继续迭代或终止；否则，拒绝参数更新，把$\lambda$扩大10倍，重新计算