动态场景下基于实例分割的SLAM（毕业设计动景SLAM代码阅读部分）

四、SLAM代码研究阶段

• 本章节包括：ROS环境配置，DS-SLAM、DynaSLAM代码阅读笔记

2020.01.16

中午开始聚会，上午简单做点准备工作。首先从DS-SLAM开始。

麻烦了，居然是需要ROS环境，我虚拟机还没配置过，唉，估计配置完一上午又没了（起得晚）。

基本配置思路是2017年的一篇博客：https://www.linuxidc.com/Linux/2017-08/146030.htm

但是因为文章比较老了，会出现几个问题：

1. E: 无法定位软件包 ros-kinetic-desktop-full

   解决：https://blog.csdn.net/victoria_pierce/article/details/81638319

2. E: 无法定位软件包 Python-rosinstall

   解决：https://www.cnblogs.com/j-c-y/p/11177710.html

   这里说一下那个 “等待程序不动后，再打开一个窗口 ”这个是指不关闭原本的终端，再新打开一个终端输入…

剩下的居然一切顺利，然后下载SegNet编译安装。

突然发现一个DS-SLAM的编译教程：https://blog.csdn.net/yinghuaxuan/article/details/89969197

…等等！需要cuDNN？！眼前一黑。

我们还是等明天直接看代码吧。

2020.01.17

DS-SLAM代码阅读笔记

首先惯例从System入手，System::System里面多了一个语义分割的线程，调用Segment::Run，new了一个mpSegment，传入参数为pascal_prototxt, pascal_caffemodel, pascal_png，这三个是在System初始化里面传入的，而这个初始化在ros_tum_realtime.cc里面，实际传入为argv[5],argv[6],argv[7]；而翻阅readme得知启动时代码为roslaunch DS_SLAM_TUM.launch ，翻找该文件，发现分别是：

ORB_SLAM2_PointMap_SegNetM)/prototxts/segnet_pascal.prototxt $(find ORB_SLAM2_PointMap_SegNetM)/models/segnet_pascal.caffemodel $(find ORB_SLAM2_PointMap_SegNetM)/tools/pascal.png" output="screen" />

model和png就不说了，prototxt文件是VGG_ILSVRC_16_layer的文字版，我猜这个权重文件和网络框架文件是分开的。

mpSegment传入传出仅和mpTracker连接。然后跳到Segment.cc文件。初始化没啥好说的，看Segment::Run()启动函数，开头直接new了一个分类器，这个分类器不是在src 里面，而是在example里面的，调用了caffe的接口做的。

Classifier类中被初始化后，调用Predict进行预测，具体步骤如下：

1. 网络预处理，包括 WrapInputLayer，作者解释这个函数是这么说的：

   Wrap the input layer of the network in separate cv::Mat objects (one per channel). This way we save one memcpy operation and we don’t need to rely on cudaMemcpy2D. The last preprocessing operation will write the separate channels directly to the input yer,

   注：memcpy : 由src所指内存区域复制count个字节到dest所指内存区域

   他说的没有看懂，但是代码实际上就是将多维度矩阵按channel维度展开，降低了一个维度，关键代码如下：

   for (int i = 0; i < input_layer->channels(); ++i)
       {
           cv::Mat channel(height, width, CV_32FC1, input_data);
           input_channels->push_back(channel);
           input_data += width * height;
       }
   

2. 图片预处理，包括Preprocess函数：该函数基本功能是按照网络设计的num_channels_数将图片先强制转换为灰度图片或者RGB图片，resize使图像与输入网络格式保持一致并浮点化，再利用cv::split函数对各个通道进行分离，这个函数比较有意思：

   cv::split(sample_float, *input_channels);
   

   输出的input_channels 是std::vector< cv::Mat >类型的，对应RGB的3个通道，是前一个步骤的结果

3. 前向传播net_->Forward()，顺便计了个时；

4. 取出结果，虽然没有运行过，不过从这些代码的处理来看最后的输出是对所有像素关于每一个类别的概率，因此需要利用cv::minMaxLoc（找到最大最小值及其定位）来生成最后的prediction_map，而这个东西在Segment里面就是label_colours；

返回Segment文件，然后他的操作让我有点迷惑：

mImgSegment_color = mImgSegment.clone();
cv::cvtColor(mImgSegment,mImgSegment_color, CV_GRAY2BGR);
LUT(mImgSegment_color, label_colours, mImgSegment_color_final);

其中LUT不太常用，我查了一下，详见博客：https://blog.csdn.net/y363703390/article/details/79431450

比较好奇为什么最后的概率图需要转化为RGB并LUT，我的猜测是为了将单通道转换为3通道，至于转化为3通道做什么，因为手头只有虚拟机，难以验证，伤心。

最后用mSkipIndex这个变量实现两帧之间跳过一帧。结束了基本Segment线程，还是比较简单的。

下一步看一下TracKing中如何融合的。

2020.01.19（02.25更新）
Tracking线程的入口是ros_tum_realtime.cc中的SLAM.TrackRGBD（即，仅针对RGBD），挂上一堆线程锁后来到Tracking::GrabImageRGBD，现将图像转化为GRAY后和orbslam出现了不同。

在原ORBSLAM2中，这一步简单的创建了当前帧Frame，Frame经过Track后返回相机位姿；而DS-SLAM则创建了Frame后等待分割图像传入，注意这里的Frame构造和原来的其实也是不一样的，这里添加了动态异常点的检查ProcessMovingObject并将结果传给T_M，并利用swap将当前帧作为imGrayPre留到下次使用。

对于ProcessMovingObject函数，这个拿出来单独说一下：

1. 清除之前的记录后第一步是寻找角点（关键点），采用的是opencv内的两个之前没见过的函数。

 cv::goodFeaturesToTrack(imGrayPre, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);
cv::cornerSubPix(imGrayPre, prepoint, cv::Size(10, 10), cv::Size(-1,-1),cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));

参考博客https://blog.csdn.net/guduruyu/article/details/69537083

1. 利用cv::calcOpticalFlowPyrLK建立光流金字塔，https://blog.csdn.net/weixin_30535167/article/details/94978062。其中state表示输出二维点向量（用单精度浮点坐标）包括第二幅图像中计算的输入特征的新点位置。一个小细节，之前我猜测用光流金字塔可能和特征点的金字塔配合，事实证明我想多了。
2. 注意，这里的state为一个数组。在calcOpticalFlowPyrLK中，如果对应特征的光流被发现，数组中的每一个元素都被设置为 1， 否则设置为 0。之后一部分for语句的那段，我个人理解是在预测范围内一个像素范围内进行寻找之前帧的预测运动点，利用sum来对特征点进行核减和新增。
3. 利用cv::findFundamentalMat函数计算F矩阵，这里又有两个for循环，第一个是对F_nextpoint和F_prepoint的。这里的mask由cv::findFundamentalMat中获得，api解释这个参数Output array of N elements, every element of which is set to 0 for outliers and to 1 for the other points. The array is computed only in the RANSAC and LMedS methods. For other methods, it is set to all 1’s. 就是区分内外点的，值为0时，代表该匹配点事错误的匹配（离群值）。第一个循环带入对极约束。对极约束理论值为0，这里将过小时dd<=0.1视为有效点。第二个for 循环针对的是nextpoint和prepoint的，计算对极约束大于阈值认为异常点，然后将正确的点存到T_M里面。
4. 注意，这两个for循环很相似，但是本质不同：F_nextpoint和F_prepoint由步骤3生成，范围更大但是更加不精准。我们利用这些大样本点获得了基础矩阵F，并利用RANSAC和对极约束优化了F_nextpoint和F_prepoint，这就是第一个for循环的本质；第二个for循环利用大样本点生成的F，通过对极约束公式修正精准度较高的nextpoint和prepoint，存入T_M里面。

之后，两种图像利用了CalculEverything函数后再追踪。这个CalculEverything函数是在Frame类里面的，作用如下：

1. 检查每个像素点的位置，当检查出“人”时进行下一步处理
2. 利用CheckMovingKeyPoints函数检查移动点，即检查剔除区域（T_M集中每个像素点及周围15像素）是不是具有动态先验“人类”的标签。
3. 如果flag_orb_mov==1，即标签带有先验动态，则在特征点金字塔内删除该特征点。
4. 剩下的和ORB-SLAM2差不多（提取orb的描述子、去畸变等）。

顺便提一句，DS-SLAM对部分函数进行了整体（还谈不上优化，但是看起来轻松很多）。后面还有八叉树的部分，不过我不太关心这个，就跳过了。

2020.01.20

DynaSLAM代码阅读笔记

和之前的DS-SLAM不同，DynaSLAM采用的分割是经典的实例分割，而且并不是像前者那样调用c++接口，而是直接嵌入Python，保存结果图片。在之前的项目中我曾经试图想要将Keras嵌入至ORB-SLAM2，但是由于buff接口之类的问题很难解决，网上相关资料也比较少，折腾几天最后还是放弃了（当时临近考研），最后将目光转向的用C++直接读取模型的另一条路，不知道作者是如何解决。

严格来说，直接读取嵌入模型并不比再读取模型文件简单，其中一个很麻烦的问题就是类型转换，在DynaSLAM里面，作者专门创建了一个包含多个转换类的文件conversion.cpp去处理这个问题。仔细看完相关代码，不得不说这个是一个很好的关于c++嵌入python的例子，可以学习一下，但是如果我去做的话，我可能会避开这种选项，试着用tf读取权重而不是嵌入python。

DynaSLAM还有一个问题，它并没有采用另外一个线程而是先实例分割后保存图片。但是由于使用了串联级别的实例分割，不太能确定是否能够达到实时的效果，这个可能还需要测试一下，因为并行线程可以用多加处理器解决。

DynaSLAM的RGB追踪分为两部分，论文称为Low-Cost Tracking 和 Tracking 对应Tracking线程中LightTrack和Track()，中间夹杂着多视角几何模块GeometricModelCorrection()，后面跟着背景填充InpaintFrames和数据库更新GeometricModelUpdateDB。其中Track()函数与ORB-SLAM的完全一致。

我这边写一下Low-Cost Tracking 和 Tracking的区别吧：

	DynaSLAM的LightTrack	DynaSLAM的Track
如果没有初始化	直接return	直接初始化
mbOnlyTracking	mbOnlyTracking=true	分类
重定位	Relocalization(1)【1】	Relocalization();
追踪运动模型	LightTrackWithMotionModel(fasle)【2】	TrackWithMotionModel()
TrackLocalMap、drawer等	不开启	开启
更新运动模型、关键帧	否	当bok==true
追踪失败	return	重启
记录上一帧	否	是

1. 当Relocalization()传入为0时，与原ORB-SLAM相同，mnLastRelocFrameId = mCurrentFrame.mnId；当传入为1时跳过该句。
2. 在原TrackWithMotionModel()函数中，mlpTemporalPoints和mLastFrame会被改变和优化，因此新的LightTrackWithMotionModel添加了 ‘’还原了原来的mlpTemporalPoints和mLastFrame‘’ 的小改动。

2020.01.21

几何推断部分在Geometry类中，该类的主要函数分别是GeometricModelCorrection、InpaintFrames、GeometricModelUpdateDB。其中InpaintFrames涉及到背景填充，暂时跳过。

首先看GeometricModelCorrection，主要由GetRefFrames(currentFrame)、ExtractDynPoints(vRefFrames,currentFrame)、DepthRegionGrowing(vDynPoints,imDepth)、CombineMasks(currentFrame,mask)四个主要函数组成：

1. GetRefFrames：该函数从预先在Geometry.h文件设置的类DataBase（vector <ORB_SLAM2::Frame>）中获取关键帧序列。其步骤如下：
   • 求得当前帧的欧拉角和平移距离
   • 计算关键帧数据库的欧拉角和平移距离，并用二范数计算与当前帧的距离distRot和dist。
   • 利用公式vDist = 0.7×Precision+0.3×Recall计算距离阈值（？）
   • 在关键参考帧小于等于5的约束下，计算距离相对较小的几个关键帧
2. ExtractDynPoints：前面一部分在取出关键点的深度、坐标信息，然后涉及到了位姿计算，可能是我功底太差，有点迷迷糊糊的没看懂。等我学一下深蓝的SLAM课程补补课回头再看看这个问题。
3. DepthRegionGrowing：
   • 检查了动态点周围四连通区域内的点深度信息是否能得到验证
   • 生成动态物体的掩码图像
   • 利用dilate及可计算的核进行形态学滤波
4. CombineMasks：联合之前生成的语义分割掩码生成最终掩码。