第7讲3 三角测量

最新推荐文章于 2024-04-10 18:26:11 发布

不服输的小白

最新推荐文章于 2024-04-10 18:26:11 发布

阅读量1.3w

点赞数 16

分类专栏：视觉SLAM

本文链接：https://blog.csdn.net/llfjcmx/article/details/83410318

版权

视觉SLAM 专栏收录该内容

14 篇文章 18 订阅

订阅专栏

之所以把SLAM初始化和三角测量放在一起是因为它们之间有一定的关系，理解了三角测量之后才能理解初始化。

三角测量

在得到了相机的运动之后，下一步我们需要用相机的运动来估计特征点的空间位置，但是在单目SLAM中，仅通过单张图像是无法获得像素的深度信息的，我们需要用三角测量（三角化）的方法来估计地图点的深度。

先来看一些什么是三角测量？三角测量是指通过在两处观察同一个点的夹角，来确定该点的距离。以下图为例：

考虑图像 $I_{1},I_{2}$ ，相机的光心为 $O_{1},O_{2}$ ，以左图为参考，右图的变换矩阵为。

假设在 $I_{1}$ 中有特征点 $p_{1}$ ，对应到 $I_{2}$ 中的特征点 $p_{2}$ ，理论上讲 $O_{1}p_{1}$ 和 $O_{2}p_{2}$ 会相交于某一点，该点即是两个特征点所对应的地图点在三维场景中的位置，但是由于噪声的影响，这两条直线往往无法相交，因此可以通过最小二乘法来求解出距离最近的那个点作为相交点。

按照对极几何中的定义，如果设 $x_1,x_{2}$ 为两个特征点的归一化坐标，于是有下列关系式：

$s_{1}x_{1}=s_{2}Rx_{2}+t$

ps: 我对上面这个公式不是很理解或者说看法不一样。 ${\color{Blue} x_{1}}$ 是归一化坐标，所以 ${\color{Blue} s_{1}x_{1}}$ 可以认为是去归一化的坐标（也就是相机光心为 ${\color{Blue} O_{1}}$ 时的相机坐标），同理 ${\color{Blue} s_{2}x_{2}}$ 为 ${\color{Blue} P}$ 点在相机光心为 ${\color{Blue} O_{2}}$ 时的相机坐标，所以我认为是 ${\color{Blue} s_{2}x_{2}=s_{1}Rx_{1}+t}$ 。但是计算 ${\color{Blue} s_{1},s_{2}}$ 的思路还是不变的。

上面公式中的R、t、 $x_{1},x_{2}$ 都是知道的，现在要求的就是两个特征点的深度 $s_{1},s_{2}$ 。

我们可以分开来求，首先求 $s_{2}$ ：

上式的两边同时左乘 $x_{1}^{\Lambda }$ ，得到： $s_{1}x_{1}^{\Lambda }x_{1}=0=s_{2}x_{1}^{\Lambda }Rx_{2}+x_{1}^{\Lambda }t$

等式的左边可以看成是一个方程，只有 $s_{2}$ 是未知的，根据它可以直接求解出 $s_{2}$ 。

有了 $s_{2}$ ，反代入也可以很简单的求出 $s_{1}$ 。

但是，由于噪声的存在，我们求出来的 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;   
}