初入SLAM(1)——Harris角点检测

Reference

https://blog.csdn.net/lbaihao/article/details/78919700
https://docs.opencv.org/4.x/dc/d0d/tutorial_py_features_harris.html

1 数学推导

Harris角点检测原理:

Harris算法利用窗口内图像灰度的自相关性进行角点判定，设置一个窗口,并在图像中移动,计算移动前与移动后窗口所在区域图像的自相关系数。

自相关函数计算如下，(x,y)为窗口中心位置，w(u,v)为权重（一般取高斯函数），L表示窗口，(u,v)表示窗口中的图像位置。
$$\mathrm{c}(\mathrm{x},\mathrm{y},\Delta \mathrm{x},\Delta \mathrm{y})=\sum _{(\mathrm{u},\mathrm{v})\in \mathrm{L}}\mathrm{w}(\mathrm{u},\mathrm{v})\cdot [\mathrm{I}(\mathrm{u},\mathrm{v})-\mathrm{I}(\mathrm{u}+\Delta \mathrm{x},\mathrm{v}+\Delta \mathrm{y}){]}^2$$
对其中的I二元函数进行泰勒展开：
$$\begin{array}{c}\mathrm{I}(\mathrm{u}+\Delta \mathrm{x},\mathrm{v}+\Delta \mathrm{y})=\mathrm{I}(\mathrm{u},\mathrm{v})+{\mathrm{I}}_{\mathrm{x}}(\mathrm{u},\mathrm{v})\cdot \Delta \mathrm{x}+{\mathrm{I}}_{\mathrm{y}}(\mathrm{u},\mathrm{v})\cdot \Delta \mathrm{y}+\cdots +{\mathrm{R}}_{\mathrm{n}}(\mathrm{u},\mathrm{v})\\ \approx \mathrm{I}(\mathrm{u},\mathrm{v})+{\mathrm{I}}_{\mathrm{x}}(\mathrm{u},\mathrm{v})\cdot \Delta \mathrm{x}+{\mathrm{I}}_{\mathrm{y}}(\mathrm{u},\mathrm{v})\cdot \Delta \mathrm{y}\end{array}$$
代入自相关函数，得： c ( x , y , Δ x , Δ y ) = ∑ ( u , v ) ϵ L w ( u , v ) ⋅ [ I ( u , v ) − I ( u + Δ x , v + Δ y ) ] 2 ≈ ∑ ( u , v ) ∈ L w ( u , v ) ⋅ { I ( u , v ) − [ I ( u , v ) + I x ( u , v ) ⋅ Δ x + I y ( u , v ) ⋅ Δ y ] } 2 = ∑ ( u , v ) ∈ L w ( u , v ) [ I x ( u , v ) ⋅ Δ x + I y ( u , v ) ⋅ Δ y ] 2 \begin{aligned} \mathrm{c}(\mathrm{x}, \mathrm{y}, \Delta \mathrm{x}, \Delta \mathrm{y}) &=\sum_{(\mathrm{u}, \mathrm{v}) \epsilon \mathrm{L}} \mathrm{w}(\mathrm{u}, \mathrm{v}) \cdot[\mathrm{I}(\mathrm{u}, \mathrm{v})-\mathrm{I}(\mathrm{u}+\Delta \mathrm{x}, \mathrm{v}+\Delta \mathrm{y})]^{2} \\ & \approx \sum_{(\mathrm{u}, \mathrm{v}) \in \mathrm{L}} \mathrm{w}(\mathrm{u}, \mathrm{v}) \cdot\left\{\mathrm{I}(\mathrm{u}, \mathrm{v})-\left[\mathrm{I}(\mathrm{u}, \mathrm{v})+\mathrm{I}_{\mathrm{x}}(\mathrm{u}, \mathrm{v}) \cdot \Delta \mathrm{x}+\mathrm{I}_{\mathrm{y}}(\mathrm{u}, \mathrm{v}) \cdot \Delta \mathrm{y}\right]\right\}^{2} \\ &=\sum_{(\mathrm{u}, \mathrm{v}) \in \mathrm{L}} \mathrm{w}(\mathrm{u}, \mathrm{v})\left[\mathrm{I}_{\mathrm{x}}(\mathrm{u}, \mathrm{v}) \cdot \Delta \mathrm{x}+\mathrm{I}_{\mathrm{y}}(\mathrm{u}, \mathrm{v}) \cdot \Delta \mathrm{y}\right]^{2} \end{aligned} c(x,y,Δx,Δy)​=(u,v)ϵL∑​w(u,v)⋅[I(u,v)−I(u+Δx,v+Δy)]2≈(u,v)∈L∑​w(u,v)⋅{ I(u,v)−[I(u,v)+Ix​(u,v)⋅Δx+Iy​(u,v)⋅Δy]}2=(u,v)∈L∑​w(u,v)[Ix​(u,v)⋅Δx+Iy​(u,v)⋅Δy]2​
将平方项展开并写出矩阵形式，得：
$$\begin{array}{l}{\left[I_x(u,v)\cdot \Delta x+I_y(u,v)\cdot \Delta y\right]}^2\\ =I^2{}_x(u,v)\Delta x^2+I^2{}_y(u,v)\Delta y^2+2I_x(u,v)I_y(u,v)\Delta x\Delta y\\ =I^2{}_x(u,v)\Delta x^2+I^2{}_y(u,v)\Delta y^2+2I_x(u,v)I_y(u,v)\Delta x\Delta y\\ =\left[\begin{array}{lc}{I}_x^2(u,v)\Delta x+I_x(u,v)I_y(u,v)\Delta y& I_y^2(u,v)\Delta y+I_x(u,v)I_y(u,v)\Delta x\end{array}\right]\cdot \left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\\ =\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\cdot \left[\begin{array}{cc}{I}_x^2(u,v)& I_x(u,v)I_y(u,v)\\ I_x(u,v)I_y(u,v)& I_y^2(u,v)\end{array}\right]\cdot \left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\end{array}$$

代入自相关函数，得：
$$\begin{array}{l}c(x,y,\Delta x,\Delta y)\approx {\sum }_{(u,v)\in L}w(x,y){\left[I_x(u,v)\cdot \Delta x+I_y(u,v)\cdot \Delta y\right]}^2\\ \\ ={\sum }_{(u,v)\in L}w(x,y)[I^2{}_x(u,v)\Delta x^2+I^2{}_y(u,v)\Delta y^2+2I_x(u,v)I_y(u,v)\Delta x\Delta y]\\ \\ ={\sum }_{(u,v)\in L}w(x,y)\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\left[\begin{array}{cc}{I}_x^2(u,v)& I_x(u,v)I_y(u,v)\\ I_x(u,v)I_y(u,v)& I_y^2(u,v)\end{array}\right]\left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\\ \\ =\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\left\{{\sum }_{(u,v)\in L}w(x,y)\left[\begin{array}{cc}{I}_x^2(u,v)& I_x(u,v)I_y(u,v)\\ I_x(u,v)I_y(u,v)& I_y^2(u,v)\end{array}\right]\right\}\left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\\ \\ =\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\left[\begin{array}{cc}{\sum }_{(u,v)ϵL}{I}_x^2(u,v)& {\sum }_{(u,v)ϵL}{I}_x(u,v)I_y(u,v)\\ {\sum }_{(u,v)\in L}{I}_x(u,v)I_y(u,v)& {\sum }_{(u,v)ϵL}{I}_y^2(u,v)\end{array}\right]\left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\\ \\ =\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\cdot \left[\begin{array}{ll}A& C\\ C& B\end{array}\right]\cdot \left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\end{array}$$

最终我们得到的公式为：
$$\begin{array}{l}c(x,y,\Delta x,\Delta y)=\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\cdot M\cdot \left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\end{array}$$
$$M=\sum _{x,y}w(x,y)\left[\begin{array}{cc}{I}_x{I}_x& I_x{I}_y\\ I_x{I}_y& I_y{I}_y\end{array}\right]$$
这里$I_x$和$I_y$是图像在x和y方向上的图像导数（使用cv2.Sobel()可以很容易得到它们）

椭圆的方程式

$$\frac{x^2}{a^2}+\frac{y^2}{b^2}=1$$
矩阵表达：
$$\left[\begin{array}{ll}x& y\end{array}\right]\left[\begin{array}{ll}\frac{1}{a^2}& 0\\ 0& \frac{1}{b^2}\end{array}\right]\left[\begin{array}{l}x\\ y\end{array}\right]=1$$
一个nxn的矩阵，可以求解其特征值，我们对上述中间矩阵求解特征值，可以得到
$$\{\begin{array}{l}{\lambda }_1=\frac{1}{a^2}\\ {\lambda }_2=\frac{1}{b^2}\end{array}\{\begin{array}{l}a=\frac{1}{\sqrt{{\lambda }_1}}\\ b=\frac{1}{\sqrt{{\lambda }_2}}\end{array}$$
特征值越大，轴半径越短。

M矩阵

$$\begin{array}{l}c(x,y,\Delta x,\Delta y)=\left[\begin{array}{ll}\Delta x& \Delta y\end{array}\right]\cdot M\cdot \left[\begin{array}{l}\Delta x\\ \Delta y\end{array}\right]\end{array}$$
$$M=\sum _{x,y}w(x,y)\left[\begin{array}{cc}{I}_x{I}_x& I_x{I}_y\\ I_x{I}_y& I_y{I}_y\end{array}\right]$$
我们发现自相关函数其实就是一个椭圆的矩阵表示。
那么，我们可以通过求解$M$矩阵的特征值得到${\lambda }_1$和${\lambda }_2$。

1. ${\lambda }_1$和${\lambda }_2$都特别小，轴长都很大，此时表示点在平坦区域。
2. ${\lambda }_1$和${\lambda }_2$都特别大，而且比较接近，轴长都很小，可以认为是角点。
3. ${\lambda }_1$和${\lambda }_2$其中一个特别小，一个特别大，一个轴长，一个轴短，可以认为此时是边缘区域。
   

如果我们按照上面的方法，很明显要对每一个点都计算很多东西，速度太慢，因此提出了一个经验公式。
$$R=det(M)-k(trace(M){)}^2$$
其中，

• $der(M)={\lambda }_1\cdot {\lambda }_2=AB-C^2$
• $trace(M)={\lambda }_1+{\lambda }_2=A+B$
• ${\lambda }_1$和${\lambda }_2$是$M$的特征值
• $k$是一个经验值，一般取0.02-0.04

2 Opencv 实现

2.1 函数 cv2.cornerHarris()【粗略了解就行】

2.1.1 参数

• img：输入图像，应该是float32的灰度图像
• blockSize： 角点检测所考虑的领域大小
• ksize： 使用Sobel导数的孔径参数
• k：Harris检测分数$R$中$k$的参数值

# -*-coding:utf-8-*-
import numpy as np
import cv2

filename = '1.jpg'
img = cv2.imread(filename)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

# opencv需要输入是float32类型的
gray = np.float32(gray)
# 返回值是一个跟图片同样大小的矩阵分数图
dst = cv2.cornerHarris(gray, 5, 3, 0.04)

# 通过图像自身的分数图的大小关系，得到哪些像素点应该是角点，
# 这些像素点的值都变为[0,0,255]，也就是红色。
img[dst>0.01*dst.max()]=[0,0,255]
cv2.imshow('dst', img)
cv2.waitKey(0)

2.2 cv2.goodFeaturesToTrack()

上面那个api的问题在于返回的不是角点的位置坐标，而是一个分数图，我们又直接用分数图处理了源图片。
cv2.goodFeaturesToTrack()，不仅支持Harris角点检测，也支持Shi Tomasi算法的角点检测。

2.2.1 参数

• gray_src：8位或者32位单通道灰度图像
• corners：位置点向量，保存的是检测到的角点的坐标
• max_corners：定义为可以检测到的角点的数量的最大值；
• qualityLevel：检测到的角点的质量等级，角点特征值小于qualityLevel最大特征值的点将被舍弃；
• minDistance：两角点间最小间距，以像素为单位；
• mask：指定检测区域，若检测整幅图像，则mask为空
• blockSize：计算自相关函数协方差矩阵的窗口大小；
• useHarrisDetector：是否使用Harris角点检测，为false，则使用Shi-Tomasi算子
• k：Harris角点检测的超参数，一般取0.02到0.04，userHarrisDetector为false，则不起作用。

# -*-coding:utf-8-*-
import numpy as np
import cv2

filename = '1.jpg'
img = cv2.imread(filename)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

gray = np.float32(gray)


corners = cv2.goodFeaturesToTrack(gray, maxCorners=1000, qualityLevel=0.01, minDistance=10)
corners = np.int0(corners)
for i in corners:
    x,y = i.ravel()
    cv2.circle(img, (x, y), 2, (0, 0, 255), -1)

cv2.imshow('out.jpg', img)

cv2.waitKey(0)

亚像素

cv2.cornerSubPix()

参数：

• image：输入图像
• corners：输入角点的初始坐标
• winSize：搜索窗口边长的一半，如果winSize=(5,5)，则一个大小为（52+1）（52+1）=1111的搜索窗口将被使用
• zeroZone：搜索区域中间的dead region边长的一半。如果值设置为（-1，-1）则表示没有这个区域
• cirteria：角点精准化迭代过程的终止条件，一般使用$criteria=(cv2.TermCriteri{a}_{E}PS+cv2.TermCriteri{a}_{M}A{X}_{I}TER,100,0.001)$