基于ORB-SLAM2与YOLOv8剔除动态特征点(三种方法)

写上篇文章时测试过程比较乱，写的时候有些地方有点失误，所以重新写了这篇
本文内容均在RGB-D环境下进行程序测试

本文涉及到的动态特征点剔除速度均是以https://cvg.cit.tum.de/data/datasets/rgbd-dataset/download#freiburg3_walking_xyz数据进行实验

方法1:segment坐标点集合逐一排查剔除

利用YOLOv8的segment获取动态对象（这里指人person）所在区域的坐标点集合segpoints，之后将提取的特征点的坐标逐一与segpoints中的所有坐标作判断，将出现在segpoints中的特征点的坐标改为(-1,-1)，然后在畸变校正时会将坐标为(-1,-1)的异常坐标剔除。但是segpoints中的数据量太大，完成一次剔除任务花费的时间太长(基本在40~50ms，这个与动态区域的大小即segpoints中的点数是有关的)。另外，特征点坐标为浮点型，而segpoints中的坐标为整型，其实没必要非用 = 判断，可以判断特征点在获取的动态目标区域坐标的周围1(可以调整，我最终在程序中使用半径为2)个像素就可以了，这已经很接近=了。

下面是部分代码：

std::vector<cv::Point> segpoints;
for (auto& obj:objs_seg) {
    int idx = obj.label;
    if (idx == 0)
    {
        cv::Mat locations;
        cv::findNonZero(obj.boxMask == 255, locations);
        for (int i = 0; i < locations.rows; ++i) {
            cv::Point segpoint = locations.at<cv::Point>(i);
            segpoint.x += obj.rect.x;
            segpoint.y += obj.rect.y;

            segpoints.push_back(segpoint);
        }
    }
}
// 动态特征点的判断
for (int k=0; k<mvKeys.size(); ++k){
    const cv::KeyPoint& kp = mvKeys[k];
    bool isDynamic = false;
    for (int kk = 0; kk < segpoints.size(); ++kk) {
        if (kp.pt.x > segpoints[kk].x-3 && kp.pt.x < segpoints[kk].x+3 && kp.pt.y > segpoints[kk].y-3 && kp.pt.y < segpoints[kk].y+3) 
        {
            mvKeys[k] = cv::KeyPoint(-1,-1,-1);
            isDynamic = true;
            break;
        }
    }
    vbInDynamic_mvKeys.push_back(isDynamic);
}

方法2:利用目标检测框

利用YOLOv8进行目标检测，将检测到的目标分为两类：动态对象和静态对象。
这里仅将person设为动态对象。获取动态对象及静态对象的检测框后判断提取的特征点是否在动态对象检测框内以及是否在静态对象检测框内。

1.特征点在动态对象检测框内而不在静态对象检测框内，则满足剔除条件，将其剔除；
2.其余情况皆不满足剔除条件。

采用这种方法速度提升至0.02~0.03ms.

struct DyObject {
    cv::Rect_<float> rect;
    int              id = 0;
};

std::vector<ORB_SLAM2::DyObject> detect_results;
for (auto& obj:objs_det)
{
    int class_id = 0;// id为0代表其为静态对象
    int class_label = obj.label;
    if (class_label == 0){// 如果是人person则将其id改为1即动态对象
        class_id = 1;
    }
    cv::Rect_<float> bbox;
    bbox = obj.rect;
    ORB_SLAM2::DyObject detect_result;
    detect_result.rect = bbox;
    detect_result.id = class_id;
    detect_results.push_back(detect_result);
}
// 判断特征点是否在动态检测框内
bool Frame::isDynamic(const int& i,std::vector<DyObject>& detect_results){
        const cv::KeyPoint& kp = mvKeys[i];
        float kp_u  = kp.pt.x;
        float kp_v = kp.pt.y;
        bool is_dynamic = false;
        for(auto& result:detect_results)
        {
            int idx = result.id;
            if (idx == 1){
                double left = result.rect.x;
                double top = result.rect.y;
                double right = result.rect.x + result.rect.width;
                double bottom = result.rect.y + result.rect.height;
                if(kp_u>left-2 && kp_u<right+2 && kp_v>top-2 && kp_v<bottom-2)
                {
                    // 如果特征点在动态目标检测框内
                    is_dynamic = true;
                }
            }
        }
        return is_dynamic;
}

// 判断特征点是否在静态检测框内
bool Frame::isStatic(const int& i,std::vector<DyObject>& detect_results){
        const cv::KeyPoint& kp = mvKeys[i];
        float kp_u  = kp.pt.x;
        float kp_v = kp.pt.y;
        bool is_static = false;
        for(auto& result:detect_results)
        {
            int idx = result.id;
            if (idx == 0){
                double left = result.rect.x;
                double top = result.rect.y;
                double right = result.rect.x + result.rect.width;
                double bottom = result.rect.y + result.rect.height;
                if(kp_u>left && kp_u<right && kp_v>top && kp_v<bottom)
                {
                    is_static = true;
                }
            }
        }
        return is_static;

}

优化(方法3):目标检测框+实例分割

针对方法1关于速度即处理数据量太大的问题，其实可以将方法1与方法2结合运用，先利用方法2进行判断特征点是否在动态目标的检测框内(不过不需要判断是否在静态目标的检测框内了，方法2中如果在静态目标检测框内就保留该点而不会被剔除，这里舍弃此步骤也是宁缺毋滥的原则)，如果判断结果为真的话，则利用方法1将特征点与实例分割的Mask坐标进行判断即可，这样就可以节省很多时间了。

// 动态目标特征点的判断
//先定义一种目标检测的结果结构
struct DyObject {
    cv::Rect_<float> rect;
    std::vector<cv::Point> pts;
    int              id = 0;
};

for (auto& obj:objs_seg) {
    int idx = obj.label;
    std::vector<cv::Point> segpoints;
    cv::Mat locations;
    cv::findNonZero(obj.boxMask == 255, locations);
    for (int i = 0; i < locations.rows; ++i) {
        cv::Point segpoint = locations.at<cv::Point>(i);
        cv::Rect_<float> rect;
        segpoint.x += obj.rect.x;
        segpoint.y += obj.rect.y;
        segpoints.push_back(segpoint);
    }
    ORB_SLAM2::DyObject detect_result;
    detect_result.rect = obj.rect;
    detect_result.pts = segpoints;
    detect_result.id = idx;
    detect_results.push_back(detect_result);
}

速度控制在了25ms以内。

方案1可以被舍弃了，对于方法2与方法3，测试一下二者在精度上的差异，因为从上面的工作中可以看出方法2的速度很快，如果精度差异很小的话为了SLAM实时性还是采用方法2比较好。
TUM提供了SLAM轨迹精度评估工具：
evaluate_ate.py、evaluate_rpe.py、associate.py
具体内容：https://cvg.cit.tum.de/data/datasets/rgbd-dataset/tools
将上面三个代码下载后就可以对TUM数据集的结果轨迹进行精度评估了。

首先是方法2仅利用目标检测框的一个特征点剔除情况：紫色的点就是之后会被剔除的点。