TinyGrape Lab
EN/中文

NOTE / 4/7/2019

[ORB-SLAM2] Monocular Initialization

SLAMTechnical NotesSLAMVIOSensor Fusion

ORB-SLAM proposes an automatic initialization procedure that selects Homography or Fundamental models according to the scene and delays initialization when quality is inadequate. This is a working note on its details.

1. Initialization flow

Step 0. Select a reference frame and extract ORB features

The reference frame needs more than 100 extracted ORB features.

Step 1. Match current-frame and reference-frame ORB features

  1. The current frame also needs more than 100 features; otherwise return to Step 0.
  2. Use the DBoW2 bag of words to accelerate matching.
  3. If fewer than 100 matches are found, return to Step 0.
  4. Otherwise initialize.

Step 2. Estimate Homography and Fundamental models concurrently

For Homography,

xc=Hcrxr.(1)\mathbf x_c=\mathbf H_{cr}\mathbf x_r. \tag{1}

RANSAC uses four correspondences per hypothesis.

For Fundamental matrix,

xcTFxr=0.(2)\mathbf x_c^{\mathsf T}\mathbf F\mathbf x_r=0. \tag{2}

RANSAC uses eight correspondences per hypothesis.

The RANSAC iteration count is fixed. In every iteration, both models receive a score:

SM=∑i[ρM(dcr2(xci,xri,M))+ρM(drc2(xci,xri,M))],ρM(d2)={Γ−d2,d2<TM,0,d2≥TM.(3)\begin{aligned} S_M&=\sum_i\left[ \rho_M(d_{cr}^2(\mathbf x_c^i,\mathbf x_r^i,M))+ \rho_M(d_{rc}^2(\mathbf x_c^i,\mathbf x_r^i,M)) \right],\\ \rho_M(d^2)&= \begin{cases} \Gamma-d^2,&d^2<T_M,\\ 0,&d^2\ge T_M. \end{cases} \end{aligned} \tag{3}

MM is H\mathbf H or F\mathbf F. TMT_M comes from a 95% chi-squared test: TH=5.99T_H=5.99 for two degrees of freedom and TF=3.84T_F=3.84 for one degree of freedom, assuming one-pixel standard deviation. The original ORB-SLAM implementation sets Γ=TH\Gamma=T_H.

After all iterations, retain the highest-scoring H\mathbf H and F\mathbf F.

Step 3. Select Homography or Fundamental

RH=SHSH+SF.(4)R_H=\frac{S_H}{S_H+S_F}. \tag{4}

Use Homography if RH>0.45R_H>0.45; otherwise use Fundamental.

Steps 4–5. Recover pose, map points, and run BA

Recover relative pose and 3D map points from the selected model, then run bundle adjustment.

The main technical questions are how to estimate H\mathbf H and F\mathbf F, and how to decompose each into rotation R\mathbf R and translation t\mathbf t.

2. Homography matrix

In the first camera frame, a 3D point is P=[X,Y,Z]T\mathbf P=[X,Y,Z]^{\mathsf T}. If the points lie on a plane,

nTP+d=0,−nTPd=1.(5–6)\mathbf n^{\mathsf T}\mathbf P+d=0, \qquad -\frac{\mathbf n^{\mathsf T}\mathbf P}{d}=1. \tag{5--6}

Projecting P\mathbf P into the second frame yields

s2p2=K(RP+t)=K(R−tnTd)P=K(R−tnTd)s1K−1p1.\begin{aligned} s_2\mathbf p_2 &=\mathbf K(\mathbf R\mathbf P+\mathbf t)\\ &=\mathbf K\left(\mathbf R-\frac{\mathbf t\mathbf n^{\mathsf T}}d\right)\mathbf P\\ &=\mathbf K\left(\mathbf R-\frac{\mathbf t\mathbf n^{\mathsf T}}d\right) s_1\mathbf K^{-1}\mathbf p_1. \end{aligned}

Since image points are homogeneous, H\mathbf H is defined up to scale:

p2=sHp1,H=K(R−tnTd)K−1.(7–8)\mathbf p_2=s\mathbf H\mathbf p_1, \qquad \mathbf H=\mathbf K\left(\mathbf R-\frac{\mathbf t\mathbf n^{\mathsf T}}d\right)\mathbf K^{-1}. \tag{7--8}

Expanding the relation gives

h11u1+h12v1+h13−u1u2h31−v1u2h32−u2h33=0,h21u1+h22v1+h23−u1v2h31−v1v2h32−v2h33=0.(9)\begin{aligned} h_{11}u_1+h_{12}v_1+h_{13}-u_1u_2h_{31}-v_1u_2h_{32}-u_2h_{33}&=0,\\ h_{21}u_1+h_{22}v_1+h_{23}-u_1v_2h_{31}-v_1v_2h_{32}-v_2h_{33}&=0. \end{aligned} \tag{9}

Solution 1

Set h33=1h_{33}=1, giving eight unknowns. Each correspondence gives two equations; four correspondences solve H\mathbf H. With more matches, solve the overdetermined system

Mx=b\mathbf M\mathbf x=\mathbf b

by least squares, for example

x=(MTM)−1MTborx=R−1QTb\mathbf x=(\mathbf M^{\mathsf T}\mathbf M)^{-1}\mathbf M^{\mathsf T}\mathbf b \quad\text{or}\quad \mathbf x=\mathbf R^{-1}\mathbf Q^{\mathsf T}\mathbf b

from QR decomposition.

Solution 2

Keep all nine parameters and solve

Mx=0.\mathbf M\mathbf x=0.

The solution is x=ηξ\mathbf x=\eta\boldsymbol\xi, another up-to-scale form. ξ\boldsymbol\xi is the right singular vector associated with the smallest singular value of M\mathbf M, or the eigenvector of MTM\mathbf M^{\mathsf T}\mathbf M with its smallest eigenvalue. ORB-SLAM uses this form and estimates Homography from eight matches during RANSAC to remain consistent with Fundamental estimation.

ORB-SLAM also normalizes 2D features to zero mean and unit variance before Homography computation, improving numerical conditioning.

3. Fundamental matrix

For the same 3D point observed by two cameras,

s1p1=KP,s2p2=K(RP+t).(11)s_1\mathbf p_1=\mathbf K\mathbf P, \qquad s_2\mathbf p_2=\mathbf K(\mathbf R\mathbf P+\mathbf t). \tag{11}

Define normalized coordinates xi=K−1pi\mathbf x_i=\mathbf K^{-1}\mathbf p_i. Then

s2x2=s1Rx1+t.(12)s_2\mathbf x_2=s_1\mathbf R\mathbf x_1+\mathbf t. \tag{12}

Crossing with t\mathbf t gives the epipolar constraint

x2T[t]×R⏟Ex1=0,(13)\mathbf x_2^{\mathsf T} \underbrace{[\mathbf t]_\times\mathbf R}_{\mathbf E} \mathbf x_1=0, \tag{13}

where E\mathbf E is the essential matrix. Reintroducing pixel coordinates gives

p2TK−T[t]×RK−1⏟F=K−TEK−1p1=0.(14)\mathbf p_2^{\mathsf T} \underbrace{\mathbf K^{-\mathsf T}[\mathbf t]_\times\mathbf R\mathbf K^{-1}}_{ \mathbf F=\mathbf K^{-\mathsf T}\mathbf E\mathbf K^{-1}} \mathbf p_1=0. \tag{14}

F\mathbf F is up to scale and hence has eight degrees of freedom. One correspondence contributes one equation, so eight pairs solve it. Expanding Equation (14) is linear in the nine elements of F\mathbf F; solve Mx=0\mathbf M\mathbf x=0 with the same SVD approach as Homography.

4. Scoring models

For Homography, use squared Mahalanobis distance between corresponding points:

x2′=Hx1,d2=(x2−x2′)TΣ−1(x2−x2′).\mathbf x_2'=\mathbf H\mathbf x_1, \qquad d^2=(\mathbf x_2-\mathbf x_2')^{\mathsf T} \boldsymbol\Sigma^{-1} (\mathbf x_2-\mathbf x_2').

This follows a two-degree-of-freedom chi-squared distribution, so Equation (3) uses Γ=TH=5.99\Gamma=T_H=5.99.

For Fundamental matrix, use squared point-to-epipolar-line distance. The line in camera 2 is

x2TFx1⏟n=0,\mathbf x_2^{\mathsf T}\underbrace{\mathbf F\mathbf x_1}_{\mathbf n}=0,

and the squared distance is

d2=(x2TFx1)2(Fx1)12+(Fx1)22.d^2= \frac{(\mathbf x_2^{\mathsf T}\mathbf F\mathbf x_1)^2} {(\mathbf F\mathbf x_1)_1^2+(\mathbf F\mathbf x_1)_2^2}.

Under unit-pixel Gaussian noise it follows one-degree-of-freedom chi-squared, hence TF=3.84T_F=3.84. TMT_M separates inliers from outliers.

5. Decompose Fundamental matrix into rotation and translation

Given F\mathbf F,

E=KTFK.\mathbf E=\mathbf K^{\mathsf T}\mathbf F\mathbf K.

The essential matrix is

E=s[t]×R,\mathbf E=s[\mathbf t]_\times\mathbf R,

and has five degrees of freedom. Let

W=[0−10100001],E=UΣVT.\mathbf W= \begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}, \qquad \mathbf E=\mathbf U\boldsymbol\Sigma\mathbf V^{\mathsf T}.

The four candidates are

R=UWVT,t=−U[0,0,1]T,R=UWVT,t=−U[0,0,1]T,R=UWTVT,t=−U[0,0,1]T,R=UWTVT,t=−U[0,0,1]T.\begin{aligned} \mathbf R&=\mathbf U\mathbf W\mathbf V^{\mathsf T}, &\mathbf t&=\phantom{-}\mathbf U[0,0,1]^{\mathsf T},\\ \mathbf R&=\mathbf U\mathbf W\mathbf V^{\mathsf T}, &\mathbf t&=-\mathbf U[0,0,1]^{\mathsf T},\\ \mathbf R&=\mathbf U\mathbf W^{\mathsf T}\mathbf V^{\mathsf T}, &\mathbf t&=\phantom{-}\mathbf U[0,0,1]^{\mathsf T},\\ \mathbf R&=\mathbf U\mathbf W^{\mathsf T}\mathbf V^{\mathsf T}, &\mathbf t&=-\mathbf U[0,0,1]^{\mathsf T}. \end{aligned}

Choose by triangulation: points must lie in front of both cameras, have low reprojection error, sufficient parallax, and yield a clearly unique best candidate. If multiple candidates remain equally good, reject this initialization attempt.

6. Recover pose from Homography

ORB-SLAM produces eight Homography pose candidates, then selects one using triangulated-point count and parallax. It accepts only when:

  1. the best count is more than 0.70.7 times the runner-up count;
  2. best parallax exceeds the minimum;
  3. the best count exceeds the configured minimum;
  4. the best count exceeds 0.90.9 times the match count.