Vins estimator 5-point method

Polar geometry

• In the first frame coordinate system , Set up space ：$P=[x,y,z{]}^T$

• Pinhole based camera , The position of the two pixels $s_1{p}_1=KP$ and $s_2{p}_2=K(RP+t)$

  • among ,$R,t$ Is the motion of two coordinate systems
  • If you use Homogeneous coordinates , be ：$p_1=KP$ and p_2 = K(RP+t)$

• take $x_1=K^{-1}{p}_1$ ,$x_1=K^{-1}{p}_2$

  • Then there are ：$x_2=R{x}_1+t$
  • Multiply both sides at the same time $t^{\wedge }$, Yes ：$t^{\wedge }{x}_2=t^{\wedge }R{x}_1$
  • Multiply both sides at the same time $x_2^T$, Yes ：$x_2^T{t}^{\wedge }{x}_2=x_2^T{t}^{\wedge }R{x}_1$
  • because $t^{\wedge }{x}_2$ Is with the $t,x_2$ Vectors that are all vertical , Then the coordinates are 0, Yes ：$x_2^T{t}^{\wedge }R{x}_1=0$

• obtain ：$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$

  • This formula is a contrapolar geometric constraint

  • $E=t^{\wedge }R$ Is the essential matrix

  • $F=K^{-T}E{K}^{-1}$ Based on the matrix

  • $p_2^{T}F{p}_1=0$

• because $E,F$ Only the internal parameters of the camera are different , The internal reference is usually in slam It's known that , So in practice, it is easier to use $E$.

5 Point method

• Constraint solving ：
  • First, constrain the polar geometry ：$p_2^{T}Fp=0$
  • The basic matrix can be considered without knowing the calibration matrix , Besides , if $k_1,k_2$ It is known that , Call the camera calibrated , Again , Suppose the point on the image $p,p_2$ Already booked , So the antipolar Geometry ：$p_2^{T}Ep=0$
  • If the $E$ Of 9 Member variables are fully expanded , We get a linear equation , Every match point , A linear equation can be obtained ,5 Click to get 5 An equation .
  • but $E$ yes $3\times 3$ Matrix , Yes 9 Member variables , Only 5 A linear equation , So we get a linear solution space , This solution space has 4 A degree of freedom .
  • So we get the linear solution space ：$E=xX+yY+zZ+wW$

// Todo