ORB-SLAM2源码学习：Initializer.cc⑧: Initializer::CheckRT检验三角化结果

PaLu-LvL

已于 2025-01-22 15:21:17 修改

阅读量1.3k

点赞数 10

CC 4.0 BY-SA版权

分类专栏：计算机视觉 # ORB-SLAM2 # 初始化文章标签： c++ 计算机视觉 opencv ubuntu 人工智能学习

于 2025-01-22 13:15:53 首次发布

本文链接：https://blog.csdn.net/2301_76831056/article/details/145300799

前言

ORB-SLAM2源码学习：Initializer.cc⑦: Initializer::Triangulate特征点对的三角化_cv::svd::compute-CSDN博客

经过上面的三角化我们成功得到了三维点，但是经过三角化成功的三维点并不一定是有效的，需要筛选才能作为初始化地图点。

1.函数声明

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
                       const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,
                       const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)

2.函数定义

我们把相机1的光轴中心作为世界坐标系的原点，从相机1到相机2的位姿：

则从相机2到相机1的位姿：

对应关系：

则相机2的光轴中心O2在相机1的坐标系下的坐标为：

计算相机2 的光轴中心O2 在相机1 坐标系下的坐标是为了求解三维点分别在两个坐标系下和光轴中心的夹角。如果我们要求向量夹角，那么前提是这些向量都在同一个坐标系下。我们看相机1坐标系，此时O1是相机1 的光轴中心，也是相机1 坐标系原点， P3d 是相机1坐标系（世界坐标系）下的三维点，这就必须得到O2在相机1 坐标系下的坐标，也就是我们前面推导的过程。

计算夹角是为了判断三维点的有效性，因为初始化地图点（三角化得到的三维点）特别重要，后续跟踪都是以此为基础的，所以在确定三维点时要非常小心。确定一个合格的三维点需要通过以下条件：

1.三维点的3 个坐标都必须是有限的实数。

2.三维点深度值必须为正。

3.三维点和两帧图像光轴中心夹角需要满足一定的条件。夹角越大，视差越大，三角化结果越准确。

4. 三维点的重投影误差小于设定的阈值。

经过上面条件的筛选，最后剩下的三维点才是合格的三维点。我们会记录当前位姿对应的合格三维点数目和视差。

具体的流程

1.计算前的参数准备与声明

// 对给出的特征点对及其R t , 通过三角化检查解的有效性，也称为 cheirality check

    // Calibration parameters
	//从相机内参数矩阵获取相机的校正参数
    const float fx = K.at<float>(0,0);
    const float fy = K.at<float>(1,1);
    const float cx = K.at<float>(0,2);
    const float cy = K.at<float>(1,2);

	//特征点是否是good点的标记，这里的特征点指的是参考帧中的特征点
    vbGood = vector<bool>(vKeys1.size(),false);
	//重设存储空间坐标的点的大小
    vP3D.resize(vKeys1.size());

	//存储计算出来的每对特征点的视差
    vector<float> vCosParallax;
    vCosParallax.reserve(vKeys1.size());

2.构建投影矩阵计算相机光心2在世界坐标系下的坐标

这里与前边推导的过程一致。

 // Camera 1 Projection Matrix K[I|0]
    // Step 1：计算相机的投影矩阵  
    // 投影矩阵P是一个 3x4 的矩阵，可以将空间中的一个点投影到平面上，获得其平面坐标，这里均指的是齐次坐标。
    // 对于第一个相机是 P1=K*[I|0]
 
    // 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵
    cv::Mat P1(3,4,				//矩阵的