基于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僅利用目標檢測框的一個特征點剔除情況：紫色的點就是之后會被剔除的點。

然后是方法3的特征點剔除情況：

上面兩張圖片的對比可以看出方法2會將一些有用的特征點也標記為動態特征點，而方法3會更精確。關于圖片中紅色圓圈，是我做的紋理分析，目前還沒完全做好所以就先不講了。

我對ORB-SLAM2與我基于ORB-SLAM2andYOLOv8(方法2與方法3)在數據集rgbd_dataset_freiburg3_walking_xyz的結果軌跡進行了精度評估，結果如下：

	精度評估
TUM-freiburg3_walking_xyz	ORB-SLAM2	DWT-SLAM det	DWT-SLAM seg
RPE	0.555583	0.022521	0.018761
ATE	0.474276	0.017388	0.014755

方法3利用目標檢測框+實例分割的方法的精度是最優的。

下面再測測https://cvg.cit.tum.de/data/datasets/rgbd-dataset/download#freiburg3_walking_rpy

	精度評估
TUM-freiburg3_walking_rpy	ORB-SLAM2	DWT-SLAM det	DWT-SLAM seg
RPE	0.968605	0.035853	0.035431
ATE	0.788089	0.029942	0.028222

https://cvg.cit.tum.de/data/datasets/rgbd-dataset/download#freiburg3_sitting_halfsphere

	精度評估
TUM-freiburg3_walking_halfphere	ORB-SLAM2	DWT-SLAM det	DWT-SLAM seg
RPE	0.357984	0.045250	0.029718
ATE	0.294075	0.036301	0.023612

從以上從三個數據集獲得的三組精度評估結果來看，方法3的精度最高，25ms的動態特征點處理速度也是可接受的(我的電腦算是比較舊了)。