视觉SLAM十四讲笔记-7-3

视觉SLAM十四讲笔记-7-3

文章目录

  • 视觉SLAM十四讲笔记-7-3
    • 7.5 三角测量
    • 7.6 实践:三角测量
      • 讨论

7.5 三角测量

之前两节使用了对极几何约束估计了相机运动,也讨论了这种方法的局限性。在得到运动之后,下一步需要用相机的运动估计特征点的空间位置。在单目SLAM中,仅通过单张图像无法获得像素的深度信息,需要通过三角测量(Triangulation)(或三角化)的方法估计地图点的深度。
视觉SLAM十四讲笔记-7-3_第1张图片
三角测量是指,通过不同位置对同一个路标点进行观察,从观察到的位置推断出路标点的距离。三角测量最早由高斯提出并应用于测量学中。在SLAM中,主要用三角测量来估计相机像素点的距离
和上一节类似,考虑图像 I 1 I_1 I1 I 2 I_2 I2,以左图为参考,右图的变换矩阵为 T T T。相机光心为 O 1 O_1 O1 O 2 O_2 O2。在 I 1 I_1 I1 中特征点 p 1 p_1 p1,在 I 2 I_2 I2 中特征点 p 2 p_2 p2。理论上,直线 O 1 p 1 O_1p_1 O1p1 O 2 p 2 O_2p_2 O2p2 在场景中会交于一点 P P P,该点即两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的问题,这两条直线往往无法相交。因此,可以通过最小二乘法求解
按照对极几何中的定义,设 x 1 x_1 x1 x 2 x_2 x2 为两个特征点的归一化坐标,它们之间满足:

s 2 x 2 = s 1 R x 1 + t s_2x_2 = s_1Rx_1 + t s2x2=s1Rx1+t
现在已知 R , t R,t R,t,想求解两个特征点的深度 s 1 s_1 s1 s 2 s_2 s2。从几何上看,可以在射线 O 1 p 1 O_1p_1 O1p1 上寻找 3D 点,使其投影位置接近 p 2 p_2 p2。同理,也可以在 O 2 p 2 O_2p_2 O2p2 上找,或者在两条线的中间找。
不同的策略对应着不同的计算公式,当然它们大同小异。例如,希望计算 s 1 s_1 s1,那么先对上式两侧左乘一个 x 2 ∧ x_2^\wedge x2,得:
s 2 x 2 ∧ x 2 = 0 = s 1 x 2 ∧ R x 1 + x 2 ∧ t s_2x_2^\wedge x_2 = 0 = s_1x_2^\wedge Rx_1 + x_2^\wedge t s2x2x2=0=s1x2Rx1+x2t
该式左侧为零,右侧可以看成 s 1 s_1 s1 的方程,可以根据它直接求得 s 1 s_1 s1。有了 s 1 s_1 s1 s 2 s_2 s2 也非常容易求出。
于是,就得到了两帧下点的深度,确定了它们的空间坐标。当然,由于噪声的存在,估得的 R , t R,t R,t 不一定使得上式为零,所以更为常见的做法是求最小二乘解而不是直接解。

7.6 实践:三角测量

mkdir triangulation
cd triangulation
code .
//launch.json
{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "g++ - 生成和调试活动文件",
            "type": "cppdbg",
            "request":"launch",
            "program":"${workspaceFolder}/build/triangulation",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启动整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "Build",
            "miDebuggerPath": "/usr/bin/gdb"
        }
    ]
}
//tasks.json
{
	"version": "2.0.0",
	"options":{
		"cwd": "${workspaceFolder}/build"   //指明在哪个文件夹下做下面这些指令
	},
	"tasks": [
		{
			"type": "shell",
			"label": "cmake",   //label就是这个task的名字,这个task的名字叫cmake
			"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
			"args":[
				".."
			]
		},
		{
			"label": "make",  //这个task的名字叫make
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"command": "make",  //这个task要执行的任务是make
			"args": [

			]
		},
		{
			"label": "Build",
			"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
			"dependsOn":[				//这个label依赖于上面两个label
				"cmake",
				"make"
			]
		}
	]
}
#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(ORBCV)

#在g++编译时,添加编译参数,比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
set(CMAKE_CXX_FLAGS "-std=c++11")

#一定要加上这句话,加上这个生成的可执行文件才是可以Debug的,不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

#此工程要调用opencv库,因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)

#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(triangulation triangulation.cpp)

#链接OpenCV库
target_link_libraries(triangulation ${OpenCV_LIBS})
#include 
#include 
// #include "extra.h" // used in opencv2
using namespace std;
using namespace cv;

void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keypoints_1,
    std::vector<KeyPoint> &keypoints_2,
    std::vector<DMatch> &matches);

void pose_estimation_2d2d(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat &R, Mat &t);

void triangulation(
    const vector<KeyPoint> &keypoint_1,
    const vector<KeyPoint> &keypoint_2,
    const std::vector<DMatch> &matches,
    const Mat &R, const Mat &t,
    vector<Point3d> &points);

/// 作图用
inline cv::Scalar get_color(float depth)
{
    float up_th = 50, low_th = 10, th_range = up_th - low_th;
    if (depth > up_th)
        depth = up_th;
    if (depth < low_th)
        depth = low_th;
    return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}

// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv)
{
    //
    // if(argc != 3)
    // {
    //     cout << "usage: feature_extraction img1 img2" << endl;
    //     return 1;
    // }
    // //---读取图像
    // Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    // Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    // assert(img_1.data != nullptr && img_2.data != nullptr);
    //

    //---读取图像
    Mat img_1 = imread("./1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("./2.png", CV_LOAD_IMAGE_COLOR);

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;

    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;

    //估计两张相机间的运动
    Mat R, t;
    pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t); //调用函数

    //三角化
    vector<Point3d> points;
    triangulation(keypoints_1, keypoints_2, matches, R, t, points);

    //---验证三角化点与特征点的重投影关系
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    Mat img1_plot = img_1.clone();
    Mat img2_plot = img_2.clone();
    for (int i = 0; i < matches.size(); ++i)
    {
        //第一个图
        float depth1 = points[i].z;
        cout << "depth:" << depth1 << endl;
        Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
        cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);

        //第二个图,把第一个点在相机坐标系下的坐标转化到第二个相机坐标系下
        Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;
        float depth2 = pt2_trans.at<double>(2, 0);
        cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);
    }
    cv::imshow("img 1", img1_plot);
    cv::imshow("img 2", img2_plot);
    cv::waitKey();

    return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches)
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr detector = FeatureDetector::create ( "ORB" );
    // Ptr descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist)
            min_dist = dist;
        if (dist > max_dist)
            max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }
}

//估计两张图像间运动
void pose_estimation_2d2d(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat &R, Mat &t)
{
    //相机内参,TUM Freiburg2
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

    //---把匹配点转换为vector的形式
    vector<Point2f> points1;
    vector<Point2f> points2;

    for (int i = 0; i < (int)matches.size(); ++i)
    {
        points1.push_back(keypoints_1[matches[i].queryIdx].pt);
        points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    }

    //---计算本质矩阵
    Point2d principal_point(325.1, 249.7); //相机光心,TUM dataset标定值
    double focal_length = 521;             //相机焦距,TUM dataset标定值
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    cout << "essential_matrix is " << endl
         << essential_matrix << endl;

    //---从本质矩阵中恢复旋转和平移矩阵
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    // cout << "R is" << endl << R << endl;
    // cout << "t is" << endl << t << endl;
}

void triangulation(
    const vector<KeyPoint> &keypoint_1,
    const vector<KeyPoint> &keypoint_2,
    const std::vector<DMatch> &matches,
    const Mat &R, const Mat &t,
    vector<Point3d> &points)
{
    Mat T1 = (Mat_<float>(3, 4) << 1, 0, 0, 0,
              0, 1, 0, 0,
              0, 0, 1, 0);
    Mat T2 = (Mat_<float>(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) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

    vector<Point2f> pts_1, pts_2;
    //将匹配的特征点坐标由像素坐标系变换到相机归一化平面
    for (DMatch m : matches)
    {
        // 将像素坐标转换至相机坐标
        pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
        pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
    }
    Mat pts_4d;
    cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

    //转换成非齐次坐标
    for (int i = 0; i < pts_4d.cols; ++i)
    {
        Mat x = pts_4d.col(i);
        x /= x.at<float>(3, 0); //归一化
        Point3d p(
            x.at<float>(0, 0),
            x.at<float>(1, 0),
            x.at<float>(2, 0));
        points.push_back(p);
    }
}

Point2f pixel2cam(const Point2d &p, const Mat &K)
{
    return Point2f(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

运行结果:
视觉SLAM十四讲笔记-7-3_第2张图片
视觉SLAM十四讲笔记-7-3_第3张图片
视觉SLAM十四讲笔记-7-3_第4张图片

讨论

三角测量是由平移得到的,有平移才会有对极几何的三角形,才谈得上三角测量。因此,纯旋转是无法使用三角测量的,因此在平移为零时,对极约束一直为零。当然,实际数据往往不能完全为零。在平移存在的情况下,还要关心三角测量的不确定性,这会引出一个三角测量的矛盾

当平移很小时,像素上的不确定性将导致较大的深度不确定性。
也就是说,如果特征点运动一个像素 δ x \delta x δx ,使得视线角变化了一个角度 δ θ \delta \theta δθ ,那么将测量到深度值有 δ d \delta d δd 的变化。从几何关系可以看到,当 t t t 较大时, δ d \delta d δd 明显变小,这说明平移较大时,在同样的相机分辨率下,三角化测量将更精确。对该过程的定量分析可以使用余弦定理得到。
因此,要提高三角化的精度,一种方式是提高特征点的精度,也就是要提高图像分辨率—但这会使得图像变大,增加计算成本。另一种是使平移量增大。但是,这会导致图像的外观发生明显的变化。外观变化会使得特征提取与匹配变得困难。这就是三角化的矛盾,也称为视差:增大平移,可能会导致匹配失效;而平移太小,则三角化精度不够。
在单目视觉中,由于单目图像没有深度信息,我们要
等待特征点被追踪几帧
后,产生了足够的视角,再用三角化来确定新增特征点的深度值。这有时也被称为延迟三角化。但是,如果相机发生了原地旋转,导致视差很小,就不好估计新观测到的特征点的深度。这种情况在机器人场合下更常见,因为原地旋转往往是一个机器人场景的指令。在这种情况下,单目视觉就可能出现追踪失败、尺度不正确等情况。
虽然本节只介绍了三角化的深度估计,但也能够定量地计算每个特征点地位置以及不确定性。所以,如果假设特征点服从高斯分布,并且不断地对它进行观测,在信息正确地情况下,我们就能够期望它地方差不断减小乃至收敛。这就得到了一个滤波器,称为深度滤波器Depth Filter。不过,由于它地原理较复杂,将在后续章节中进行详细讨论。下面讨论,如何通过3D-2D地匹配点估计相机运动,以及3D-2D的估计方法。

你可能感兴趣的:(视觉slam十四讲笔记,经验分享)