初入SLAM(2)——用最小二乘法求亚像素坐标

引言

在立体匹配过程中，我们希望匹配点之间的差距能尽可能小，而初入SLAM——Harris角点检测中，我们接受了使用Opencv获得Harris角点并详细推导了其数学公式。这里的角度坐标是像素坐标，对应的是（整数，整数）。为了获取更精确的像素坐标，我们需要求得亚像素坐标。

Reference

• 最小二乘法
• 初入SLAM——Harris角点检测
• Opencv——亚像素角点cornerSubPixel()源代码分析

资源文件

• cornerSubPixel()讲解

原理讲解

这副图片，我相信你各种博客都看到过，但是大部分博客都没有讲清楚为什么。

解答

• q，即待求的亚像素点，很神秘，未知。
• $p_i$，即q周围的点，坐标是已知的，自己选择
• $(p_i-q)$，即是第一个向量
• $p_i$处的灰度，$G_i$，即是第二个向量

我们再看上面的图片，

1. $p_0$这种情况下，位于一块白色区域，此时，梯度为0
2. $p_1$这种情况，位于边缘，既黑白相交处，此时，梯度不为0，但是，与$p_1-q$相垂直！

所有对于一个标准的角点，都会导致：
$$G_i\ast (p_i-q)=0$$

最小二乘法

对于上面那个方程，我们其实取了很多的$p_i$，那么我们是求不出一个准确的点$q$满足所有的点的，相当于这是一个无解的方程，那么怎么解一个无解的方程勒？
我们将上面的公式转换一下：
$$G_i\ast q=G_i\ast p_i$$
我们令$$\begin{gathered} G_i：A \\ q：\left[\begin{array}{c}x\\ y\end{array}\right] \\ {G}_i\ast p_i：B \end{gathered}$$
那么我们就是要求解方程：
$$A\left[\begin{array}{c}x\\ y\end{array}\right]=B$$
为了更好的具象化，我们给$A$和$B$具象化赋值
我们假设$$A=\left[\begin{array}{c}11\\ 01\\ 21\end{array}\right]$$
$$B=\left[\begin{array}{c}2\\ 2\\ 3\end{array}\right]$$

此时我们要求解的方程就是
$$\left[\begin{array}{c}11\\ 01\\ 21\end{array}\right]\ast \left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}2\\ 2\\ 3\end{array}\right]$$
从列的角度看
$$\left[\begin{array}{l}1\\ 0\\ 2\end{array}\right]\times x+\left[\begin{array}{l}1\\ 1\\ 1\end{array}\right]\times y=\left[\begin{array}{l}2\\ 2\\ 3\end{array}\right]$$
我们定义$a_1=\left[\begin{array}{l}1\\ 0\\ 2\end{array}\right]a_2=\left[\begin{array}{l}1\\ 1\\ 1\end{array}\right]b=\left[\begin{array}{l}2\\ 2\\ 3\end{array}\right]$
那么其实我们可以把$a_1$和$a_2$当作基底，我们现在的问题就是找到一组$x,y$能够最接近$B$，画到图上就如下图所示。

按照正常求解，我们是不可能找到一组$a_1$和$a_2$的线性组合，使得组合后的向量刚好等于$B$，因为任何$a_1$和$a_2$的线性组合只能在$a_1,a_2$所在的平面上。
既然找不到完美的解，那么我们就只能找一个最接近的解，而这个解就是$B$在$a_1,a_2$平面上的投影，垂足就是最接近解的终点与准确解之间的误差。如下图所示。

现在我们就是要求解$A\left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right]=P$，而这个一定是有解的。
另外，我们知道，$P$与$b$之间的误差为：$$e=B-P=B-A\left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right]$$
要想使$b$与$P$之间的差距最小，那么e一定是垂直于$a_1,a_2$组成的平面S的，也就是要垂直于相交向量$a_1,a_2$，所有我们就可以得出要求$e\ast a_1=0和e\ast a_2=0$，用矩阵表示就是：
$$A^{T}e=0$$
代入$e$可得：
$$\begin{gathered} A^T(B-A\left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right])=0 \\ {A}^{T}A\left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right]=A^{T}B \\ \left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right]={\left(A^{T}A\right)}^{-1}{A}^{T}B \end{gathered}$$
至此，我们就求出来了近似解$\hat{x}$。

拉回到原来的公式

$$G_i\ast q=G_i\ast p_i$$
我们令$$\begin{gathered} G_i：A \\ q：\left[\begin{array}{c}x\\ y\end{array}\right] \\ {G}_i\ast p_i：B \end{gathered}$$

通过上面的定义和公式可以推出：
$$\begin{gathered} q=(G_i^T{G}_i{)}^{-1}(G_i^T)G_i{p}_i \\ =(G_i^T{G}_i{)}^{-1}(G_i^T{G}_i)p_i \end{gathered}$$
那么此时，我们已经能够通过多个点求得一个点的坐标了。

权重引入

但是这就一定准确吗？我们采用多点进行计算，本意是为了更准确，但各点离中心距离不一，所以补可一视同仁，要引入权重，一般采用高斯权重。假设$p_i$处权重为$w_i$，上式进一步修正为：
$$q=(G_i^T{G}_i{w}_i{)}^{-1}(G_i^T{G}_i{w}_i)p_i$$

迭代和终止条件

求解一次后，即可得到一个亚像素点$q(q_x,q_y)$。如果以$q$为中心点，再次：
1.选取窗口，得到新的一组$p_i$
2.对$p_i$求梯度
3.用最小二乘法求解
即得到新的点$q_i$。
如此多迭代次数，会得到一系列亚像素点$q_2,q_3,q_4,....q_n$。那么什么时候终止呢？
OpenCV的做法是：
指定迭代次数，比如，迭代10次后，不再进行计算，认为得到最优解。
指定结果精度，比如，设定$ϵ=1.0e^{-6}$，如果$q_n-q_{n-1}<=ϵ$，即认为$q_n$是最优解。

代码分析

初入SLAM（3）——cornerSubPixel源码分析