1.2D-2D对极几何
输入：相机内参、像素匹配点对，输出：相机位姿
1.1本质矩阵
\(E\) 矩阵 \(E=t^{\wedge} R\)
对极约束：\(x_2^Tt^{\wedge} Rx_1=0\)，\(x_1,x_2\)都是相机系归一化点坐标。
推导：\(z_1x_1=P_w,z_2x_2=RP_w+t\)
\(x_2^{\wedge}t^{\wedge}z_2x_2=x_2^{\wedge}t^{\wedge}Rz_1x_1=0\)
1.2基础矩阵
\(p_2^TK^{-T}t^{\wedge} RK^{-1}p_1=0\)，\(p_1,p_2\)都是像素坐标。
\(E\) 矩阵、\(F\) 矩阵只相差了相机内参：\(F=K^{-T}x_2^Tt^{\wedge} Rx_1K^{-1}\)
8对法，线性最小二乘法，即可求解。
1.3单应矩阵
\(H\) 矩阵(Homography)：描述的是两个平面之间的映射关系，如果场景中的特征点都落在同一平面上(比如墙面、地面)，则可以通过单应性进行运动估计。
\(p_2=Hp_1\)
4对匹配点即可求解。
1.4 *度
秩与*度(方阵\(A_{n*n}\))：
矩阵的秩，指的是经过初等变换之后的非零行（列）的个数，若不存在零行（列），则为满秩矩阵\(Rank(A)=n\)；关于矩阵的秩的另一种理解：A矩阵将n维空间中的向量映射到k（k<=n）维空间中，\(k=Rank(A)\)
矩阵（参数矩阵）的*度，指的是要想求解出矩阵的所有元素至少需要列几个线性方程组。若矩阵本身带有 x 个约束，则只需要列\((n*n-x)\)个方程组即可求出所有参数，即矩阵A的*度为\((n*n-x)\)。
- 1.尺度等价性要减少一个*度？
以基础矩阵为例,\(F\) 矩阵满足:
由于等式右侧是0，乘以任意常数以后还是表示同样两点之间的变换，所以是F是尺度等价的。
(对于9个参数的向量e，我们只需要通过8个方程计算出其中8个未知数即可， 8个数都用第9个数表示，由于尺度等价，所以第9个数取什么值都是对的)
- 2.为什么\(E\) 矩阵的秩为2？
\(E=t^{\wedge} R\)
一个矩阵乘以可逆矩阵秩不变，因为可逆矩阵可以表示为初等矩阵的乘积，而初等变换不改变矩阵的秩。
\(Rank(R)=3\)
对于\(t^{\wedge}\):
\(\left[ \begin{matrix} 0 & -t_3 & t_2 \\ t_3 & 0 & -t_1\\ -t_2 & t_1 &0 \end{matrix} \right] \rightarrow \left[ \begin{matrix} 0 & -t_3 & t_2 \\ t_2t_3 & 0 & -t_1t_2\\ -t_2t_3 & t_1t_3 &0 \end{matrix} \right] \rightarrow \left[ \begin{matrix} 0 & -t_3t_1 & t_2t_1 \\ 0 & t_1t_3 & -t_1t_2\\ -t_2t_3 & t_1t_3 &0 \end{matrix} \right] \rightarrow \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & t_3 & -t_2\\ -t_2 & t_1 &0 \end{matrix} \right]\)
\(Rank(t^{\wedge})=2\)
- 3.为什么\(F\) 矩阵的秩为2？
\(F=K_r^{-T}t^{\wedge} RK_l^{-1}\)
因为两个相机的内参矩阵和旋转矩阵都是满秩(可逆)矩阵，\(Rank(t^{\wedge})=2\),所以\(Rank(F)=2\).
- 4.为什么\(F\) 矩阵的*度为7，\(E\) 矩阵的*度为5，\(H\) 矩阵的*度为8？
\(F\) 矩阵：
左右相机内参的待定参数各为4;平移参数是3;旋转矩阵R的*度是3，所以待定参数是3;
加在一起是14个参数，也就是正常来说把14个参数都确定了才能确定F，但是实际上F是一个\(3*3\)的矩阵，只包含9个参数，所以计算F的*度最大是9，也就是9个参数就可以确定F。
同时F满足下面两个约束：(1)尺度等价性，(2)\(det(F)=0\)，所以*度为9-2=7。
\(E\) 矩阵：
平移的待定参数是3，旋转矩阵R的*度是3，所以待定参数是3，共6个参数，也就是要想确定E矩阵，确定6个参数就够了，不用考虑矩阵的所有9个参数。
同时E满足下面约束：(1)尺度等价性，所以*度是6-1=5。
\(H\) 矩阵：
\(H\) 矩阵具有尺度等价性：9-1=8。
1.5总结
-
尺度不确定性：对t长度的归一化，直接导致单目视觉的尺度不确定性
-
初始化的纯旋转问题
单目视觉初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，从E分解到R,t的过程中，导致t为零，那么得到的E也为零，这将导致无法求解R。不过此时可以依靠H求取旋转，但是仅仅有旋转时，无法使用三角测量估计特征点的空间位置。 -
单应矩阵和本质矩阵使用情景区别
单应矩阵用于场景的特征点都落在同一个平面上的时候使用，比如墙，地面，可以通过单应性来估计运动，因为当特征点共面的时候，基础矩阵的*度下降也就是出现退化现象。
本质矩阵用于估计特征点不共面的情况下。
2.三角测量
已知：相机内参，两帧相机外参\(T_1\)和\(T_2\)，像素匹配点对\(uv_1\)和\(uv_2\)
求：像素点对应的世界点
方法：
根据内参，计算像素点对应的相机系归一化点\(p_1\)和\(p_2\)(\(x_c\)和\(y_c\))
根据\(z_1*p_1=T_1*P_w\)
\(z_2*p_2=T_2*P_w\)
叉乘：
第一个式子：
\[ (p_{1})_{×}*T_1* P_w = \left\{ \begin{matrix} 0 & -1 & y1_c \\ 1 & 0 & -x1_c \\ -y1_c & x1_c & 0 \end{matrix} \right\} * \left\{ \begin{matrix} T1_1 \\ T1_2 \\ T1_3 \end{matrix} \right\} *P_w=0 \] \(T1_1、T1_2、T1_3\)都是1×4，是矩阵T1(3*4)的第1，2，3行。
可得到：
同理，第二个式子可得到
\[\left\{ \begin{matrix} T2_2-y2_c*T2_3 \\ T2_1-x2_c*T2_3 \end{matrix} \right\} *P_w =0 \] 综合：
\[\left\{ \begin{matrix} T1_2-y1_c*T1_3 \\ T1_1-x1_c*T1_3 \\ T2_2-y2_c*T2_3 \\ T2_1-x2_c*T2_3 \end{matrix} \right\} *P_w =0 \]for(int i=0;i<n;i++)
{
cv::KeyPoint kp1 = vKFs[0].mvKeys[i];
cv::KeyPoint kp2 = vKFs[1].mvKeys[i];
cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx)*invfx, (kp1.pt.y-cy)*invfy, 1.0);
cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx)*invfx, (kp2.pt.y-cy)*invfy, 1.0);
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
cv::Mat x3D = vt.row(3).t();
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
cout<<i<<","<<x3D.t()<<endl;
}
总结：
- 1.三角测量是平移得到的，纯旋转无法使用三角测量。
- 2.当平移很小时，像素上的不确定性将导致较大的深度不确定性。
3.3D-2D
PnP(perspective n points)
输入:3d空间点 2d像素点，输出:相机的位姿
(1)直接线性变换DLT：
最少通过6对匹配点实现矩阵TT的线性求解，当匹配点多于6对时，可以使用SVDSVD对超定方程求最小二乘解。
(2)3对点估计位姿的P3P:
3D−2D匹配点，3D点是A,B,C(世界坐标系中坐标)，2D点是a,b,c(成像平面像素坐标)
P3P只利用3个点的信息，当给定匹配点多余3组时，难以利用更多信息。
如果3D点或者2D点受噪声影响，或者存在误匹配，算法失效。
在SLAM中，通常先使用 P3P/EPnP等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment)。
(3)使用BundleAdjustment优化求解位姿
4.3D-3D
ICP估计相机位姿
根据一组已经匹配好的3D点，\(P={p_1,..,p_n}，P′={p_1′,..,p_n′}\)
从中找到一个欧氏变换的R，t，使得任意i满足\(p_i=Rp_i'+t\)
和PnP类似，ICP有两种解法:
线性代数求解(SVD方法)、非线性优化方法(类似于Bundle Adjustment)
- 小点：
\(P_c=T_{cw}*P_w\)
要获得当前相机所在的世界点：\(P_c\)为0,\(P_w = T_{wc}*[0,0,0,1]\) - 矩阵连乘
已知\(b= [r_1,t_1;0,1]*a = T_{ba}*a\)
和\(c= [r_{2},t_{2};0,1]*b = T_{cb}*b\)
则
\(c= T_{cb}*T_{ba}*a\)
\(= r_{2}*r_{1}*a+r_{2}*t_{1}+t_{2}\)
\(= [r_{2}*r_{1},r_{2}*t_{1}+t_{2};0,1]*a\)
或者已知\(T_{c1w}\)-对应(\(r_1,t_1\))、\(T_{c2w}\)-对应(\(r_2,t_2\))
转化到第一帧为基准系：
则\(T_{c1c1}\)为单位帧
\(T_{wc1}--对应(r_{1}^t,-r_{1}^t*t_1)\)
\(T_{c2c1}=T_{c2w}*T_{wc1}=[r_2*r_{1}^t,-r_2*r_{1}^t*t_1+t_2;0,1]\) - 添加尺度：
\(P_2=T_{21}T_{1w}P_w=(r_1,s_1,t_1)(r_2,s_2,t_2)P_w=r_1s_1(r_2s_2P_w+t_2)+t_1 =r_1r_2s_1s_2P_w+r_1s_1t_2+t_1\)
所以：\(R=r_1r_2,t=\frac {r_1s_1t_2+t_1}{s_1s_2}\)