ORB-SLAM2 ---- Frame::UndistortKeyPoints函数

最新推荐文章于 2024-05-31 23:45:00 发布

APS2023

最新推荐文章于 2024-05-31 23:45:00 发布

阅读量570

点赞数 2

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签： c++ orb-slam slam 计算机视觉图像处理

本文链接：https://blog.csdn.net/qq_41694024/article/details/126315228

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 46 订阅

订阅专栏

1.这个函数主要做什么

用内参对特征点去畸变，结果报存在mvKeysUn中。

2.单目相机的畸变模型

        为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入会对成像过程中光线的传播产生新的影响：

        一是透镜自身的形状对光线传播的影响。

        二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使光线穿过透镜投影到成像面时的位置发生变化。
        由透镜形状引起的畸变（Distortion，也叫失真）称为径向畸变。

        在针孔模型中，一条直线投影到像素平面上还是一条直线。

        可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称它们主要分为两大类：桶形畸变和枕形畸变
        桶形畸变图像放大率随着与光轴之间的距离增加而减小而枕形畸变则恰好相反。

        在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
        除了透镜的形状会引入径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行,所以也会引人切向畸变。

        为了更好地理解径向畸变和切向畸变,我们用更严格的数学形式对两者进行描述。考虑归一化平面上的任意一点 $p$ ，它的坐标为 $[x,y]^{T}$ ，也可写成极坐标的形式 $[r,\theta ]^{T}$ ，其中 $r$ 表示点 $p$ 与坐标系原点之间的距离， $\theta$ 表示与水平轴的夹角。

        径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。

        切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。

        通常假设这些畸变呈多项式关系，即：

        其中， $[x_{distorted},y_{distorted}]^{T}$ 是畸变后点的归一化坐标。另外，对于切向畸变，可以便用另外两个参数 $p1,p2$ 进行纠正：

         因此，对于相机坐标系中的一点 $P$ ，我们能够通过5个畸变系数找到这个点在像素平面上的正确位置：

    （注意，式中的 $x,y,r,\theta$ 是失真即修正前像素坐标， $x_{distorted},y_{distorted}$ 为修正后的像素坐标，像素坐标是先归一化再进行去畸变再计算成像素坐标的，切记！）

3.代码解析

void Frame::UndistortKeyPoints()
{
    // Step 1 如果第一个畸变参数为0，不需要矫正。第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0
	//变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)
    if(mDistCoef.at<float>(0)==0.0)
    {
        mvKeysUn=mvKeys;
        return;
    }


    // Step 2 如果畸变参数不为0，用OpenCV函数进行畸变矫正
    // Fill matrix with points
    // N为提取的特征点数量，为满足OpenCV函数输入要求，将N个特征点保存在N*2的矩阵中
    cv::Mat mat(N,2,CV_32F);
	//遍历每个特征点，并将它们的坐标保存到矩阵中
    for(int i=0; i<N; i++)
    {
		//然后将这个特征点的横纵坐标分别保存
        mat.at<float>(i,0)=mvKeys[i].pt.x;
        mat.at<float>(i,1)=mvKeys[i].pt.y;
    }

    // Undistort points
    // 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变
    //为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y） 
    mat=mat.reshape(2);
    cv::undistortPoints(	
		mat,				//输入的特征点坐标
		mat,				//输出的校正后的特征点坐标覆盖原矩阵
		mK,					//相机的内参数矩阵
		mDistCoef,			//相机畸变参数矩阵
		cv::Mat(),			//一个空矩阵，对应为函数原型中的R
		mK); 				//新内参数矩阵，对应为函数原型中的P
	
	//调整回只有一个通道，回归我们正常的处理方式
    mat=mat.reshape(1);

    // Fill undistorted keypoint vector
    // Step 存储校正后的特征点
    mvKeysUn.resize(N);
	//遍历每一个特征点
    for(int i=0; i<N; i++)
    {
		//根据索引获取这个特征点
		//注意之所以这样做而不是直接重新声明一个特征点对象的目的是，能够得到源特征点对象的其他属性
        cv::KeyPoint kp = mvKeys[i];
		//读取校正后的坐标并覆盖老坐标
        kp.pt.x=mat.at<float>(i,0);
        kp.pt.y=mat.at<float>(i,1);
        mvKeysUn[i]=kp;
    }
}

    ///原始左图像提取出的特征点（未校正）
    std::vector<cv::KeyPoint> mvKeys;
    ///原始右图像提取出的特征点（未校正）
    std::vector<cv::KeyPoint> mvKeysRight;
	///校正mvKeys后的特征点
    std::vector<cv::KeyPoint> mvKeysUn;

首先我们判断它是否要去畸变（因为如果第一个畸变参数为0，则不需要矫正），直接让原来的特征点作为矫正后的特征点即可。

如果存在畸变参数，则创建N*2的矩阵存放所有特征点的x，y值，调用opencv的去畸变函数undistortPoints（原理如上），主要作用是计算出去畸变后的特征点坐标覆盖mat矩阵，然后用mvKeysUn容器存储矫正后的特征点坐标。