DS-SLAM代码解析和问题整理 (二)

最新推荐文章于 2023-03-20 20:16:18 发布

每天都在努力学习SLAM的小黑

最新推荐文章于 2023-03-20 20:16:18 发布

阅读量1.8k

点赞数 7

分类专栏： DS-SLAM 文章标签： slam

本文链接：https://blog.csdn.net/weixin_45626706/article/details/115741078

版权

DS-SLAM 专栏收录该内容

5 篇文章 15 订阅

订阅专栏

Frame::CalculEverything

首先Frame.cc 定义了一个结合语义分割结果移除动态的外点的函数CalculEverything

void Frame::CalculEverything( cv::Mat &imRGB,         // RGB图
                              const cv::Mat &imGray,  // 灰度图
                              const cv::Mat &imDepth, // 深度图
                              const cv::Mat &imS)     // 语义分割后的图片

为了方便理解,我在这里贴一下这个函数的源头:

ros_tum_realtime中的TrackRGBD -> System::TrackRGBD -> Tracking.::GrabImageRGBD -> Frame::CalculEverything

这个函数主要执行了:

step1: 逐个遍历语义分割之后的图像的像素,检查出 “人”后跳出循环
step 2 去除位于人身上的动态点
step 3 计算ORB描述子
Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
Step 5 获取图像的深度，并且根据这个深度推算其右图中匹配的特征点的视差
Step 6 计算去畸变后图像边界，将特征点分配到网格中
Step 7 将特征点分配到图像网格中

因为后5步和ORB-SLAM2操作相同,我们就重点来看前两步(作者为了在计算ORB特征点的描述子时可以不涉及到动态点的影响,还专门把计算描述子的函数单独封装到了Frame.cc的Frame::ExtractORBDesp中.意图在动态点删除后再进行计算)

首先,函数定义了是否有异常点(人)的标志位

int flagprocess = 0

然后就是找图像中是否有人存在,一旦找到就立马推出循环(代码看下来,作者认为人是测试场景中唯一的动态物体,需要去除的目标也只有是人.后续的改进点可以是增加移动物体的类型,比如人手中拿着的书本或是针对户外数据集的汽车等)

// 在语义分割图当中，人的灰度值是15 ，当检测到人时，flagprocess 标志位设为1.
for ( int m=0; m<imS.rows; m+=1 )
{
    for ( int n=0; n<imS.cols; n+=1 )
    {
        int labelnum = (int)imS.ptr<uchar>(m)[n];
        if(labelnum == PEOPLE_LABLE) //如果像素的标签是人,就跳出循环,表示找到有异常点
        {
            flagprocess=1;
            break;
        }
    }
    if(flagprocess == 1)
    break;
}

只有当检测到人的时候，才会执行CheckMovingKeyPoints 这个函数去看是否去除特征点。

if(!T_M.empty() && flagprocess ) //存在异常点
{
    std::chrono::steady_clock::time_point tc1 = std::chrono::steady_clock::now();
    // 通过分割图imgS 和潜在的动态点T擦除关键点 mvKeysT 中的位于人身上的动态点。
    flag_mov = mpORBextractorLeft->CheckMovingKeyPoints(imGray,imS,mvKeysTemp,T_M);

    std::chrono::steady_clock::time_point tc2 = std::chrono::steady_clock::now();
    double tc= std::chrono::duration_cast<std::chrono::duration<double> >(tc2 - tc1).count();
    cout << "check time(drop off the moving points)=" << tc*1000 <<  endl;
}

Frame::ExtractORBDesp

作者为了避开计算动态点的描述子,单独将计算描述子部分的部分封装成一个函数,操作同ORB-SLAM2,不说了

void Frame::ExtractORBDesp(int flag,const cv::Mat &imgray)

Frame::ProcessMovingObject

Frame.cc 定义了一个确定需要移除的动态外点的函数ProcessMovingObject

同样为了方便理解,我在这里贴一下这个函数的源头:

Frame::Frame -> Frame::ProcessMovingObject

这个函数主要执行了:

step 1 :计算角点(像素级->亚像素级)
step 2 :计算光流金字塔(确定角点1,2的匹配关系)
step 3 :对于光流法得到的角点进行筛选(像素块内像素差的和小于阈值)
step 4 :计算F矩阵(再对点进行了一次筛选)
step 5 :根据角点到级线的距离小于0.1筛选最匹配的角点
step 6:找到需要被删去的异常点

我们现在来具体的看一下函数:(对于第5步我个人有点小疑问,感觉是作者的一个bug,希望和大家一起讨论一下)

首先,函数将容器上一次的结果清空

F_prepoint.clear();
F_nextpoint.clear();
F2_prepoint.clear();
F2_nextpoint.clear();
T_M.clear();

然后使用cv::goodFeaturesToTrack计算Harris角点

 cv::goodFeaturesToTrack(imGrayPre, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);

void cv::goodFeaturesToTrack(
cv::InputArray image, // 输入图像（CV_8UC1 CV_32FC1）
cv::OutputArray corners, // 输出角点vector
int maxCorners, // 最大角点数目
double qualityLevel, // 质量水平系数（小于1.0的正数，一般在0.01-0.1之间）
double minDistance, // 最小距离，小于此距离的点忽略
cv::InputArray mask = noArray(), // mask=0的点忽略
int blockSize = 3, // 使用的邻域数
bool useHarrisDetector = false, // false =‘Shi Tomasi metric’
double k = 0.04 ) // Harris角点检测时使用

因为cv::goodFeaturesToTrack()提取到的角点只能达到像素级别,所以我们需要使用cv::cornerSubPix()对检测到的角点作进一步的优化计算，可使角点的精度达到亚像素级别。

void cv::cornerSubPix(
cv::InputArray image, // 输入图像
cv::InputOutputArray corners, // 角点（既作为输入也作为输出）
cv::Size winSize, // 区域大小为 NXN; N=(winSize*2+1)
cv::Size zeroZone, // 类似于winSize，但是总具有较小的范围，Size(-1,-1)表示忽略
cv::TermCriteria criteria // 停止优化的标准
);

因为DS-SLAM主要处理的是动态场景,所以作者这里使用了光流金字塔来保证我们在跟踪角点时不会因为物体移动过大而匹配失败.

// 计算光流金字塔，光流金字塔是光流法的一种常见的处理方式，能够避免位移较大时丢失追踪的情况
    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));

参数：
prevImg ：buildOpticalFlowPyramid构造的第一个8位输入图像或金字塔。
nextImg ：与prevImg相同大小和相同类型的第二个输入图像或金字塔
prevPts ：需要找到流的2D点的矢量(vector of 2D points for which the flow needs to be found;);点坐标必须是单精度浮点数。
nextPts ：输出二维点的矢量（具有单精度浮点坐标），包含第二图像中输入特征的计算新位置;当传递OPTFLOW_USE_INITIAL_FLOW标志时，向量必须与输入中的大小相同。
status ：输出状态向量（无符号字符）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。
err ：输出错误的矢量; 向量的每个元素都设置为相应特征的错误，错误度量的类型可以在flags参数中设置; 如果未找到流，则未定义错误（使用status参数查找此类情况）。
winSize ：每个金字塔等级的搜索窗口的winSize大小。
maxLevel ：基于0的最大金字塔等级数;如果设置为0，则不使用金字塔（单级），如果设置为1，则使用两个级别，依此类推;如果将金字塔传递给输入，那么算法将使用与金字塔一样多的级别，但不超过maxLevel。
criteria ：参数，指定迭代搜索算法的终止条件（在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时）。
flags ：操作标志：
OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS使用最小特征值作为误差测量（参见minEigThreshold描述）;如果没有设置标志，则将原稿周围的色块和移动点之间的L1距离除以窗口中的像素数，用作误差测量。
minEigThreshold ：算法计算光流方程的2x2正常矩阵的最小特征值，除以窗口中的像素数;如果此值小于minEigThreshold，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。

对于光流法得到的角点进行筛选。DS-SLAM将筛选的结果放入 F_prepoint F_nextpoint 两个数组当中。光流角点是否跟踪成功保存在status数组当中

for (int i = 0; i < state.size(); i++) // state存储追踪到的图像2的角点数目
    {
        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;    // 角点1
            int x2 = nextpoint[i].x, y2 = nextpoint[i].y;  // 角点2

            // 认为超过规定区域的,太靠近边缘。 跟踪的光流点的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）太大，那么也舍弃这个匹配点
            // 如果3*3图像块内像素差的和大于2120(limit_of_check,经验值可以调整),就认为匹配不正确
            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]);
            }
        }
    }

筛选之后的光流点计算 F 矩阵(再对点进行了一次筛选)

cv::Mat F = cv::findFundamentalMat(F_prepoint, F_nextpoint, mask, cv::FM_RANSAC, 0.1, 0.99);

CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2,
OutputArray mask, int method = FM_RANSAC,
double param1 = 3., double param2 = 0.99 );
@param param1用于RANSAC的参数。它是从一个点到一条外极线的最大距离（以像素为单位），超过该距离的点被视为离群点，不用于计算最终的基本矩阵。
它可以设置为1-3，这取决于点定位的精度、图像分辨率和图像噪声。
@param param2参数仅用于RANSAC或LMedS方法。它规定了估计矩阵正确的理想置信水平（概率）。
@param mask由N个元素组成的输出数组，其中每个元素的异常值设置为0，其他点设置为1。该数组仅在RANSAC和LMedS方法中计算。

然后就是最令我疑惑的step5.

  for (int i = 0; i < mask.rows; i++) // mask.rows表示
    {
        if (mask.at<uchar>(i, 0) == 0);
        else
        {
            // Circle(pre_frame, F_prepoint[i], 6, Scalar(255, 255, 0), 3);
            // 基线的A,B,C
            double A = F.at<double>(0, 0)*F_prepoint[i].x + F.at<double>(0, 1)*F_prepoint[i].y + F.at<double>(0, 2);
            double B = F.at<double>(1, 0)*F_prepoint[i].x + F.at<double>(1, 1)*F_prepoint[i].y + F.at<double>(1, 2);
            double C = F.at<double>(2, 0)*F_prepoint[i].x + F.at<double>(2, 1)*F_prepoint[i].y + F.at<double>(2, 2);
            double dd = fabs(A*F_nextpoint[i].x + B*F_nextpoint[i].y + C) / sqrt(A*A + B*B); //Epipolar constraints (论文公式3,点到直线距离)
            if (dd <= 0.1)  //角点2到直线的距离小于0.1(米?),则符合要求
            {
                F2_prepoint.push_back(F_prepoint[i]);  // 更加精确的符合要求的角点
                F2_nextpoint.push_back(F_nextpoint[i]);
            }
        }
    }
    F_prepoint = F2_prepoint;
    F_nextpoint = F2_nextpoint;

我们先把step 6讲完,再来细细讨论下step5

利用极线约束进行验证，并且不满足约束的放入T_M 矩阵

for (int i = 0; i < prepoint.size(); i++)
    {
        if (state[i] != 0)
        {
            // 直线的一般式:Ax+By+C=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]); //为了找异常点,所以通过精度不是很高的匹配点来搜索
        }
    }

我们可以看到,整个函数的目的是为了求出动态点,将其放入异常点集合T_M中.从论文的算法一中我们可以清楚地看到作者的逻辑在这里插入图片描述作者为了求出动态点集合,对其进行了函数的step 1,2,3,4,6步.但是第五步的目的是为了得到匹配程度更高的F2_prepoint,F2_nextpoint

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;

但奇怪的是最后这两个变量就没有再使用了

在这里插入图片描述我试着将整个for循环注释掉,整个代码运行下来就会出现明显的区别.也希望有知道的同学可以在评论区和我讨论一下,感谢!

最后我们再回过头来看一下一开始Frame.cc新增的变量:

cv::Mat imGrayPre; // 上一帧的灰度图
std::vector<cv::Point2f> prepoint, nextpoint;      // prepoint:cornerSubPix亚像素计算后得到的角点 
												   // nextpoint:对上一帧图像进行光流金字塔得到的本帧图像的角点
std::vector<cv::Point2f> F_prepoint, F_nextpoint;  // F_nextpoint:先是在step3经过图像块像素差筛选过后的精度较高的角点,最后是在step5经过图像块像素差筛选以及经过重投影与极线的距离筛选后的精确相对最高的角点
std::vector<cv::Point2f> F2_prepoint, F2_nextpoint;// 经过图像块像素差筛选以及经过重投影与极线的距离筛选后的精确相对最高的角点

std::vector<uchar> state; // 记录光流点是否跟踪成功的集合，成功status =1,否则为0
std::vector<float> err;   // 输出错误的矢量.只为了用在光流金字塔中.
std::vector<std::vector<cv::KeyPoint>> mvKeysPre;  //没有使用过

以上就是DS-SLAM 的Frame.cc解析的全部内容,Thanks for reading!

每天都在努力学习SLAM的小黑

关注

7
点赞
踩
16

收藏

觉得还不错? 一键收藏
7
评论
DS-SLAM代码解析和问题整理 (二)

目录Frame::CalculEverythingFrame::ExtractORBDespFrame::ProcessMovingObject接着我们来看Frame.cc修改了哪些内容Frame.cc定义了几个新的变量,我们将结合后面的代码来仔细分析cv::Mat imGrayPre; // The previous imagestd::vector<cv::Point2f> prepoint, nextpoint; std::vector<cv::Point2f>
复制链接

扫一扫