第7讲3 三角测量

之所以把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(0, 2)) / K.at(0, 0),
        (p.y - K.at(1, 2)) / K.at(1, 1)
    );
}

//返回的是空间三维点
void triangulation(vector key_points_1, vector key_points_2, vector matches, Mat R, Mat t, vector &space_points)
{
    Mat T1 = (Mat_(3, 4) << 
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0
    );

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

    //定义相机的内参矩阵
    Mat K = (Mat_(3, 3) <<
        529.0, 0, 325.1,
        0, 521.0, 249.9,
        0, 0, 1
    );

    //将所有匹配的特征点转化为归一化坐标
    //KeyPoints ---> Point2f
    vector 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
        参数4: 第二张图像的匹配的特征点的归一化坐标, 类型为vector
        参数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(3, 0);    //归一化
        space_points.push_back(
            Point3d(x.at(0, 0), x.at(1, 0), x.at(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 key_points_1, key_points_2;
    vector 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 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_(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_(3, 1) << space_points[i].x, space_points[i].y, space_points[i].z) + t;
        point_cam_frame2 /= point_cam_frame2.at(2, 0);  //归一化
        // Point2d space_cam_2 (
        //     point_cam_frame2.at(0, 0),
        //     point_cam_frame2.at(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中不可避免的一个步骤。

       初始化的两张图片必须要有一定程度的平移，而后的轨迹和地图都将会以这一步的平移为单位。单目初始化不能只有纯旋转，必须要有一定程度的平移，如果没有平移，单目将无法初始化。

这里还要提一个三角化的相互矛盾的地方

       在同样的相机分辨率下，平移越大，三角化测量就会越精确；但是平移越大，将会导致图像的外观发生明确变化，这会使得特征的提取和匹配变得更困难---这就是三角化的矛盾。