这一篇文章我们来一起学习一下DS-SLAM的 System.cc 系统初始化的构造函数 和Frame.cc 帧初始化构造函数 :
相比较与ORBSLAM2 作者主要改变了 Frame.cc, ORBmatcher.cc, Pointcloudmapping.cc and Segment.cc. 这五个函数，在ORBSLAM2的基础上新增了语义分割 线程 和 稠密建图线程 接下来我们一一往下学习。
在程序的主函数 ros_tum_realtime.cc 我们先对整个SLAM系统进行了初始化：
// 初始化SLAM系统，这里回调用构造函数
ORB_SLAM2::System SLAM(argv[1],argv[2], argv[5],argv[6],argv[7],ORB_SLAM2::System::RGBD, viewer);
调用了System.cc里面的系统构造函数
// 系统的构造函数
System::System(const string &strVocFile, //词典路径
const string &strSettingsFile, //配置文件路径
const string &pascal_prototxt, //模型的网络结构
const string &pascal_caffemodel, //caffemodel 训练好的模型
const string &pascal_png,
const eSensor sensor,
Viewer* pViewer,
Map* map,
ORBVocabulary* voc):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false)
这个系统的构造函数干了两件事情：
1 加载了ORB的词典文件
bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
2 开启了SLAM系统的4个线程。
mpTracker = new Tracking(this, mpVocabulary, mpMap, mpPointCloudMapping, mpKeyFrameDatabase, strSettingsFile, mSensor); // 跟踪线程
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper); //局部建图线程
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser); //回环检测线程
mptSegment =new thread(&ORB_SLAM2::Segment::Run,mpSegment); //语义分割线程
Semantic segmantation 线程的 对象的创建：
mpSegment =new Segment(
pascal_prototxt, //模型文件，网络模型
pascal_caffemodel, //训练文件，训练好的权重参数
pascal_png); // 标签文件,上色的图片
在System.cc里面 我们初始化跟踪线程，并用mpTracker 这个指针来指向这个新创建的Tracing 对象：
mpTracker = new Tracking(this, mpVocabulary, mpMap, mpPointCloudMapping, mpKeyFrameDatabase, strSettingsFile, mSensor);
这里程序会进入到Tracking 类的的构造函数里面，在Tracking的构造函数里面主要加载了一些参数和构造了ORB的特征提取器（分配了每层的pyramid特征点的个数和灰度质心圆的坐标计算），和ORBSLAM2一样，不再详细叙述：
// 构造ORB提取器 这一块和 ORBSLAM 是一模一样的
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
这样Tracking 的构造函数执行，System.cc的构造函数也执行完成，SLAM系统已经创建好了。
初始化完成的系统开始接受图片，正式进入SLAM的运行阶段
图片传递给SLAM系统在ros_tum_realtime.cc 的while循环里面：
Camera_Pose = SLAM.TrackRGBD(imRGB,imD,tframe);
在System::TrackRGBD 函数中，我们将RGB图片分别发送给Tracking线程和Semantic Segmantation线程：
// Inform Semantic segmentation thread
mpTracker->GetImg(im); //图片发送给语义分割线程
return mpTracker->GrabImageRGBD(im,depthmap,timestamp); //返回值是当前帧的Tcw的位姿矩阵
（1）首先看一下图片如何发送给语义分割线程，调用了Tracking类中的GetImg函数：
将新来的RGB(彩色)图片复制给了 语义分割线程的mImg 类成员变量，该变量存放需要语义分割的图片
void Tracking::GetImg(const cv::Mat& img)
{
unique_lock<mutex> lock(mpSegment->mMutexGetNewImg);
mpSegment->mbNewImgFlag=true; // 新来图片的标志位
img.copyTo(mpSegment->mImg); // 实现了一幅图片分别发送给 Track 线程和 语义分割 Segment线程
}
（2）在 GrabImageRGBD 函数中计算位姿，一共分为7个步骤：
step 1：将RGB或RGBA图像转为灰度图像
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
step 2 ：将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
if(mDepthMapFactor!=1 || mImDepth.type()!=CV_32F);
mImDepth.convertTo(mImDepth,CV_32F,mDepthMapFactor);
//step 3: 创建了Frame ,调用Frame构造函数去提取特征点,通过LK光流提取角点，并对生成的角点施加对极约束，极线约束删除动态点
mCurrentFrame = Frame(mImGray,mImDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mThDepth);
/step 4:等待语义分割结果 ,语义分割完成，这个函数isNewSegmentImgArrived 返回 false
while(!isNewSegmentImgArrived())
{
usleep(1);
}
std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t4 - t3).count();
cout << "wait for new segment img time =" << ttrack*1000 << endl;
step 5 : 结合语义分割结果移除 动态的 外点
mCurrentFrame.CalculEverything(
mImRGB, // 彩色图
mImGray, // 灰度图
mImDepth, //深度图
mpSegment->mImgSegmentLatest // 上面图像的语义分割的结果
);
step 6 : 移除 动态的外点之后进行正常SLAM的Track 部分【语义分割线程已经执行完】
Track();
step 7 : 得到SLAM计算的位姿
return mCurrentFrame.mTcw.clone();
以上便是整个DS-SLAM擦除运动点的大致流程。
我们这篇文章先从Step 3 ,也就是帧的构造函数开始继续学习。
Step 3 主要是调用了Frame的构造函数，我们来看一下Frame的构造函数的大致流程：
3.1 Scale Level Info 计算图像金字塔的参数
InitializeScaleLevels();
3. 2 提取ORB特征，和ORBSLAM2 大致一样
调用了特征提取仿函数 提取ORB特征点，并进行了四叉树均匀化分配
(*mpORBextractorLeft)( imgray,cv::Mat(),mvKeysTemp);
但是这里的提取的特征点里面可能是含有动态点的，所以DS-SLAM并没有在特征提取仿函数里面去计算这些特征点的描述子，而是在语义分割擦除动态点之后才开始对剩下的特征点进行描述子的计算和去畸变等操作。而这些操作在普通的ORBSLAM2里面是在特征提取仿函数里面一步完成的，这样子处理可以提高运行的速度。我们先将提取到的特征点保存在 mvKeysTemp 这里变量里面。
3.3 几何方法进行运动的一致性检测，也就是论文里的 Moving Cosistency Check这一小节
算法如下：
简单解释一下：因为运动分割是很耗费时间的，所以我们仅仅对于人身上的特征点进行擦除，这里的输入是上一帧图片F1，以及其特征点P1，当前帧的图片F2，输出是当前帧的特征点P2，以及当前帧相对于前一帧不满足几何约束的特征点，将其放置与T_M矩阵。我们首先通过LK光流函数得到当前帧的特征点，对于光流法得到的 角点进行筛选，如果角点认为超过规定区域的,太靠近边缘，或者该角点中心3*3的图像块的像素差（sum）太大，那么也舍弃这个角点。利用筛选之后的光流点计算 F 矩阵。得到F矩阵之后，对当前帧的特征点进行极线约束，不满足约束的放入T_M 矩阵，如果不满足约束 那应该就是动态点了。
接下来学习代码：
主要是 ProcessMovingObject 函数。
- 调用opencv 函数 计算Harris 角点，将结果保存在 prepoint 矩阵当中
cv::goodFeaturesToTrack(imGrayPre, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);
//cv::goodFeaturesToTrack()提取到的角点只能达到像素级别
//我们则需要使用cv::cornerSubPix()对检测到的角点作进一步的优化计算，可使角点的精度达到亚像素级别。
// 调用opencv的函数,进行亚像素的角点检测，输出的角点还是放在 prepoint 里面
cv::cornerSubPix(imGrayPre, prepoint, cv::Size(10, 10), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
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)
);
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]);
}
}
}
4 筛选之后的光流点计算 F 矩阵
cv::Mat F = cv::findFundamentalMat(F_prepoint, F_nextpoint, mask, cv::FM_RANSAC, 0.1, 0.99);
下面还有一个for循环 好像是根据F矩阵得到的mask 矩阵，通过对极几何约束去更新F_prepoint 数组和 F_nextpoint数组，不过这个我没有看太明白这样的目的是什么，后面的的代码好像也没有用到这个更新后的数组？？
5 LK光流法生成的 nextpoint ，利用极线约束进行验证，并且不满足约束的放入T_M 矩阵，如果不满足约束 那应该就是动态点了。
// 6 对第3步光流法生成的 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]);
}
}
理解： DS-SLAM主要是通过擦除人身上的 特征点 来使得整个SLAM系统更加的鲁棒，但是人虽然大概率是运动的，但也是有可能不动，这个时候如果盲目的通过语义分割的结果去擦除人身上的特征点显然不是一个明智的选择，所以这个T_M矩阵就是判断会动的点是不是人身上的，如果是人身上的点，人是运动的，才会擦除人身上的点。否则，人如果静止不动，那么人身上的特征点应该满足对极约束，不会出现在T_M矩阵当中，那么就不应该通过语义分割的结果去擦除人身上的特征点。
262
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
