DS-SLAM代码解读（三）

Tracking.cc -> Frame ->Frame.cc -> Constructor for RGB-D cameras for DS_SLAM

1. 下面对Frame()函数进行解读：

//Constructor for RGB-D cameras for DS_SLAM
Frame::Frame(
...
{
    ...
    // Scale Level Info,初始化图像金字塔的参数
    InitializeScaleLevels();
	...
    // ORB extraction，同ORBSLAM2类似，提取ORB特征,下面会在2.中展开解读这个特征点提取函数
    // ExtractORBKeyPoints中，(*mpORBextractorLeft)( imgray,cv::Mat(),mvKeysTemp)；调用了特征提取仿函数 提取ORB特征点，并进行了四叉树均匀化分配
    ExtractORBKeyPoints(0,imGray);
    ...
    cv::Mat  imGrayT = imGray;
    // Calculate the dynamic abnormal points and output the T matrix
    if(imGrayPre.data)//如果有动态物体
    {
        ...
        //移除的动态外点的函数，这个函数下面也会在3.中详细解读
        ProcessMovingObject(imGray);
        ...
        std::swap(imGrayPre, imGrayT);
    }
    else
    {
        std::swap(imGrayPre, imGrayT);
        flag_mov=0;
    }
}

2. ExtractORBKeyPoints(0,imGray)分析：

//ExtractORBKeyPoints()就在Frame.cc中
//ExtractORBKeyPoints()调用调用了特征提取仿函数，提取ORB特征点，并进行了四叉树均匀化分配
(*mpORBextractorLeft)( imgray,cv::Mat(),mvKeysTemp);

注意上面这里：
在ORBSLAM2里提取特征点和计算描述子是在特征提取仿函数里面一步完成的。
在DS-SLAM中，考虑到提取特征点里面可能有动态点，所以DS-SLAM中提取到的特征点暂时保存在 mvKeysTemp 里面，然后在语义分割擦除动态点之后才开始对剩下的特征点进行描述子的计算和去畸变等操作（具体方法是将计算描述子部分单独封装成一个函数,操作同ORB-SLAM2），以此提高效率。

//单独计算描述子部分函数，就在Frame.cc中
void Frame::ExtractORBDesp(int flag,const cv::Mat &imgray)

3. ProcessMovingObject(imGray)解读
   此函数作用是用几何方法进行运动一致性检测：
   step 1 :计算角点(像素级->亚像素级)
   step 2 :计算光流金字塔(确定角点1,2的匹配关系)
   step 3 :对于光流法得到的角点进行筛选(像素块内像素差的和小于阈值)
   step 4 :计算F矩阵(再对点进行了一次筛选)
   step 5 :根据角点到级线的距离小于0.1筛选最匹配的角点
   step 6:找到需要被删去的异常点

void Frame::ProcessMovingObject(const cv::Mat &imgray)
{
...
//step 1 调用opencv 函数 计算Harris 角点，将结果保存在 prepoint 矩阵当中
    //cv::goodFeaturesToTrack()提取到的角点只能达到像素级别
    //我们则需要使用cv::cornerSubPix()对检测到的角点作进一步的优化计算，可使角点的精度达到亚像素级别。
    // 调用opencv的函数,进行亚像素的角点检测，输出的角点还是放在 prepoint 里面
cv::goodFeaturesToTrack(imGrayPre, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);
cv::cornerSubPix(imGrayPre, prepoint, cv::Size(10, 10), cv::Size(-1, -1), 
...
//step 2 Lucas-Kanade方法计算稀疏特征集的光流。计算光流金字塔，光流金字塔是光流法的一种常见的处理方式，能够避免位移较大时丢失追踪的情况，高博的十四讲里面有讲
cv::calcOpticalFlowPyrLK(
	        imGrayPre,        // 输入图像1
	        imgray,           // 输入图像2 （t时间之后的）
	        prepoint,         // 输入图像1 的角点
	        nextpoint,        // 输出图像2 的角点
	        state,           // 记录光流点是否跟踪成功，成功status =1,否则为0
	        err,
	        cv::Size(22, 22),
	        5,       // 5层金字塔
	        cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS,20,0.01)
	        );
...
//step 3 对于光流法得到的 角点进行筛选。筛选的结果放入 F_prepoint F_nextpoint 两个数组当中。光流角点是否跟踪成功保存在status数组当中
for (int i = 0; i < state.size(); i++)
    {
        if(state[i] != 0)  // 光流跟踪成功的点
        {
            int dx[10] = { -1, 0, 1, -1, 0, 1, -1, 0, 1 };
            int dy[10] = { -1, -1, -1, 0, 0, 0, 1, 1, 1 };
            int x1 = prepoint[i].x, y1 = prepoint[i].y;
            int x2 = nextpoint[i].x, y2 = nextpoint[i].y;

            // 认为超过规定区域的,太靠近边缘。 跟踪的光流点的status 设置为0 ,一会儿会丢弃这些点
            if ((x1 < limit_edge_corner || x1 >= imgray.cols - limit_edge_corner || x2 < limit_edge_corner || x2 >= imgray.cols - limit_edge_corner
            || y1 < limit_edge_corner || y1 >= imgray.rows - limit_edge_corner || y2 < limit_edge_corner || y2 >= imgray.rows - limit_edge_corner))
            {
                state[i] = 0;
                continue;
            }

            // 对于光流跟踪的结果进行验证，匹配对中心3*3的图像块的像素差（sum）太大，那么也舍弃这个匹配点
            double sum_check = 0;
            for (int j = 0; j < 9; j++)
                sum_check += abs(imGrayPre.at<uchar>(y1 + dy[j], x1 + dx[j]) - imgray.at<uchar>(y2 + dy[j], x2 + dx[j]));
            if (sum_check > limit_of_check) state[i] = 0;

            // 好的光流点存入 F_prepoint F_nextpoint 两个数组当中
            if (state[i])
            {
                F_prepoint.push_back(prepoint[i]);
                F_nextpoint.push_back(nextpoint[i]);
            }
        }
    }
...
//step 4 筛选之后的光流点计算 F 矩阵
cv::Mat F = cv::findFundamentalMat(F_prepoint, F_nextpoint, mask, cv::FM_RANSAC, 0.1, 0.99);
...
//step 5 目的是为了得到匹配程度更高的F2_prepoint,F2_nextpoint
//这一步很奇怪Tracking部分貌似没用到，应该是用在了后面的建图中

```cpp
...
if (dd <= 0.1)  //角点2到直线的距离小于0.1(米),则符合要求
{
    F2_prepoint.push_back(F_prepoint[i]);  // 更加精确的符合要求的角点
    F2_nextpoint.push_back(F_nextpoint[i]);
}
...
//并在最后将它们赋值给F_prepoint,F_nextpoint
F_prepoint = F2_prepoint;
F_nextpoint = F2_nextpoint;
//step6 对第3步LK光流法生成的 nextpoint ，利用极线约束进行验证，并且不满足约束的放入T_M 矩阵，如果不满足约束 那应该就是动态点了
    for (int i = 0; i < prepoint.size(); i++)
    {
        if (state[i] != 0)
        {
            double A = F.at<double>(0, 0)*prepoint[i].x + F.at<double>(0, 1)*prepoint[i].y + F.at<double>(0, 2);
            double B = F.at<double>(1, 0)*prepoint[i].x + F.at<double>(1, 1)*prepoint[i].y + F.at<double>(1, 2);
            double C = F.at<double>(2, 0)*prepoint[i].x + F.at<double>(2, 1)*prepoint[i].y + F.at<double>(2, 2);
            // 点到直线的距离
            double dd = fabs(A*nextpoint[i].x + B*nextpoint[i].y + C) / sqrt(A*A + B*B);

            // Judge outliers 。 认为大于 阈值的点是动态点，存入T_M
            if (dd <= limit_dis_epi)    // 閾值大小是1
                continue;
            T_M.push_back(nextpoint[i]);
        }
    }
...
}

Frame.cc解读到此结束

参考
https://blog.csdn.net/qq_41623632/article/details/112966024
https://blog.csdn.net/weixin_45626706/article/details/115741078