DS-SLAM 运动一致性检测的源码解读

运动一致性检测是Frame.cc的Frame::ProcessMovingObject(const cv::Mat &imgray)函数。

对应DS-SLAM流程图Moving consistency check的部分 

把这个函数单独摘出来，写了一下对两帧检测，查看效果的程序：

#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <vector>
#include <iostream>
#include <cmath>
void ProcessMovingObject(const  cv::Mat &img1, cv::Mat &img2, std::vector<cv::Point2f> F_prepoint,std::vector<cv::Point2f> F_nextpoint)
{cv::Mat prevGray, imgray;cv::cvtColor(img1, prevGray, cv::COLOR_BGR2GRAY);cv::cvtColor(img2, imgray, cv::COLOR_BGR2GRAY);std::vector<cv::Point2f> prepoint, nextpoint ;std::vector<cv::Point2f> F2_prepoint, F2_nextpoint ,T_M;double limit_dis_epi = 3; double limit_of_check = 2000;int limit_edge_corner = 5;std::vector<uchar> state;std::vector<float> err;// Detect dynamic target and ultimately optput the T matrixcv::goodFeaturesToTrack(prevGray, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);cv::cornerSubPix(prevGray, prepoint, cv::Size(10, 10), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));cv::calcOpticalFlowPyrLK(prevGray, imgray, prepoint, nextpoint, state, err, cv::Size(22, 22), 5, cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.01));// 绘制特征点for (size_t i = 0; i < prepoint.size(); ++i) {cv::circle(img1, prepoint[i], 2, cv::Scalar(0, 255, 0), -1); // 绿色实心圆, 半径为2}// 显示绘制后的图像cv::imshow("Image with Feature Points", img1);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;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;}double sum_check = 0;for (int j = 0; j < 9; j++)sum_check += abs(prevGray.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;if (state[i]){F_prepoint.push_back(prepoint[i]);F_nextpoint.push_back(nextpoint[i]);}}}// F-Matrixcv::Mat mask = cv::Mat(cv::Size(1, 300), CV_8UC1);cv::Mat F = cv::findFundamentalMat(F_prepoint, F_nextpoint, mask, cv::FM_RANSAC, 0.1, 0.99);for (int i = 0; i < mask.rows; i++){if (mask.at<uchar>(i, 0) == 0);//waidianelse{// Circle(pre_frame, F_prepoint[i], 6, Scalar(255, 255, 0), 3);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 constraintsif (dd <= 0.1){F2_prepoint.push_back(F_prepoint[i]);F2_nextpoint.push_back(F_nextpoint[i]);}}}// zhixianshi neidian F_prepoint = F2_prepoint;F_nextpoint = F2_nextpoint;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 outliersif (dd <= limit_dis_epi) continue;  // inelse{T_M.push_back(nextpoint[i]);}for (size_t i = 0; i < T_M.size(); ++i) {cv::circle(img2, T_M[i], 2, cv::Scalar(255, 0, 0), -1); // blue实心圆, 半径为2}}}// 绘制光流矢量cv::Mat flowImg;cv::cvtColor(img2, flowImg, cv::COLOR_BGRA2BGR); // 复制以绘制for (size_t i = 0; i < F_prepoint.size(); ++i) {if (state[i]) {cv::line(flowImg, F_prepoint[i], F_nextpoint[i], cv::Scalar(0, 0, 255), 2);cv::circle(flowImg, F_nextpoint[i], 2, cv::Scalar(0, 255, 0), -1);}}// 显示结果cv::imshow("Optical Flow", flowImg);}
int main(int argc, char** argv) {if (argc < 3) {std::cerr << "Usage: " << argv[0] << " <image1_path> <image2_path>" << std::endl;return -1;}// 读取图片cv::Mat img1 = cv::imread(argv[1]);cv::Mat img2 = cv::imread(argv[2]);if (img1.empty() || img2.empty()) {std::cerr << "Could not open or find the images!" << std::endl;return -1;}// 存储用于LK光流的点std::vector<cv::Point2f> points1, points2;ProcessMovingObject(img1, img2, points1, points2 );// 等待按键cv::waitKey(0);return 0;
}

编译运行：

g++ -o main1 main1.cpp `pkg-config --cflags --libs opencv`
./main1 image1.jpg image3.jpg

（1）光流跟踪效果。 

（2） RANSANC求基础矩阵并去除外点，然后去除不符合对极约束的点。

自己调了一下阈值：

（3） 把不符合对极约束的点用蓝色表示。