之所以把SLAM初始化和三角測量放在一起是因為它們之間有一定的關係，理解了三角測量之後才能理解初始化。

三角測量

       在得到了相機的運動之後，下一步我們需要用相機的運動來估計特徵點的空間位置，但是在單目SLAM中，僅通過單張影象是無法獲得畫素的深度資訊的，我們需要用三角測量（三角化）的方法來估計地圖點的深度。

先來看一些什麼是三角測量？三角測量是指通過在兩處觀察同一個點的夾角，來確定該點的距離。以下圖為例：

考慮影象，相機的光心為，以左圖為參考，右圖的變換矩陣為。

假設在中有特徵點，對應到中的特徵點，理論上講和會相交於某一點，該點即是兩個特徵點所對應的地圖點在三維場景中的位置，但是由於噪聲的影響，這兩條直線往往無法相交，因此可以通過最小二乘法來求解出距離最近的那個點作為相交點。

按照對極幾何中的定義，如果設為兩個特徵點的歸一化座標，於是有下列關係式：

ps: 我對上面這個公式不是很理解或者說看法不一樣。是歸一化座標，所以可以認為是去歸一化的座標（也就是相機光心為時的相機座標），同理為點在相機光心為時的相機座標，所以我認為是。但是計算的思路還是不變的。

上面公式中的R、t、都是知道的，現在要求的就是兩個特徵點的深度。

我們可以分開來求，首先求：

上式的兩邊同時左乘，得到：

等式的左邊可以看成是一個方程，只有是未知的，根據它可以直接求解出。

有了，反代入也可以很簡單的求出。

但是，由於噪聲的存在，我們求出來的不一定能夠使得上式精確等於0，因此在實際情況中，更常見的做法是求最小二乘解而不是零解。

實踐部分：如何使用OpenCV進行三角測量

下面是進行三角化的簡單思路：

下面是用OpenCV進行實現的程式碼:

triangulation.hpp

#include "feature_extract_match.hpp"
#include "estimation.hpp"

//畫素座標轉換為歸一化座標,返回的是(X/Z, Y/Z), 真正的歸一化座標為(X/Z, Y/Z, 1)
Point2d pixel_to_camera(Point2d p, Mat K)
{
    return Point2d(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

//返回的是空間三維點
void triangulation(vector<KeyPoint> key_points_1, vector<KeyPoint> key_points_2, vector<DMatch> matches, Mat R, Mat t, vector<Point3d> &space_points)
{
    Mat T1 = (Mat_<double>(3, 4) << 
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0
    );

    //注意Mat_類的使用
    Mat T2 = (Mat_<double> (3, 4) << 
        R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
    );

    //定義相機的內參矩陣
    Mat K = (Mat_<double>(3, 3) <<
        529.0, 0, 325.1,
        0, 521.0, 249.9,
        0, 0, 1
    );

    //將所有匹配的特徵點轉化為歸一化座標
    //KeyPoints ---> Point2f
    vector<Point2d> points_1, points_2;
    for(int i=0; i< matches.size(); i++)
    {
        points_1.push_back( pixel_to_camera(key_points_1[matches[i].queryIdx].pt, K) );
        points_2.push_back( pixel_to_camera(key_points_2[matches[i].trainIdx].pt, K) );
    }

    /*
    呼叫cv::triangulatePoints(InputArray projMatr1, InputArray projMatr2, InputArray projPoints1, InputArray projPoints2, OutputArray points4D)進行三角測量
        引數1: 第一張影象的[R, t]組成的3*4矩陣
        引數2: 第二張影象的[R, t]組成的3*4矩陣
        引數3: 第一張影象的匹配的特徵點的歸一化座標, 型別為vector<Point2d>
        引數4: 第二張影象的匹配的特徵點的歸一化座標, 型別為vector<Point2d>
        引數5: 輸出的3d座標, 是一個4*N矩陣表示的齊次座標(每一列都是一個點的座標), 因此要將所有的元素除以最後一維的數得到非齊次座標XYZ
    */
    Mat pts_4d;
    cv::triangulatePoints(T1, T2, points_1, points_2, pts_4d);

    // cout << "pts_4d = " << endl;
    // cout << pts_4d << endl; 

    //轉換為非齊次座標
    for(int i = 0; i < pts_4d.cols; i++)
    {
        Mat x = pts_4d.col(i);  //獲取第i列
        x /= x.at<double>(3, 0);    //歸一化
        space_points.push_back(
            Point3d(x.at<double>(0, 0), x.at<double>(1, 0), x.at<double>(2, 0))
        );
    }
}
 

test_triangulation.cpp

#include "feature_extract_match.hpp"
#include "estimation.hpp"
#include "triangulation.hpp"

int main(int argc, char **argv)
{
    Mat img1, img2;
    img1 = imread("../datas/1.png");
    img2 = imread("../datas/2.png");

    vector<KeyPoint> key_points_1, key_points_2;
    vector<DMatch> matches;
    feature_extract_match(img1, img2, key_points_1, key_points_2, matches);

    cout << "一共找到了： " << matches.size() << "個匹配點" << endl;

    Mat R, t;
    pose_estimation_2d2d(key_points_1, key_points_2, matches, R, t);

    cout << "R = " << R << endl;
    cout << "t = " << t << endl;

    //進行三角測量
    vector<Point3d> space_points;
    triangulation(key_points_1, key_points_2, matches, R, t, space_points);

    cout << "space_points.size() = " << space_points.size() << endl;

    //內參矩陣
    Mat K = (Mat_<double>(3, 3) <<
        529.0, 0, 325.1,
        0, 521.0, 249.9,
        0, 0, 1
    );
    //驗證三角化點和特徵點的重投影關係
    //驗證方法: 三角化點進行歸一化, 特徵點重投影到歸一化平面
    for(int i = 0; i < matches.size(); i++)
    {
        //將畫素點投影到歸一化平面上
        Point2d pix_cam_1 = pixel_to_camera(key_points_1[ matches[i].queryIdx ].pt, K);       
        //將空間點投影到歸一化平面上
        Point2d space_cam_1 (
            space_points[i].x / space_points[i].z,
            space_points[i].y / space_points[i].z
        );          
        cout << "pix_cam_1 in first frame is : " << pix_cam_1 << endl;
        cout << "space_cam_1 in first frame is : " << space_cam_1 << ", d = " << space_points[i].z << endl;

        //the second image
        //畫素座標直接可以轉換為歸一化座標
        Point2d pix_cam_2 = pixel_to_camera( key_points_2[ matches[i].trainIdx ].pt, K );
        //R * 世界座標 + t ----> I2的相機座標系下的座標,然後再投影到歸一化平面
        Mat point_cam_frame2 = R * (Mat_<double>(3, 1) << space_points[i].x, space_points[i].y, space_points[i].z) + t;
        point_cam_frame2 /= point_cam_frame2.at<double>(2, 0);  //歸一化
        // Point2d space_cam_2 (
        //     point_cam_frame2.at<double>(0, 0),
        //     point_cam_frame2.at<double>(1, 0)
        // );
        cout << "pix_cam_2 in two frame is : " << pix_cam_2 << endl;
        cout << "space_cam_2 in two frame is : " << point_cam_frame2.t() << endl;

        cout << endl;
    }

    return 0;   
}

討論:

       由於E(本質矩陣)本身具有尺度等價性,它分解得到的R、t也有一個尺度等價性。而本身具有約束,所以我們認為t具有一個尺度。換言之，在分解過程中，對t乘以任意非零常數，分解都是成立的。因此，我們通常把t進行歸一化，讓它的長度等於1.

       對t長度的歸一化，直接導致了單目視覺的尺度不確定性。因為對t乘以任意一個比例常數後，對極約束仍然是成立的。換言之，在單目SLAM中，對軌跡和地圖同時縮放任意倍數，我們得到的圖仍然是一樣的。

單目視覺的初始化問題

       在單目視覺中，我們對兩張影象的t進行歸一化，相當於固定了一個尺度。雖然我們不知道它的實際長度是多少，但是我們可以以這時的t為單目1，計算相機運動和特徵點的3D位置，這一步稱為單目SLAM的初始化。

       在初始化之後，就可以用3D-2D來計算相機的運動了，初始化之後的軌跡和地圖的單位，就是初始化時固定的尺度。因此，初始化也是單目SLAM中不可避免的一個步驟。

       初始化的兩張圖片必須要有一定程度的平移，而後的軌跡和地圖都將會以這一步的平移為單位。單目初始化不能只有純旋轉，必須要有一定程度的平移，如果沒有平移，單目將無法初始化。

這裡還要提一個三角化的相互矛盾的地方

       在同樣的相機解析度下，平移越大，三角化測量就會越精確；但是平移越大，將會導致影象的外觀發生明確變化，這會使得特徵的提取和匹配變得更困難---這就是三角化的矛盾。