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\)
叉乘：
第一個式子：
\(T1_1、T1_2、T1_3\)都是1×4，是矩陣T1(3*4)的第1，2，3行。
可得到：
同理，第二個式子可得到
綜合：
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)
5.旋轉矩陣
\(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}\)