First entry to SLAM (2) - use the least square method to find sub-pixel coordinates

introduction

In the process of stereo matching, we hope that the gap between matching points can be as small as possible. When we first entered SLAM-Harris corner detection , we accepted the use of Opencv to obtain Harris corners and deduced its mathematical formula in detail. The angular coordinates here are pixel coordinates corresponding to (integer, int). In order to obtain more accurate pixel coordinates, we need to obtain sub-pixel coordinates.

Reference

• least square method
• First entry to SLAM - Harris corner detection
• Opencv - sub-pixel corner cornerSubPixel () source code analysis

resource

• cornerSubPixel() explanation

Principle explanation

I believe you have seen this picture on various blogs, but most of them did not explain why.

answer

• q, which is the sub-pixel to be requested, is very mysterious and unknown.
• $p_i$, that is, the points around q, the coordinates are known, choose by yourself
• $(p_i-q)$ , which is the first vector
• $p_i$The gray level at $G_i$, which is the second vector

Let's look at the picture above again,

1. $p_0$In this case, it is located in a white area, at this time, the gradient is 0
2. $p_1$In this case, it is located at the edge, where black and white intersect. At this time, the gradient is not 0, but, with $p_1-q$ is vertical!

All for a standard corner, will lead to:
$$G_i\ast (p_i-q)=0$$

least square method

For the above equation, we actually took a lot of $p_i$, then we cannot find an exact point $If\; q$ satisfies all points, it is equivalent to an equation with no solution, sohow to solve an equation with no solution?
Let's convert the above formula:
$$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}$$
Then we want to solve the equation:
$$A\left[\begin{array}{c}x\\ y\end{array}\right]=B$$
For better visualization, we give$A$ and$B$ Concrete assignment
We assume$$A=\left[\begin{array}{c}11\\ 01\\ 21\end{array}\right]$$
$$B=\left[\begin{array}{c}2\\ 2\\ 3\end{array}\right]$$

At this time, the equation we want to solve is
$$\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]$$
The default value is
$$\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]$
So in fact we can put $a_1$和$a_2$As a basis, our problem now is to find a set of $x,y$ can be closest to$B$ , drawn on the graph as shown in the figure below.

According to the normal solution, it is impossible for us to find a set of$a_1$和$a_2$The linear combination of , so that the combined vector is just equal to $B$ , since any$a_1$和$a_2$The linear combination can only be in $a_1,a_2$on the plane where it is located.
Since no perfect solution can be found, we can only find the closest solution, and this solution is $B$ in$a_1,a_2$Projected onto a plane, the foot is the error between the endpoint closest to the solution and the exact solution. As shown below.

Now we just want to solve $A\left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right]=P$ , and this must have a solution.
Also, we know that$P$ andbbThe error between$b$$$e=B-P=B-A\left[\begin{array}{c}\hat{x}\\ \hat{y}\end{array}\right]$$
to make$b$ andPPThe gap between$P$$a_1,a_2$Composed of the plane S, that is, to be perpendicular to the intersection vector $a_1,a_2$, so we can draw the requirements $e\ast a_1=0ande\ast a_2=0$ , represented by matrix:
$$A^{That\text{'}s}it=$$
Enter$$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}$$
So far, we have found an approximate solution$\hat{x}$。

Pull back to the original formula

$$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}$$

From the above definitions and formulas, it can be deduced that:
$$\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}$$
Then at this time, we have been able to obtain the coordinates of a point through multiple points.

weight introduction

But is this necessarily accurate? We use multiple points for calculation. The original intention is to be more accurate, but the distance from each point to the center is different, so the compensation can be treated equally. To introduce weights, Gaussian weights are generally used. Suppose $p_i$where the weight is $w_i$, the above formula is further revised as:
$$q=(G_i^T{G}_i{w}_i{)}^{-1}(G_i^T{G}_i{w}_i)p_i$$

Iteration and termination conditions

After solving once, you can get a sub-pixel point $q(q_x,q_y)$ . if$q$ is the center point, again:
1. Select the window to get a new set of$p_i$
2. To $p_i$Find the gradient
3. Solve it with the least square method
to get the new point $q_i$.
With so many iterations, a series of sub-pixel points $q_2,q_3,q_4,....q_n$. So when does it end?
OpenCV's method is:
specify the number of iterations, for example, after 10 iterations, no calculation is performed, and the optimal solution is considered.
Specify the result precision, for example, set $ϵ=1.0e^{-6}$ ,and$q_n-q_{n-1}<=ϵ$ , that is, consider$q_n$is the optimal solution.

code analysis

First entry to SLAM (3) - cornerSubPixel source code analysis