線性三角化法 Triangulate

定義：slam中的三角測量指通過不同位置觀測一個三維空間中特征點的夾角，從而測得點的深度值；同理亦可通過三角化恢復二維特征點的三維坐標，單目情況下由於平移t尺度的不確定性只能恢復相對三維坐標。

考慮圖像 I1 和 I2，以左圖為參考，右圖的變換矩陣為 T。相機光心為 O1 和 O2。在 I1 中有特征點 p1，對應 I2 中有特征點 p2。理論上直線 O1p1 與 O2p2 在場景中會相交於一點 P，該點即是兩個特征點所對應的地圖點在三維場景中的位置。

然而由於噪聲的影響，這兩條直線往往無法相交 因此，也可以通過最二小乘去求解。

 

---

方法：

目標：在此使用DLT恢復$p_1$  ,$p_2$  對應的三維坐標 X；

已知：相機內參K ，外參$T_1$，$T_2$  三維點P對應的圖像點$p_1$  ,$p_2$像素坐標$x_1=\begin{pmatrix}u_1\\v_1\\1\end{pmatrix}$  ,$x_2=\begin{pmatrix}u_2\\v_2\\1\end{pmatrix}$；

 

相機矩陣 $P=KT$        其中  $K 3 \times3$  ,    $T=[R|t]  3\times4$  ;

像素坐標 $x_1=KT_1X=P_1X$;

　　　　$x_2=KT_2X=P_2X$;

通過叉乘消去純量因子，使每個圖像點得到三個方程，其中兩個是線性獨立的，對於第一幅圖像有；

$x_1^{\wedge}x_1=x_1^{\wedge}PX=0$

其中 $x_1^{\wedge}=\begin{pmatrix}0&-1&v_1\\1&0&-u_1\\-v_1&u_1&0\end{pmatrix}$

展開得

$u_1\left(P^3_1X\right)-\left(P^1_1X\right)=0$

$v_1\left(P^3_1X\right)-\left(P^2_1X\right)=0$

$u_1\left(P^2_1X\right)-v_1\left(P^1_1X\right)=0$

其中$P^1$ 是P的行，這些關於X的分量是線性的；

 

同理對於第二幅圖像也可以得到三個方程；

 

各取兩個方程得到四個齊次未知量為四的方程，組成$AX=0$的方程；

$A=\left(\begin{array}{cc}{u_{1} P_{1}^{3}} & {-P_{1}^{1}} \\ {v_{1} P_{1}^{3}} & {-P_{1}^{2}} \\ {u_{2} P_{2}^{3}} & {-P_{2}^{1}} \\ {v_{2} P_{2}^{3}} & {-P_{2}^{2}}\end{array}\right)$

通過SVD解 $AX=0$  得到X

 

---

代碼

**
 * @brief 給定投影矩陣P1,P2和圖像上的點kp1,kp2，從而恢復3D坐標
 *
 * @param kp1 特征點, in reference frame
 * @param kp2 特征點, in current frame
 * @param P1  投影矩陣P1
 * @param P2  投影矩陣P２
 * @param x3D 三維點
 * @see       Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312      chinese 218
 */
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    // 在DecomposeE函數和ReconstructH函數中對t有歸一化
    // 這里三角化過程中恢復的3D點深度取決於 t 的尺度，
    // 但是這里恢復的3D點並沒有決定單目整個SLAM過程的尺度
    // 因為CreateInitialMapMonocular函數對3D點深度會縮放，然后反過來對 t 有改變

    cv::Mat A(4,4,CV_32F);

    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

    //Ax=0
    cv::Mat u,w,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();//豎排
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);//齊次  尺度變換成1
}