ORB-SLAM2学习（原理）：Frame.cc_orbslam原理详解-CSDN博客

径向畸变：由透镜形状引起的畸变称之为径向畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线。可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。它们主要分为两大类，桶形畸变（中）和枕形畸变（右）。

桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。
在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。

切向畸变：除了透镜的形状会引入径向畸变外，在相机的组装过程中由于不能使得透镜和成像面
严格平行也会引入切向畸变。

为更好地理解径向畸变和切向畸变，我们用更严格的数学形式对两者进行描述。我们知道平面上的任意一点 $\boldsymbol{\emph{p}}$ 可以用笛卡尔坐标表示为 $[x,y]^{T}$ , 也可以把它写成极坐标的形式

正模型，比如只选择k1; p1; p2 这三项等。

值得一提的是，存在两种去畸变处理（Undistort，或称畸变校正）做法。我们可以选择先对整张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置。或者，我们也可以先考虑图像中的某个点，然后按照去畸变方程，讨论它去畸变后的空间位置。二者都是可行的，不过前者在视觉SLAM 中似乎更加常见一些。显然ORB-SLAM2是用后者。

（2）计算图像边界，Frame::ComputeImageBounds(const cv::Mat &imLeft)

参考链接

ORB2单目读代码笔记7--去畸变、计算图像边界、特征点网格划分_不能再吃了OvO的博客-CSDN博客

(01)ORB-SLAM2源码无死角解析-(12)图像、特征点、关键点去畸变_江南才尽，年少无知！的博客-CSDN博客

基本原理

这个过程一般是在第一帧或者是相机标定参数发生变化之后进行。初始图像四个顶点坐标赋值给mat矩阵：

对这四个点进行畸变矫正cv::undistortPoints。

cv::undistortPoints(mat,          //输入点的mat矩阵
                    mat,          //输出点的mat矩阵
                    mK,           //相机的内参数矩阵
                    mDistCoef,    //去畸变参数
                    cv::Mat(),    //是一个3✖3的旋转矩阵，仅对双目相机有效，使左右极线平行
                    mK);          //P是投影矩阵,在单目相机中， Tx=Ty=0

相机参数DKRP的解释_td092的博客-CSDN博客

OpenCV去畸变undistortPoints原理解析

最后结果是这样的图像。

（3）确定特征点所在的图像栅格，Frame::AssignFeaturesToGrid()

参考链接ORB-SLAM2 Frame.cc部分读代码二-pudn.com

ORB-SLAM2代码（四）特征点匹配_陋室逢雨的博客-CSDN博客_orbslam特征点匹配

函数作用： 找到在以x,y为中心，半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点。传入的r为windowSize = 100。

基本原理:

1.给每个格子存储的特征点队列 mGrid 预先分配存储空间。

2. 遍历每个去畸变后的特征点，如果特征点的坐标在网格范围内则将特征点的索引写到对应的mGrid 中。

mGrid的用处可以参考下图：

（图来源ORB-SLAM2代码（四）特征点匹配_陋室逢雨的博客-CSDN博客_orbslam特征点匹配）

问题：这个0.5是为什么呢？希望有大佬指点一下

void Frame::AssignFeaturesToGrid()
{
    // Step 1  给存储特征点的网格数组 Frame::mGrid 预分配空间
	// ? 这里0.5 是为什么？节省空间？
    // FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48
    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
	//开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间
    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
            mGrid[i][j].reserve(nReserve);

（4）特征点所在网格的网格坐标，Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)

其中，Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)，计算某个特征点所在网格的网格坐标，如果找到特征点所在的网格坐标，记录在nGridPosX,nGridPosY里，返回true，没找到返回false。

（5）设置相机姿态，Frame::SetPose(cv::Mat Tcw)

给新的帧设置Pose

（6）获得旋转矩阵和变换矩阵，Frame::UpdatePoseMatrices()

参数	含义
mOw	当前相机光心在世界坐标系下坐标
mTcw	世界坐标系到相机坐标系的变换矩阵
mRcw	世界坐标系到相机坐标系的旋转矩阵
mtcw	世界坐标系到相机坐标系的平移向量
mRwc	相机坐标系到世界坐标系的旋转矩阵

（7）获得特定区域内的坐标点，Frame::GetFeaturesInArea

参考链接ORB-SLAM2 ---- Frame::GetFeaturesInArea 函数_Courage2022的博客-CSDN博客_getfeaturesinarea

判断frame是否已创建_ORB SLAM2源码解读(三)：Frame类_weixin_39747399的博客-CSDN博客

基本作用:

其作用是找到在以x, y为中心,边长为2r的方形内且在[minLevel, maxLevel]的特征点

（8）从双目中恢复深度，Frame::ComputeStereoMatches()

其作用是为左图的每一个特征点在右图中找到匹配点，根据基线(有冗余范围)上描述子距离找到匹配，再进行SAD精确定位，最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对，然后利用抛物线拟合得到亚像素精度的匹配，匹配成功后会更新 mvuRight 和 mvDepth

（9）从RGBD相机中获得深度，Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)

根据像素坐标获取深度信息，如果深度存在则保存下来，这里还计算了假想右图的对应特征点的横坐标

（10）计算特征点在三维空间的坐标Frame::UnprojectStereo(const int &i)

其作用是将特征点坐标反投影到3D地图点（世界坐标），在已知深度的情况下，则可确定二维像素点对应的尺度，最后获得3D中点坐标

（11）Frame::ComputeBoW()

参考链接：ORB-SLAM2 ---- Frame::ComputeBoW函数_Courage2022的博客-CSDN博客

【ORB SLAM 2的描述子计算和描述子汉明距离的计算代码解析】_楚歌again的博客-CSDN博客_orb 汉明距离

函数作用： 计算当前帧特征点对应的词袋Bow，即输入特征点的描述子，输出词袋向量mBowVec（记录的是单词的id及其对应权重TF-IDF值）和特征向量mFeatVec（记录node id及其对应的图像 feature对应的索引）。

上层调用

    bool Tracking::TrackReferenceKeyFrame()
    {
        // Compute Bag of Words vector
        // Step 1：将当前帧的描述子转化为BoW向量
        mCurrentFrame.ComputeBoW();

函数本身

/**
 * @brief 计算当前帧特征点对应的词袋Bow，主要是mBowVec 和 mFeatVec
 * 
 */
void Frame::ComputeBoW()
{
	
    // 判断是否以前已经计算过了，计算过了就跳过
    if(mBowVec.empty())
    {
		// 将描述子mDescriptors转换为DBOW要求的输入格式
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
		// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
        mpORBvocabulary->transform(vCurrentDesc,	//当前的描述子vector
								   mBowVec,			//输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
								   mFeatVec,		//输出，记录node id及其对应的图像 feature对应的索引
								   4);				//4表示从叶节点向前数的层数
    }
}

其中Converter::toDescriptorVector(mDescriptors)，描述子mDescriptors的数据格式为Mat::zeros((int)keypoints.size(), 32, CV_8UC1);行是(int)keypoints.size()特征点的数量，列是32，每个元素就是CV_8UC1，例如：

[116,  89,  30,  96,   9, 205,  83, 164, 235, 184, 174,   8, 246, 243,  65, 114, 128, 244, 108,  74,   9,  33, 218,  48, 249, 185,  37,  89, 192,  51,  66,  35]

1*32,每个元素是8bite uint,如116=01110100

toDescriptorVector将描述子格式转换为词袋需要的格式：

基本原理: