2D-2D 3D-2D

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\)
叉乘：

\[\left\{ \begin{matrix} (p_{1})_{×}*T_1 \\ (p_{2})_{×}*T_2 \end{matrix} \right\} * P_w = 0 \]

​ 第一个式子：

\[ (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} T1_2-y1_c*T1_3 \\ T1_1-x1_c*T1_3 \end{matrix} \right\} *P_w =0 \]

​ 同理，第二个式子可得到

\[\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}\)

标签：wedge,matrix,自由度,矩阵,T1,2D,row,3D
来源： https://www.cnblogs.com/recordmoment/p/12560698.html