引言
在立体匹配过程中,我们希望匹配点之间的差距能尽可能小,而初入SLAM——Harris角点检测中,我们接受了使用Opencv获得Harris角点并详细推导了其数学公式。这里的角度坐标是像素坐标,对应的是(整数,整数)。为了获取更精确的像素坐标,我们需要求得亚像素坐标。
Reference
资源文件
原理讲解
这副图片,我相信你各种博客都看到过,但是大部分博客都没有讲清楚为什么。
解答
- q,即待求的亚像素点,很神秘,未知。
- p i p_i pi,即q周围的点,坐标是已知的,自己选择
- ( p i − q ) (p_i-q) (pi−q),即是第一个向量
- p i p_i pi处的灰度, G i G_i Gi,即是第二个向量
我们再看上面的图片,
- p 0 p_0 p0这种情况下,位于一块白色区域,此时,梯度为0
- p 1 p_1 p1这种情况,位于边缘,既黑白相交处,此时,梯度不为0,但是,与 p 1 − q p_1-q p1−q相垂直!
所有对于一个标准的角点,都会导致:
G i ∗ ( p i − q ) = 0 G_i*(p_i-q)=0 Gi∗(pi−q)=0
最小二乘法
对于上面那个方程,我们其实取了很多的 p i p_i pi,那么我们是求不出一个准确的点 q q q满足所有的点的,相当于这是一个无解的方程,那么怎么解一个无解的方程勒?
我们将上面的公式转换一下:
G i ∗ q = G i ∗ p i G_i*q=G_i*p_i Gi∗q=Gi∗pi
我们令 G i : A q : [ x y ] G i ∗ p i : B G_i: A\\q:\left[ \begin{array}{c} x\\ y\\ \end{array} \right] \\ G_i*p_i:B Gi:Aq:[xy]Gi∗pi:B
那么我们就是要求解方程:
A [ x y ] = B A\left[ \begin{array}{c} x\\ y\\ \end{array} \right] =B A[xy]=B
为了更好的具象化,我们给 A A A和 B B B具象化赋值
我们假设 A = [ 1 1 0 1 2 1 ] A=\left[ \begin{array}{c} 1 \ 1\\ 0 \ 1\\ 2 \ 1\\ \end{array} \right] A=⎣
⎡1 10 12 1⎦
⎤
B = [ 2 2 3 ] B=\left[ \begin{array}{c} 2\\ 2\\ 3\\ \end{array} \right] B=⎣
⎡223⎦
⎤
此时我们要求解的方程就是
[ 1 1 0 1 2 1 ] ∗ [ x y ] = [ 2 2 3 ] \left[ \begin{array}{c} 1 \ 1\\ 0 \ 1\\ 2 \ 1\\ \end{array} \right]*\left[ \begin{array}{c} x\\ y\\ \end{array} \right] =\left[ \begin{array}{c} 2\\ 2\\ 3\\ \end{array} \right] ⎣
⎡1 10 12 1⎦
⎤∗[xy]=⎣
⎡223⎦
⎤
从列的角度看
[ 1 0 2 ] × x + [ 1 1 1 ] × y = [ 2 2 3 ] \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] ⎣
⎡102⎦
⎤×x+⎣
⎡111⎦
⎤×y=⎣
⎡223⎦
⎤
我们定义 a 1 = [ 1 0 2 ] a 2 = [ 1 1 1 ] b = [ 2 2 3 ] 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] a1=⎣
⎡102⎦
⎤a2=⎣
⎡111⎦
⎤b=⎣
⎡223⎦
⎤
那么其实我们可以把 a 1 a_1 a1和 a 2 a_2 a2当作基底,我们现在的问题就是找到一组 x , y x,y x,y能够最接近 B B B,画到图上就如下图所示。
按照正常求解,我们是不可能找到一组 a 1 a_1 a1和 a 2 a_2 a2的线性组合,使得组合后的向量刚好等于 B B B,因为任何 a 1 a_1 a1和 a 2 a_2 a2的线性组合只能在 a 1 , a 2 a_1,a_2 a1,a2所在的平面上。
既然找不到完美的解,那么我们就只能找一个最接近的解,而这个解就是 B B B在 a 1 , a 2 a_1,a_2 a1,a2平面上的投影,垂足就是最接近解的终点与准确解之间的误差。如下图所示。
现在我们就是要求解 A [ x ^ y ^ ] = P A\left[ \begin{array}{c} \hat{x}\\ \hat{y}\\ \end{array} \right]=P A[x^y^]=P,而这个一定是有解的。
另外,我们知道, P P P与 b b b之间的误差为: e = B − P = B − A [ x ^ y ^ ] e=B-P=B-A \left[ \begin{array}{c} \hat{x}\\ \hat{y}\\ \end{array} \right] e=B−P=B−A[x^y^]
要想使 b b b与 P P P之间的差距最小,那么e一定是垂直于 a 1 , a 2 a_1,a_2 a1,a2组成的平面S的,也就是要垂直于相交向量 a 1 , a 2 a_1,a_2 a1,a2,所有我们就可以得出要求 e ∗ a 1 = 0 和 e ∗ a 2 = 0 e*a_1=0和e*a_2=0 e∗a1=0和e∗a2=0,用矩阵表示就是:
A T e = 0 A^{T} e=0 ATe=0
代入 e e e可得:
A T ( B − A [ x ^ y ^ ] ) = 0 A T A [ x ^ y ^ ] = A T B [ x ^ y ^ ] = ( A T A ) − 1 A T B 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 AT(B−A[x^y^])=0ATA[x^y^]=ATB[x^y^]=(ATA)−1ATB
至此,我们就求出来了近似解 x ^ \hat{x} x^。
拉回到原来的公式
G i ∗ q = G i ∗ p i G_i*q=G_i*p_i Gi∗q=Gi∗pi
我们令 G i : A q : [ x y ] G i ∗ p i : B G_i: A\\q:\left[ \begin{array}{c} x\\ y\\ \end{array} \right] \\ G_i*p_i:B Gi:Aq:[xy]Gi∗pi:B
通过上面的定义和公式可以推出:
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 q=(G_i^TG_i)^{-1}(G_i^T)G_ip_i\\ =(G_i^TG_i)^{-1}(G_i^TG_i)p_i q=(GiTGi)−1(GiT)Gipi=(GiTGi)−1(GiTGi)pi
那么此时,我们已经能够通过多个点求得一个点的坐标了。
权重引入
但是这就一定准确吗?我们采用多点进行计算,本意是为了更准确,但各点离中心距离不一,所以补可一视同仁,要引入权重,一般采用高斯权重。假设 p i p_i pi处权重为 w i w_i wi,上式进一步修正为:
q = ( G i T G i w i ) − 1 ( G i T G i w i ) p i q=(G_i^TG_iw_i)^{-1}(G_i^TG_iw_i)p_i q=(GiTGiwi)−1(GiTGiwi)pi
迭代和终止条件
求解一次后,即可得到一个亚像素点 q ( q x , q y ) q(q_x,q_y) q(qx,qy)。如果以 q q q为中心点,再次:
1.选取窗口,得到新的一组 p i p_i pi
2.对 p i p_i pi求梯度
3.用最小二乘法求解
即得到新的点 q i q_i qi。
如此多迭代次数,会得到一系列亚像素点 q 2 , q 3 , q 4 , . . . . q n q_2,q_3,q_4,....q_n q2,q3,q4,....qn。那么什么时候终止呢?
OpenCV的做法是:
指定迭代次数,比如,迭代10次后,不再进行计算,认为得到最优解。
指定结果精度,比如,设定 ϵ = 1.0 e − 6 \epsilon=1.0e^{-6} ϵ=1.0e−6,如果 q n − q n − 1 < = ϵ q_n-q_{n-1}<=\epsilon qn−qn−1<=ϵ,即认为 q n q_n qn是最优解。