对极约束
设第一帧到第二帧的运动为$R,t$，相机中心为$O_1,O_2$，$I_1$中特征点$p_1$对应$I_2$中的特征点$p_2$，如果匹配正确，说明是同一个空间点在两个成像平面的投影。连线$overrightarrow{O_{1}p_1}$和连线$overrightarrow{O_{２}p_２}$相交于点$P$。$O_1,O_2,P$确定一个平面，为极平面。$e_1,e_2$为极点，$O_1O_2$为基线，$l_1,l_2$为极线。
仅从第一帧的角度看，射线$overrightarrow{O_{1}p_1}$是某像素可能出现的空间位置，该射线上所有点都会投影到该像素点，连线$overrightarrow{e_2p_2}$是$P$点在第二帧中可能出现的投影位置，即射线$overrightarrow{O_{1}p_1}$在第二帧中的投影。而$p_2$确定，所以能推断出$P$的位置。
数学表达
在第一帧坐标系下，$P$点坐标为$left[X,Y,Z ight]^T$，注意这不是世界坐标系下的坐标。
参考《视觉SLAM十四讲》P86
$ZP_{uv} = Zegin{bmatrix} u \ v \ 1 end{bmatrix} = Kleft( RP_w + t ight) = KTP_w$，$P_w$为世界坐标系下三维坐标点。
上式两侧都是其次坐标，因为齐次坐标乘上非零常数后表达同样的意思，所以可以把$Z$去掉。
$P_{uv} = KTP_w$($TP_w$表示一个世界坐标系下的齐次坐标变换为相机坐标系下，为了使它与$K$相乘，需要取它前三维组成向量，$TP_{w}$最后一维为１)
齐次坐标：https://blog.csdn.net/jeffasd/article/details/77944822
知道两个不同帧中的像素点$p_1,p_2$的像素坐标。
$s_1p_1 = KP,s_2p_2 = K left( RP + t ight)$
$x_1 = s_1K^{-1}p_1 = P, x_2 = s_2K^{-1}p_2 = RP + t$
所以有$Rx_1 + t = x_2$
同时左乘$t^{wedge}$，得$t^{wedge}x_2 = t^{wedge}Rx_1$，同时左乘$x_2^T$，$x_2^Tt^{wedge}x_2 = x_2^Tt^{wedge}Rx_1$
$t^{wedge}x_2$是一个与$t$和$x_2$都垂直的向量。将它和$x_2$作内积，将得０。
得一个简洁的式子：$x_2^{T}t^{wedge}Rx_1 = 0$
重新代入$x_1,x_2$有$left( s_2K^{-1}p_2 ight)^{T}t^{wedge}Rleft( s_1K^{-1}p_1 ight) = 0 $，$s_1,s_2$为标量。（$left( lambda A ight)^T = lambda A^{T},lambdain P$ ）
所以有：$p_2^TK^{-T}t^{wedge}RK^{-1}p_1 = 0$，即对极约束，中间包含了平移和旋转，将其中分成两个矩阵基础矩阵F和本质矩阵E。
简化为：
$E = t^{wedge}R,F = K^{-T}EK^{-1},x_2^TEx_1 = p_2^TFp_1 = 0$
相机位置关系求解有以下两步：
１.根据配对点位置求E和F。
２.根据E和F求R,t。
其中F的相机内参已知。
本质矩阵性质：
１．E在不同尺度下是等价的
２．本质矩阵E的奇异值是$left[sigma,sigma,0 ight]^T$的形式。
３．平移与旋转各有３个自由度，所以$t^{wedge}R$有６个自由度，由于尺度等价性，E只有５自由度
最少用５对点求解E。但是E的性质是非线性的，难求解，只考虑它的尺度等价性，用八对点估计E(八点法)，利用了E的线性性质。
考虑一对匹配点，x 1 =[ u 1 , v 1 , 1] T , x 2 =[ u 2 , v 2 , 1] T
由八对特征点，可求本质矩阵E中的各个元素。
Ｅ已经求得了，可根据本质矩阵Ｅ，求得Ｒ，ｔ。可由奇异值分解（ＳＶＤ）得到$E = U{Sigma}V^T$
$Sigma$为奇异值矩阵，$Sigma = diag(sigma,sigma,0)$， $left(t_1,R_1 ight) left(t_2,R_2 ight) left(t_1,R_2 ight) left(t_2,R_1 ight)$四种组合。
1 // 本程序演示如何从Essential矩阵计算R,t 2 // 3 4 #include <Eigen/Core> 5 #include <Eigen/Dense> 6 #include <Eigen/Geometry> 7 8 using namespace Eigen; 9 10 #include <sophus/so3.h> 11 12 #include <iostream> 13 14 using namespace std; 15 16 int main(int argc, char **argv) { 17 18 // 给定Essential矩阵 19 Matrix3d E; 20 E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097, 21 0.3939270778216369, -0.03506401846698079, 0.5857110303721015, 22 -0.006788487241438284, -0.5815434272915686, -0.01438258684486258; 23 24 // 待计算的R,t 25 Matrix3d R; 26 Vector3d t; 27 // SVD and fix sigular values 28 // START YOUR CODE HERE 29 JacobiSVD<MatrixXd> svd(E, ComputeThinU | ComputeThinV); 30 Matrix3d V = svd.matrixV(),U = svd.matrixU(); 31 Matrix3d S = U.inverse() * E * V.transpose().inverse(); 32 cout<<"S:"<<S<<endl; 33 S<<S(0,0),0,0, 0,S(1,1),0, 0,0,0; 34 cout<<"S:"<<S<<endl; 35 36 37 // END YOUR CODE HERE 38 // set t1, t2, R1, R2 39 // START YOUR CODE HERE 40 41 Matrix3d t_wedge1; 42 Matrix3d t_wedge2; 43 Eigen::Matrix3d RZ = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();//绕ｚ轴90度 44 Eigen::Matrix3d RZf = Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();//绕ｚ轴-90度 45 46 //cout<<RZ<<endl; 47 t_wedge1 = U*RZ*S*U.transpose(); 48 t_wedge2 = U*RZf*S*U.transpose(); 49 50 Matrix3d R1; 51 Matrix3d R2; 52 53 R1 = U*RZ.transpose()*V.transpose(); 54 R2 = U*RZf.transpose()*V.transpose(); 55 56 // END YOUR CODE HERE 57 58 cout << "R1 = " << R1 << endl; 59 cout << "R2 = " << R2 << endl; 60 cout << "t1 = " << Sophus::SO3::vee(t_wedge1) << endl; 61 cout << "t2 = " << Sophus::SO3::vee(t_wedge2) << endl; 62 63 // check t^R=E up to scale 64 Matrix3d tR11 = t_wedge1 * R1; 65 cout << "t1^R1 = " << tR11 << endl; 66 67 Matrix3d tR12 = t_wedge1 * R2; 68 cout << "t1^R2 = " << tR12 << endl; 69 70 Matrix3d tR21 = t_wedge2 * R1; 71 cout << "t2^R1 = " << tR21 << endl; 72 73 Matrix3d tR22 = t_wedge2 * R2; 74 cout << "t2^R2 = " << tR22 << endl; 75 76 77 return 0; 78 }
执行结果：
四种组合相乘，结果一致（-E=E）
单应矩阵
特征点都落在同一平面上。
图１
$n^{T}X_{1}+d = 0$其中n为平面法向量，$O_{1}$为第一帧中心。$X_{1}的坐标不是left[X,Y,Z ight],第三维并不是Z,所以d并不等于Z$。
图２
在图１中$n^{T}X_{1}+d = 0,得-/frac{n^{T}X_{1}}{d} = 1$
又因为$Zegin{bmatrix} u \ v \ 1 end{bmatrix} = egin{bmatrix} f_{x} & 0 & c_{x} \ 0 & f_{y} & c_{y} \ 0 & 0 & 1 end{bmatrix} egin{bmatrix} X \ Y \ Z end{bmatrix} riangleq Kcdot{P}$齐次坐标乘非零数表达同样含义，所以$egin{bmatrix} u \ v \ 1 end{bmatrix} = egin{bmatrix} f_{x} & 0 & c_{x} \ 0 & f_{y} & c_{y} \ 0 & 0 & 1 end{bmatrix} egin{bmatrix} X \ Y \ Z end{bmatrix} riangleq Kcdot{P}$
$m_{1} = KX_1,m_{2} = KX_2$
$m_{2} = K left( RX_{1} + t ight) = Kleft( RX_{1} + t left(-frac{n^{T}X_{1}}{d} ight) ight)= Kleft( R- frac{t{n}^{T}}{d} ight)X_{1} = K left( R - frac{tn^T}{d} ight)K^{-1}m_1$
即$m_2 = Hm_1$
等号在非零因子下成立，即处理中可以乘以一个非零因子使$h_9$为１。
$egin {bmatrix} u_2 \ v_2 \ 1 end {bmatrix} = egin {bmatrix} h_1 & h_2 & h_3 \ h_4 & h_5 & h_6 \ h_7 & h_8 & h_9 end {bmatrix} = egin {bmatrix} h_1u_1 + h_2v_1+h_3 \ h_4u_1 + h_5v_1 + h_6 \ h_7u_1 + h_8v_1 + h_9 end{bmatrix}$
右式除以矩阵第三项h_7u_1 + h_8v_1 + h_9
其中$h_9 = 1$，可以整理得
可通过４对匹配特征点算出，解方程（求$h_1~h_8$）。
这种方法可以求解出Ｈ单应矩阵，通过数值法/解析法分解，得Ｒ和ｔ。
自由度下降，会出现退化。