对极约束公式推导，八点法方程组对极约束公式推导；本质矩阵、基础矩阵定义，以及八点法（Eight Point Alg

对极约束公式由来：十四讲公式(7.9)

已知： $\boldsymbol P=[X,Y,Z]^\top$ 是空间中的一个点，它在相机两帧的成像平面上的像素点为 $\boldsymbol p_1 , \boldsymbol p_2$ ，相机内参为 $\boldsymbol K$ , 两帧间相机运动为 $\boldsymbol R, \boldsymbol t$ , 即满足：

s_1 \boldsymbol p_1 = \boldsymbol {KP}, \quad s_2 \boldsymbol p_2 = \boldsymbol{K(RP+t)} \qquad（1）

由以上信息，在齐次坐标表示下，推导出对极约束：

\boldsymbol{ p_2^\top K^{-\top} t^{\wedge} R K^{-1}p_1 = 0 } \qquad \qquad \qquad (2)

解：把公式（1）写成齐次坐标表示方式，可以简化为：

\begin{aligned} \boldsymbol p_1 &= \boldsymbol {KP}， \quad \boldsymbol{ p_2 = K(RP+t) } \\ \boldsymbol { x_1 \triangleq K^{-1} p_1 } &= \boldsymbol P ， \\ \boldsymbol { x_2 \triangleq K^{-1} p_2 } &= \boldsymbol { RP + t } \longrightarrow \\ \boldsymbol { R^{-1}(K^{-1} p_2 - t)} &= \boldsymbol P \\ \therefore \boldsymbol { K^{-1} p_1 } &= \boldsymbol{ R^{-1}(K^{-1} p_2 - t) } \\ \boldsymbol { RK^{-1} p_1} &= \boldsymbol {K^{-1} p_2 - t } \\ 两边左乘 \boldsymbol { t^\wedge } ,& \quad再左乘 (\boldsymbol {K^{-1} p_2})^\top \\ \boldsymbol { (K^{-1} p_2)^\top t^\wedge R K^{-1} p_1 } & = \boldsymbol{ \underline { (K^{-1} p_2)}^\top t^{\wedge} \underline { K^{-1} p_2 } - (K^{-1} p_2)^\top t^{\wedge} t } \\ 上式右边为0,& \quad 则得到： \\ \boldsymbol { (K^{-1} p_2)^\top t^\wedge R K^{-1} p_1 } &= 0 \\ \boldsymbol { p_2^\top K^{-\top} t^\wedge R K^{-1} p_1} &= 0 \end{aligned}

以下对上述过程中等号右边变为0进行说明, 右边如下：

\boldsymbol {\underline { (K^{-1} p_2)}^\top t^{\wedge} \underline { K^{-1} p_2 } - (K^{-1} p_2)^\top t^{\wedge} t }

分析：

$\boldsymbol{t^\wedge t}$ 是一个向量自己和自己的叉乘，为0，第二项为0；
$\boldsymbol {t^\wedge ( K^{-1} p_2) }$ 代表两个向量叉乘，其结果和两个向量都垂直，此时再与其中一个向量 $\boldsymbol { K^{-1} p_2 }$ 内积，第一项为0.

得证.

八点法如何求解本质矩阵 $\boldsymbol {E}$ ：十四讲公式（7.13）

把对极约束中的 $\boldsymbol {t^\wedge R}$ 定义为本质矩阵（Essential Matrix） $\boldsymbol E$ ，把 $\boldsymbol {K^{-\top} E K^{-1} }$ 定义为基础矩阵(Fundamental Matrix) $\boldsymbol F$ 。即：

\begin{aligned} \boldsymbol E &= \boldsymbol{ t^\wedge R} \\ \boldsymbol F &= \boldsymbol{ K^{-\top} E K^{-1}} \\ \boldsymbol { x_2 E x_1 } &= 0 ， \quad 其中 \boldsymbol{ x_1 \triangleq K^{-1} p_1, \quad x_2 \triangleq K^{-1} p_2 } \end{aligned}

因此，根据公式(2)，这是一个方程组，只要找到足够多的点 $\boldsymbol {p_1, p_2}$ 就可以通过方程组解出 $\boldsymbol {E}$ , 因此也就得到 $\boldsymbol {R, t}$ 。

由于 $\boldsymbol {t^\wedge R}$ 是一个 $3 \times 3$ 的矩阵，有9个未知数。又由于公式（2）是在齐次坐标下表示，因此存在尺度等价性，因此8个未知数是相互独立的。所以需要8对 $\boldsymbol {p_1, p_2}$ 来求解 $\boldsymbol E$ 。这就是八点法(Eight-point-algorithm).

考虑一对点 $\boldsymbol {p_1, p_2}$ ，它们对应的像素平面归一化坐标为 $\boldsymbol {x_1} = [u_1, v_1, 1]^\top, \quad \boldsymbol {x_2} = [u_2, v_2, 1 ]^\top$ , 根据对极约束，有下式成立：

\begin{aligned} ( \begin{array}{} u_2 & v_2 & 1 \end{array} ) \left ( \begin{array} {} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \end{array} \right ) \left ( \begin{array}{} u_1 \\ v_1 \\ 1 \end{array} \right ) &= 0 \\ \left( \begin{array}{} u_2 e_1 + v_2 e_4 + e_7 & u_2 e_2 + v_2 e_5 + e_8 & u_2 e_3 + v_2 e_6 + e_9 \end{array} \right ) \left ( \begin{array}{} u_1 \\ v_1 \\ 1 \end{array} \right ) &= 0 \\ u_1 u_2 e_1 + u_1 v_2 e_4 + u_1 e_7 \quad + \quad v_1 u_2 e_2 + v_1 v_2 e_5 + v_1 e_8 \quad + \quad u_2 e_3 + v_2 e_6 + e_9 &= 0 \\ u_1 u2 e_1 + u_2v_1 e_2 + u_2 e_3 + u_1 v_2 e4 + v_1 v_2 e_5 + v_2 e_6 + u_1 e_7 + v_1 e_8 + e_9 &= 0 \\ \left ( \begin{array}{} u_1 u_2 & u_2 v_1 & u_2 & u_1 v_2 & v_1 v_2 & v_2 & u_1 & v_1 & 1 \end{array} \right ) \left ( \begin{array}{} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \\ e_9 \end{array} \right ) & = 0 \end{aligned}

上面是一对点形成的方程，若选取8对点，则可以得到一个方程组 ( $u^{(i)}, v^{(i)}$ 代表第 $i$ 个点 )，如下：

\left ( \begin{array}{} u_1^{(1)} u_2^{(1)} & u_2^{(1)} v_1^{(1)} & u_2^{(1)} & u_1^{(1)} v_2^{(1)} & v_1^{(1)} v_2^{(1)} & v_2^{(1)} & u_1^{(1)} & v_1^{(1)} & 1 \\ \quad \\ u_1^{(2)} u_2^{(2)} & u_2^{(2)} v_1^{(2)} & u_2^{(2)} & u_1^{(2)} v_2^{(2)} & v_1^{(2)} v_2^{(2)} & v_2^{(2)} & u_1^{(2)} & v_1^{(2)} & 1 \\ \quad \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ \quad \\ u_1^{(8)} u_2^{(8)} & u_2^{(8)} v_1^{(8)} & u_2^{(8)} & u_1^{(8)} v_2^{(8)} & v_1^{(8)} v_2^{(8)} & v_2^{(8)} & u_1^{(8)} & v_1^{(8)} & 1 \end{array} \right ) \left ( \begin{array}{} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \\ e_9 \end{array} \right ) = 0

以上8个方程构成了一个线性方程组，解方程即可得各变量 $e_i$ ，从而得到矩阵 $\boldsymbol E$ 。通过 $\boldsymbol E$ 可以恢复出相机的运动 $\boldsymbol {R,t}$ .

由本质矩阵求 $\boldsymbol {R, t}$

以下仅仅是助于理解，真正的推导过程，是1992年由Hartley提出的，从本质矩阵相差一个尺度因子的情况下恢复 $R,t$ 的四个可能解。证明过程可见《计算机视觉中的多视图几何》中文版P174。

\begin{aligned} \boldsymbol {E} = \boldsymbol { t^\wedge R} &= \boldsymbol{ U \Sigma V^\top }\\ &= \boldsymbol{ U \Sigma \underline{U^\top U} V^\top } \\ &= \boldsymbol{ \underline{U \Sigma U}^\top \underline{ U V}^\top } \end{aligned}

因此可以猜到：

\begin{aligned} \boldsymbol{t^\wedge} &= \boldsymbol{ U \Sigma U^\top } \\ \boldsymbol{R} &= \boldsymbol{ U V^\top } \end{aligned}

记

\boldsymbol {R}_{Z}(\dfrac{\pi}{2}) = \left ( \begin{array}{} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array} \right )

来表示沿 $Z$ 轴旋转90°的旋转矩阵。

重新表示 $\boldsymbol { t^\wedge , R }$ :

\begin{aligned} \boldsymbol{t_1^\wedge} &= \boldsymbol{ U R_{Z}(\dfrac{\pi}{2}) \Sigma U^\top } \\ \boldsymbol{R_1} &= \boldsymbol{ U R_{Z}(\dfrac{\pi}{2}) V^\top } \\ \boldsymbol{t_2^\wedge} &= \boldsymbol{ U R_{Z}(- \dfrac{\pi}{2}) \Sigma U^\top } \\ \boldsymbol{R_2} &= \boldsymbol{ U R_{Z}( -\dfrac{\pi}{2}) V^\top } \end{aligned}

程序实现

参见十四讲的7.4小节的代码： slambook2/ch7/pose_estimation_2d2d.cpp

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值
  double focal_length = 521;      //相机焦距, TUM dataset标定值

  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }

  //-- 计算本质矩阵
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
  cout << "essential_matrix is " << endl << essential_matrix << endl;

  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;
}

opencv 直接提供了从匹配的特征点对集合获取本质矩阵的方法 findEssentialMat()，传入特征点对，以及光心、相机焦距等参数即可得到 $E$ ; 同时opencv还提供了从本质矩阵恢复相机运动 $R,t$ 的方法 recoverPose(), 传入特征点对、光心、相机焦距等，即可计算得到 $R,t$ .

其他参考

文章1：利用SVD求得两个对应点集合的旋转矩阵R和转移矩阵t的数学推导
- 介绍
  
  给出的是当给定两个特征点对集合后，如何得到 $R=UV^\top$ 和 $t=\hat q - R \hat p$ 的数学推导，采用的是求解最优化的思路，建模为： $(R, t) = \argmin \Sigma_{i=1}^n w_i \parallel (Rp_i+t) - q_i \parallel^2$
- 链接
  
  blog.csdn.net/u012836279/…
- 英文原版链接： Least-Squares Rigid Motion Using SVD igl.ethz.ch/projects/AR…
文章2：计算两个对应点集之间的旋转矩阵R和转移矩阵T
- 介绍给出了上述数学推导的具体实现，例如如何计算特征点集合的中心点、点集重新中心化、点集之间的协方差矩阵计算、通过SVD计算得到 $R$ 、通过 $R$ 计算得到 $t$ 的实现，以及代码。
- 链接： blog.csdn.net/u012836279/…

对极约束公式推导， 八点法方程组

对极约束公式由来： 十四讲公式(7.9)

八点法如何求解本质矩阵 E\boldsymbol {E}E： 十四讲公式（7.13）

由本质矩阵求R,t\boldsymbol {R, t}R,t

程序实现

其他参考

对极约束公式推导，八点法方程组

对极约束公式由来：十四讲公式(7.9)

八点法如何求解本质矩阵 $\boldsymbol {E}$ ：十四讲公式（7.13）

由本质矩阵求 $\boldsymbol {R, t}$