Overview

• The gray region is the epipolar plane
• The orange line is the baseline
• the two blue lines are the epipolar lines

Basic Epipolar Geometry entities for pinhole cameras and panoramic camera sensors

Foundamental Matrix (基本矩阵)

$\boldsymbol{F} = \boldsymbol{K}'^{-T} \boldsymbol{E} \boldsymbol{K}^{-1}$F=K′−TEK−1

$\boldsymbol{p}'^T \cdot \boldsymbol{F} \cdot \boldsymbol{p} = 0$p′T⋅F⋅p=0

其中，$\boldsymbol{p}, \boldsymbol{p}'$p,p′ 为两个匹配 像素点坐标

$\boldsymbol{F}$F 的性质：

• 对其乘以 任意非零常数，对极约束依然满足（尺度等价性，up to scale）
• 具有 7个自由度： 2x11-15=7 (why)
• 奇异性约束 $\text{rank}(\boldsymbol{F})=2$rank(F)=2

$\boldsymbol{F}$F 与 极线和极点 的关系：

$\boldsymbol{l}' = \boldsymbol{F} \cdot \boldsymbol{p} \\[2ex] \boldsymbol{l} = \boldsymbol{F}^T \cdot \boldsymbol{p}' \\[2ex] \boldsymbol{F} \cdot \boldsymbol{e} = \boldsymbol{F}^T \cdot \boldsymbol{e}' = 0$l′=F⋅pl=FT⋅p′F⋅e=FT⋅e′=0

$\boldsymbol{F}$F 的计算：

• Compute from 7 image point correspondences
• 8点法（Eight-Point Algorithm）

Foundamental Matrix Estimation

• 类似于下面 $\boldsymbol{E}$E 的估计

Essential Matrix (本质矩阵)

• A 3×3 matrix is an essential matrix if and only if two of its singular values are equal, and the third is zero

对极约束：

$\boldsymbol{E} = \boldsymbol{t}^\wedge \boldsymbol{R} \\[2ex] \boldsymbol{E} = \boldsymbol{K}'^{T} \boldsymbol{F} \boldsymbol{K} \\[2ex] {\boldsymbol{p}'}^T \cdot \boldsymbol{E} \cdot \boldsymbol{p} = 0$E=t∧RE=K′TFKp′T⋅E⋅p=0

其中，$\boldsymbol{p}, \boldsymbol{p}'$p,p′ 为两个匹配像素点的 归一化平面坐标

$\boldsymbol{E}$E 的性质：

• 对其乘以 任意非零常数，对极约束依然满足（尺度等价性，up to scale）
• 根据 $\boldsymbol{E} = \boldsymbol{t}^\wedge \boldsymbol{R}$E=t∧R，$\boldsymbol{E}$E 的奇异值必定是 $[\sigma, \sigma, 0]^T$[σ,σ,0]T 的形式
• 具有 5个自由度：平移旋转共6个自由度 + 尺度等价性
• 奇异性约束 $\text{rank}(\boldsymbol{E})=2$rank(E)=2（因为 $\text{rank}(\boldsymbol{t^\wedge})=2$rank(t∧)=2）

$\boldsymbol{E}$E 与 极线和极点 的关系：

$\boldsymbol{l}' = \boldsymbol{E} \cdot \boldsymbol{p} \\[2ex] \boldsymbol{l} = \boldsymbol{E}^T \cdot \boldsymbol{p}' \\[2ex] \boldsymbol{E} \cdot \boldsymbol{e} = \boldsymbol{E}^T \cdot \boldsymbol{e}' = 0$l′=E⋅pl=ET⋅p′E⋅e=ET⋅e′=0

$\boldsymbol{E}$E 的计算：

• 5点法（最少5对点求解）
• 8点法（Eight-Point Algorithm）

Essential Matrix Estimation

矩阵形式：

$\begin{bmatrix} x' & y' & 1 \end{bmatrix} \cdot \begin{bmatrix} e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9} \end{bmatrix} \cdot \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = 0$[x′​y′​1​]⋅⎣⎡​e1​e4​e7​​e2​e5​e8​​e3​e6​e9​​⎦⎤​⋅⎣⎡​xy1​⎦⎤​=0

矩阵 $\boldsymbol{E}$E 展开，写成向量形式 $\boldsymbol{e}$e，并把所有点（n对点，n>=8）放到一个方程中，齐次线性方程组 如下：

$\begin{bmatrix} x'^1x^1 & x'^1y^1 & x'^1 & y'^1x^1 & y'^1y^1 & y'^1 & x^1 & y^1 & 1 \\ x'^2x^2 & x'^2y^2 & x'^2 & y'^2x^2 & y'^2y^2 & y'^2 & x^2 & y^2 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x'^nx^n & x'^ny^n & x'^n & y'^nx^n & y'^ny^n & y'^n & x^n & y^n & 1 \\ \end{bmatrix} \cdot \begin{bmatrix} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{bmatrix} = 0$⎣⎢⎢⎢⎡​x′1x1x′2x2⋮x′nxn​x′1y1x′2y2⋮x′nyn​x′1x′2⋮x′n​y′1x1y′2x2⋮y′nxn​y′1y1y′2y2⋮y′nyn​y′1y′2⋮y′n​x1x2⋮xn​y1y2⋮yn​11⋮1​⎦⎥⎥⎥⎤​⋅⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​e1​e2​e3​e4​e5​e6​e7​e8​e9​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​=0

即（the essential matrix lying in the null space of this matrix A）

$\boldsymbol{A} \cdot \boldsymbol{e} = \mathbf{0} \quad s.t. \quad \boldsymbol{A} \in \mathbb{R}^{n \times 9}, n \geq 8$A⋅e=0s.t.A∈Rn×9,n≥8

对上式 求解 最小二乘解（尺度等价性）

$\min_{\boldsymbol{e}} \|\boldsymbol{A} \cdot \boldsymbol{e}\|^2 \quad s.t. \quad \|\boldsymbol{e}^T \boldsymbol{e}\| = 1 \quad \text{or} \quad {\|\boldsymbol{E}\|}_F = 1$emin​∥A⋅e∥2s.t.∥eTe∥=1or∥E∥F​=1

SVD分解 $\boldsymbol{A}$A（或者 特征值分解 $\boldsymbol{A}^T \boldsymbol{A}$ATA）

$\text{SVD}(\boldsymbol{A}) = \boldsymbol{U} \boldsymbol{D} \boldsymbol{V}^T$SVD(A)=UDVT

$\boldsymbol{e}$e 正比于 $\boldsymbol{V}$V 的最后一列，得到 $\boldsymbol{E}$E

根据 奇异性约束 $\text{rank}(\boldsymbol{E})=2$rank(E)=2，再 SVD分解 $\boldsymbol{E}$E

$\text{SVD}(\boldsymbol{E}) = \boldsymbol{U}_E \boldsymbol{D}_E \boldsymbol{V}_E^T$SVD(E)=UE​DE​VET​

求出的 $\boldsymbol{E}$E 可能不满足其内在性质（奇异值是 $[\sigma, \sigma, 0]^T$[σ,σ,0]T 的形式），此时对 $\boldsymbol{D}_E$DE​ 进行调整，假设 $\boldsymbol{D}_E = \text{diag}(\sigma_1, \sigma_2, \sigma_3)$DE​=diag(σ1​,σ2​,σ3​) 且 $\sigma_1 \geq \sigma_2 \geq \sigma_3$σ1​≥σ2​≥σ3​，则令

$\boldsymbol{D}_E' = \text{diag}(\frac{\sigma_1+\sigma_2}{2}, \frac{\sigma_1+\sigma_2}{2}, 0)$DE′​=diag(2σ1​+σ2​​,2σ1​+σ2​​,0)

或者，更简单的（尺度等价性）

$\boldsymbol{D}_E' = \text{diag}(1, 1, 0)$DE′​=diag(1,1,0)

最后，$\boldsymbol{E}$E 矩阵的正确估计为

$\boldsymbol{E}' = \boldsymbol{U}_E \boldsymbol{D}_E' \boldsymbol{V}_E^T$E′=UE​DE′​VET​

Rotation and translation from E

The four possible solutions for calibrated reconstruction from E

• Between the left and right sides there is a baseline reversal
• Between the top and bottom rows camera B rotates 180 about the baseline
• only in (a) is the reconstructed point in front of both cameras

Suppose that the SVD of E is $U \text{diag}(1, 1, 0) V^T$Udiag(1,1,0)VT, there are (ignoring signs) two possible factorizations $E = t^\wedge R = SR$E=t∧R=SR as follows

$t^\wedge = S = UZU^T \\[2ex] R_1 = UWV^T \quad or \quad R_2 = UW^TV^T$t∧=S=UZUTR1​=UWVTorR2​=UWTVT

where

$W = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} \quad \text{and} \quad Z = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}$W=⎣⎡​010​−100​001​⎦⎤​andZ=⎣⎡​0−10​100​000​⎦⎤​

and

$\begin{aligned} W &\approx R_z(\frac{\pi}{2}) \\[2ex] Z &= W^T \cdot \text{diag}(1, 1, 0) \\[2ex] -Z &= W \cdot \text{diag}(1, 1, 0) \end{aligned}$WZ−Z​≈Rz​(2π​)=WT⋅diag(1,1,0)=W⋅diag(1,1,0)​

Rt恢复示例代码 e2rt.cpp：

Matrix3d E;
E << -0.0203618550523477,   -0.4007110038118445,  -0.03324074249824097,
      0.3939270778216369,   -0.03506401846698079,  0.5857110303721015,
     -0.006788487241438284, -0.5815434272915686,  -0.01438258684486258;

cout << "E = \n" << E << endl;

// SVD and fix sigular values
JacobiSVD<MatrixXd> svd(E, ComputeThinU | ComputeThinV);
Matrix3d m3U = svd.matrixU();
Matrix3d m3V = svd.matrixV();
Vector3d v3S = svd.singularValues();

double temp = (v3S[0]+v3S[1])/2;
Matrix3d m3S(Vector3d(temp, temp, 0).asDiagonal());

Eigen::Matrix3d m3R_z_p = Eigen::AngleAxisd( M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Eigen::Matrix3d m3R_z_n = Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
cout << "m3R_z_p = \n" << m3R_z_p << endl;
cout << "m3R_z_n = \n" << m3R_z_n << endl;

// set t1, t2, R1, R2
Matrix3d t_wedge1;
Matrix3d t_wedge2;
t_wedge1 = m3U * m3R_z_p * m3S * m3U.transpose();
t_wedge2 = m3U * m3R_z_n * m3S * m3U.transpose();

Matrix3d R1;
Matrix3d R2;
R1 = m3U * m3R_z_p.transpose() * m3V.transpose();
R2 = m3U * m3R_z_n.transpose() * m3V.transpose();

cout << "R1 = \n" << R1 << endl;
cout << "R2 = \n" << R2 << endl;
cout << "t1 = \n" << Sophus::SO3::vee(t_wedge1) << endl;
cout << "t2 = \n" << Sophus::SO3::vee(t_wedge2) << endl;

// check t^R=E up to scale
Matrix3d tR = t_wedge1 * R1;
cout << "t^R = \n" << tR << endl;

DecomposeE in ORB-SLAM2

void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t) {
    cv::Mat u,w,vt;
    cv::SVD::compute(E,w,u,vt);

    u.col(2).copyTo(t);
    t=t/cv::norm(t);

    cv::Mat W(3,3,CV_32F,cv::Scalar(0));
    W.at<float>(0,1)=-1;
    W.at<float>(1,0)=1;
    W.at<float>(2,2)=1;

    R1 = u*W*vt;
    if(cv::determinant(R1)<0)
        R1=-R1;

    R2 = u*W.t()*vt;
    if(cv::determinant(R2)<0)
        R2=-R2;
}

CheckRT in ORB-SLAM2

Homography Matrix (单应性矩阵)

• For planar surfaces, 3D to 2D perspective projection reduces to a 2D to 2D transformation
• 从零开始一起学习SLAM | 神奇的单应矩阵

单应性矩阵 通常描述处于 共同平面 上的一些点在 两张图像之间的变换关系。

$\boldsymbol{p'} = \boldsymbol{H} \cdot \boldsymbol{p}$p′=H⋅p

其中，$p, p'$p,p′ 为两个匹配像素点的 归一化平面坐标 （也可为其他点，只要 共面且3点不共线 即可）

Homography Estimation

矩阵形式：

$\begin{bmatrix} x' \\ y' \\ 1 \end{bmatrix} = \begin{bmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{bmatrix} \cdot \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}$⎣⎡​x′y′1​⎦⎤​=⎣⎡​h11​h21​h31​​h12​h22​h32​​h13​h23​h33​​⎦⎤​⋅⎣⎡​xy1​⎦⎤​

方程形式（两个约束条件）：

$x' = \frac { h_{11}x + h_{12}y + h_{13} } { h_{31}x + h_{32}y + h_{33} } \\[2ex] y' = \frac { h_{21}x + h_{22}y + h_{23} } { h_{31}x + h_{32}y + h_{33} }$x′=h31​x+h32​y+h33​h11​x+h12​y+h13​​y′=h31​x+h32​y+h33​h21​x+h22​y+h23​​

因为上式使用的是齐次坐标，所以我们可以 对 所有的 $h_{ij}$hij​ 乘以 任意的非0因子 而不会改变方程。

因此， $\boldsymbol{H}$H 具有 8个自由度，最少通过 4对匹配点（不能出现3点共线） 算出。

实际中，通过 $h_{33}=1$h33​=1 或 $\|\boldsymbol{H}\|_F=1$∥H∥F​=1 两种方法 使 $\boldsymbol{H}$H 具有 8自由度。

cont 1: H元素h33=1

线性方程：

$\boldsymbol{A} \cdot \boldsymbol{h} = \boldsymbol{b}$A⋅h=b

求解：

$\boldsymbol{A}^T \boldsymbol{A} \cdot \boldsymbol{h} = \boldsymbol{A}^T \boldsymbol{b}$ATA⋅h=ATb

所以

$\boldsymbol{h} = (\boldsymbol{A}^T \boldsymbol{A})^{-1} \boldsymbol{A}^T \boldsymbol{b}$h=(ATA)−1ATb

cont 2: H的F范数|H|=1

线性方程：

$\boldsymbol{A} \cdot \boldsymbol{h} = \mathbf{0}$A⋅h=0

求解：

$\boldsymbol{A}^T \boldsymbol{A} \cdot \boldsymbol{h} = \mathbf{0}$ATA⋅h=0

对上式 求解 最小二乘解（尺度等价性）

$\min_{\boldsymbol{h}} \|(\boldsymbol{A}^T \boldsymbol{A}) \cdot \boldsymbol{h}\|^2 \quad s.t. \quad \|\boldsymbol{h}^T \boldsymbol{h}\| = 1 \quad \text{or} \quad {\|\boldsymbol{H}\|}_F = 1$hmin​∥(ATA)⋅h∥2s.t.∥hTh∥=1or∥H∥F​=1

SVD分解 或 特征值分解

$\text{SVD}(\boldsymbol{A}^T \boldsymbol{A}) = \boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{U}^T$SVD(ATA)=UΣUT

最后 $\boldsymbol{h}$h 为 $\boldsymbol{\Sigma}$Σ 中 最小特征值 对应的 $\boldsymbol{U}$U 中的列向量（单位特征向量）；如果只用4对匹配点，那个特征值为0。

H in PTAM

• 相关代码

单应性矩阵的计算

Matrix<3> HomographyInit::HomographyFromMatches(vector<HomographyMatch> vMatches)
{
    unsigned int nPoints = vMatches.size();
    assert(nPoints >= 4);
    int nRows = 2*nPoints;
    if(nRows < 9)
        nRows = 9;
    Matrix<> m2Nx9(nRows, 9);
    for(unsigned int n=0; n<nPoints; n++)
    {
        double u = vMatches[n].v2CamPlaneSecond[0];
        double v = vMatches[n].v2CamPlaneSecond[1];

        double x = vMatches[n].v2CamPlaneFirst[0];
        double y = vMatches[n].v2CamPlaneFirst[1];

        // [u v]T = H [x y]T
        m2Nx9[n*2+0][0] = x;
        m2Nx9[n*2+0][1] = y;
        m2Nx9[n*2+0][2] = 1;
        m2Nx9[n*2+0][3] = 0;
        m2Nx9[n*2+0][4] = 0;
        m2Nx9[n*2+0][5] = 0;
        m2Nx9[n*2+0][6] = -x*u;
        m2Nx9[n*2+0][7] = -y*u;
        m2Nx9[n*2+0][8] = -u;

        m2Nx9[n*2+1][0] = 0;
        m2Nx9[n*2+1][1] = 0;
        m2Nx9[n*2+1][2] = 0;
        m2Nx9[n*2+1][3] = x;
        m2Nx9[n*2+1][4] = y;
        m2Nx9[n*2+1][5] = 1;
        m2Nx9[n*2+1][6] = -x*v;
        m2Nx9[n*2+1][7] = -y*v;
        m2Nx9[n*2+1][8] = -v;
    }

    if(nRows == 9)
        for(int i=0; i<9; i++)  // Zero the last row of the matrix,
            m2Nx9[8][i] = 0.0;  // TooN SVD leaves out the null-space otherwise

    // The right null-space of the matrix gives the homography...
    SVD<> svdHomography(m2Nx9);
    Vector<9> vH = svdHomography.get_VT()[8];
    Matrix<3> m3Homography;
    m3Homography[0] = vH.slice<0,3>();
    m3Homography[1] = vH.slice<3,3>();
    m3Homography[2] = vH.slice<6,3>();
    return m3Homography;
};

Rotation and translation from H

• Motion and structure from motion in a piecewise planar environment

手写笔记

2D-2D相机位姿估计

2D-2D相机位姿估计 通常利用 对极几何 进行计算，是单目SLAM初始化时的关键技术。

• 计算 基础矩阵或本质矩阵 适用于特征点不共面的情况，计算 单应矩阵 适用于特征点共面的情况

• 当 特征点共面 或者 相机发生纯旋转 时，基础矩阵 $F$F 的自由度下降，就会出现所谓的 退化(degenerate)。为了能够避免退化现象的影响，通常会 同时估计基础矩阵 $F$F 和 单应矩阵 $H$H，选择重投影误差比较小的那个作为最终的运动估计矩阵。

• 平移向量t 的 尺度不确定性

• 初始化的纯旋转问题：单目初始化不能只有旋转，必须要有一定程度的平移，否则由于t趋近于0，导致无从求解R或者误差非常大

• 多于8对点：RANSAC

Ref: 2D-2D相机位姿估计

Reference

• Epipolar Geometry and the Fundamental Matrix in MVG (Chapter 9)
• 《视觉SLAM十四讲》