Youtube demo: https://www.youtube.com/embed/aiqLlyiriXI
# 程序流程分析
## 主程序流程
入口程序：rgbd_tum.cc
- 读取数据集 LoadImages
- 创建实例分割的实例 SegmentDynObject
SegmentDynObject 这个类是负责实例分割的。MaskRCNN是基于Python开发的，在此用CPP来调用Python接口。由于ROS是只支持Python2.7的，所以此处的MaskRCNN是使用的Python2.
在类中，最关键的函数是：
```cpp
cv::Mat GetSegmentation(cv::Mat &image, std::string dir = "no_save", std::string rgb_name = "no_file");
```
- 初使化SLAM (RGB-D mode)
```cpp
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
```
- 设置中间结果保存路径， 结果将保存到 rgb, depth, mask, label 文件夹中
- 开始主循环，遂帧读取图像 (nImages 为数据集中图像的个数):
```cpp
for (int ni = 0; ni < nImages; ni++)
{
//LOOP
}
```
- 形态学滤波, 设置核函数
```cpp
// Dilation settings
int dilation_size = 15;
cv::Mat kernel = getStructuringElement(
cv::MORPH_ELLIPSE, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
cv::Point(dilation_size, dilation_size));
```
- 实例分割
```cpp
cv::Mat maskRCNN;
maskRCNN = MaskNet->GetSegmentation(imRGB, string(argv[5]), vstrImageFilenamesRGB[ni].replace(0, 4, ""));
cv::Mat maskRCNNdil = maskRCNN.clone();
cv::dilate(maskRCNN, maskRCNNdil, kernel);
mask = mask - maskRCNNdil; //inverse
```
得到分割后的图像maskRCNN，
进行形态学滤波， 之后求逆 （考滤：为什么要这么操作呢？）
- Tracking线程
分两种情况，
一种是使用语义分割：
```cpp
SLAM.TrackRGBD(imRGB, imD, mask, tframe, imRGBOut, imDOut, maskOut);
```
另一种是不使用语义分割，全依靠几何信息：
```cpp
SLAM.TrackRGBD(imRGB, imD, mask, tframe);
```
考滤：第二种情况与传统的ORB SLAM相比有何异同？
- 收尾工作
- 计算时间消耗
- 保存轨迹
```cpp
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
```
## Tracking分析
- TrackRGBD的跟踪主函数入口
Tracking是一个单独的线程。
```cpp
cv::Mat System::TrackRGBD(const cv::Mat &im,
const cv::Mat &depthmap,
cv::Mat &mask,
const double ×tamp,
cv::Mat &imRGBOut,
cv::Mat &imDOut,
cv::Mat &maskOut)
```
输入参数有四个：RGB图像，Depth图像，Mask 语义Label， 时间戳。
输出有三个：处理后的RGB图像，处理后的深度图像，处理后的Mask 。
考虑：进行了哪些处理？
- GrabImageRGBD (Tracking.cc)
在TrackRGBD中最主要的函数就是GrabImageRGBD。参数与上面相同，程序返回当前帧的位姿$Tcw$.
```cpp
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,
const cv::Mat &imD,
cv::Mat &mask,
const double ×tamp,
cv::Mat &imRGBOut,
cv::Mat &imDOut,
cv::Mat &maskOut)
```
- 将RGB图像转为灰度图像
```cpp
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
```
- 对深度图像进行预处理
```cpp
if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
```
convertTo的原型为：https://docs.opencv.org/trunk/d3/d63/classcv_1_1Mat.html
```cpp
void cv::Mat::convertTo (OutputArray m,
int rtype,
double alpha = 1,
double beta = 0
) const
```
m(x,y)=saturate_castDynaSLAM 代码分析
[toc]
# 目标：
- 理解DynaSLAM的主程序流程
- 理解DynaSLAM的核心算法
Youtube demo: https://www.youtube.com/embed/aiqLlyiriXI
# 程序流程分析
## 主程序流程
入口程序：rgbd_tum.cc
- 读取数据集 LoadImages
- 创建实例分割的实例 SegmentDynObject
SegmentDynObject 这个类是负责实例分割的。MaskRCNN是基于Python开发的，在此用CPP来调用Python接口。由于ROS是只支持Python2.7的，所以此处的MaskRCNN是使用的Python2.
在类中，最关键的函数是：
```cpp
cv::Mat GetSegmentation(cv::Mat &image, std::string dir = "no_save", std::string rgb_name = "no_file");
```
- 初使化SLAM (RGB-D mode)
```cpp
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
```
- 设置中间结果保存路径， 结果将保存到 rgb, depth, mask, label 文件夹中
- 开始主循环，遂帧读取图像 (nImages 为数据集中图像的个数):
```cpp
for (int ni = 0; ni < nImages; ni++)
{
//LOOP
}
```
- 形态学滤波, 设置核函数
```cpp
// Dilation settings
int dilation_size = 15;
cv::Mat kernel = getStructuringElement(
cv::MORPH_ELLIPSE, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
cv::Point(dilation_size, dilation_size));
```
- 实例分割
```cpp
cv::Mat maskRCNN;
maskRCNN = MaskNet->GetSegmentation(imRGB, string(argv[5]), vstrImageFilenamesRGB[ni].replace(0, 4, ""));
cv::Mat maskRCNNdil = maskRCNN.clone();
cv::dilate(maskRCNN, maskRCNNdil, kernel);
mask = mask - maskRCNNdil; //inverse
```
得到分割后的图像maskRCNN，
进行形态学滤波， 之后求逆 （考滤：为什么要这么操作呢？）
- Tracking线程
分两种情况，
一种是使用语义分割：
```cpp
SLAM.TrackRGBD(imRGB, imD, mask, tframe, imRGBOut, imDOut, maskOut);
```
另一种是不使用语义分割，全依靠几何信息：
```cpp
SLAM.TrackRGBD(imRGB, imD, mask, tframe);
```
考滤：第二种情况与传统的ORB SLAM相比有何异同？
- 收尾工作
- 计算时间消耗
- 保存轨迹
```cpp
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
```
## Tracking分析
- TrackRGBD的跟踪主函数入口
Tracking是一个单独的线程。
```cpp
cv::Mat System::TrackRGBD(const cv::Mat &im,
const cv::Mat &depthmap,
cv::Mat &mask,
const double ×tamp,
cv::Mat &imRGBOut,
cv::Mat &imDOut,
cv::Mat &maskOut)
```
输入参数有四个：RGB图像，Depth图像，Mask 语义Label， 时间戳。
输出有三个：处理后的RGB图像，处理后的深度图像，处理后的Mask 。
考虑：进行了哪些处理？
- GrabImageRGBD (Tracking.cc)
在TrackRGBD中最主要的函数就是GrabImageRGBD。参数与上面相同，程序返回当前帧的位姿$Tcw$.
```cpp
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,
const cv::Mat &imD,
cv::Mat &mask,
const double ×tamp,
cv::Mat &imRGBOut,
cv::Mat &imDOut,
cv::Mat &maskOut)
```
- 将RGB图像转为灰度图像
```cpp
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
```
- 对深度图像进行预处理
```cpp
if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
```
convertTo的原型为：https://docs.opencv.org/trunk/d3/d63/classcv_1_1Mat.html
```cpp
void cv::Mat::convertTo (OutputArray m,
int rtype,
double alpha = 1,
double beta = 0
) const
```
m(x,y)=saturate_cast(α(∗this)(x,y)+β)
以前没注意到居然有这么个函数可以直接解决depth facter转换的问题。
- 构造帧
```cpp
mCurrentFrame = Frame(mImGray,imDepth,imMask,_imRGB,timestamp,
mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
```
原型为：（Frame.cc）
```cpp
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
const cv::Mat &imMask,
const cv::Mat &imRGB,
const double &timeStamp,
ORBextractor* extractor, ORBVocabulary* voc,
cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
```
要注意，构造函数有好多个版本，要注意实际使用的是哪个版本。
- 轻量级的跟踪
具体做了什么，后面再看。
```cpp
LightTrack();
```
- 如果LightTrack成功, 使用几何模型来进行修正
考虑：如何修正，几何模型做了些什么工作？
```cpp
if (!mCurrentFrame.mTcw.empty())
{
mGeometry.GeometricModelCorrection(mCurrentFrame,imDepth,imMask);
}
```
- 重新构造帧
```cpp
imRGBOut = _imRGB;
mCurrentFrame = Frame(mImGray,imDepth,imMask,imRGBOut,
timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
```
考虑：为什么要重新构造帧呢？
仔细观察会发现 第四个参数发生了变化。
- Track
```cpp
Track();
```
这里才是进行正式的跟踪算法，那么之前的LightTrack有什么用呢？
- 跟踪成功后对图像进行Inpaint (不知如何翻译)
因为去掉了动态物体之后，会留下很多的黑洞，所以要修补图像。
```cpp
if (!mCurrentFrame.mTcw.empty())
{
mGeometry.InpaintFrames(mCurrentFrame, mImGray, imDepth, imRGBOut, imMask);
}
```
- 将当前帧加入到几何模块的数据库中
```观察家「
mGeometry.GeometricModelUpdateDB(mCurrentFrame);
```
函数原型为：(Geometry.cc)
```cpp
void Geometry::GeometricModelUpdateDB(const ORB_SLAM2::Frame ¤tFrame){
if (currentFrame.mIsKeyFrame)
{
mDB.InsertFrame2DB(currentFrame);
}
}
```
- 返回结果
```cpp
imDOut = imDepth;
imDepth.convertTo(imDOut,CV_16U,1./mDepthMapFactor);
maskOut = imMask;
return mCurrentFrame.mTcw.clone();
```
分析到这里，我们遇到了非常多的疑问：
- 为什么两次构建帧结构？
- LightTrack 是什么？
- Track 与传统的ORB SLAM相比有何异同？
- 几何模块的： GeometricModelCorrection、InpaintFrames、GeometricModelUpdateDB是什么？
---
# 语义模块分析
## 构造帧
- 函数定义
```cpp
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
const cv::Mat &imMask,
const cv::Mat &imRGB,
const double &timeStamp,
ORBextractor* extractor, ORBVocabulary* voc,
cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
```
- 提取ORB特征
```cpp
ExtractORB(0,imGray);
```
- 对Mask 语义标签图像进行滤波
```cpp
// Delete those ORB points that fall in Mask borders (Included by Berta)
cv::Mat Mask_dil = imMask.clone();
int dilation_size = 15;
cv::Mat kernel = getStructuringElement(cv::MORPH_ELLIPSE,
cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ),
cv::Point( dilation_size, dilation_size ) );
cv::erode(imMask, Mask_dil, kernel);
```
还记得在主程序中，对mask进行了一次dilate操作。
- 去掉人身上的ORB 特征点
```cpp
std::vector _mvKeys;
cv::Mat _mDescriptors;
for (size_t i(0); i < mvKeys.size(); ++i)
{
int val = (int)Mask_dil.at(mvKeys[i].pt.y,mvKeys[i].pt.x);
if (val == 1)
{
_mvKeys.push_back(mvKeys[i]);
_mDescriptors.push_back(mDescriptors.row(i));
}
}
mvKeys = _mvKeys;
mDescriptors = _mDescriptors;
```
到这里我们才开始真正走进了DynaSLAM的算法学习部分。之前的基本上都是ORB SLAM2中的东西 。
要理解此处的操作，先要弄清楚最初的mask长的是什么样的，查阅MaskNet.cc发现， 它就是一个0,1图像。
```sh
seg.cv::Mat::convertTo(seg, CV_8U); //0 background y 1 foreground
```
主程序中在形态学滤波后对其进行了取反操作：
```cpp
cv::Mat mask = cv::Mat::ones(480, 640, CV_8U);
cv::Mat maskRCNN;
maskRCNN = MaskNet->GetSegmentation(imRGB, string(argv[5]), vstrImageFilenamesRGB[ni].replace(0, 4, ""));
cv::Mat maskRCNNdil = maskRCNN.clone();
cv::dilate(maskRCNN, maskRCNNdil, kernel);
mask = mask - maskRCNNdil; //inverse
```
那就变成了： 1 为背景，0为前景。又因为此处进行了一次dilate操作，所以现在要进行erode操作来还原（起到了滤波的效果），相当于进行了一次开（Opening）操作。在背影图像中的前景小洞就被滤除了。
之前的dilate 膨胀操作用来去除边缘上的特征点。
之后把前景中的特征点去掉就可以了。
这就是去除人身上特征点的过程，仅此而已。
---
## LightTrack
这里面最重要的是这个函数：
```cpp
bool Tracking::LightTrackWithMotionModel(bool &bVO)
```
如果未初始化或者没有图像数据则不执行LightTrack, 也就意味着程序刚开始运行时，是不会执行的。
```cpp
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
{
cout << "Light Tracking not working because Tracking is not initialized..." << endl;
return;
}
```
下面与ORB SLAM类似。让我们来看关键的LightTrackWithMotionModel函数。
这个函数是原版本的一个变体：
```cpp
bool Tracking::TrackWithMotionModel()
```
对照这两个函数来看差异。但是我几乎没看到有多大区别，除了：
```cpp
Frame lastFrameBU = mLastFrame;
list lpTemporalPointsBU = mlpTemporalPoints;
...
mLastFrame = lastFrameBU;
mlpTemporalPoints = lpTemporalPointsBU;
```
似乎只是增加了对现场的保存工作。
在重新构建Frame之后进行传统的Track.
Track部分，作者未做改动
```cpp
void Tracking::Track()
```
那么之前的LightTrack起到了什么作用？
我觉得：
1）可能更新了Local Map (论文中图2所示的结构中没有对Local Map进行写入操作，这个猜想可能是错误的，有待验证)
2）可能优化了位姿
因为是去除了大部分动态的特征点，所以误差会减小。
# 几何模块分析
## 简介
代码在 Geometry.h/cpp 中。
虽然通过语义图像分割可以去除掉大部分动态特征点，但这些只是基于先验知识。对于先验知识以外的动态物体，可以通过几何模块将这些动态物体上的特征点去除掉。
设计了动态关键点类：
```cpp
class DynKeyPoint
{
public:
cv::Point2i mPoint;
int mRefFrameLabel;
};
```
设计了一个数据库类：
```cpp
class DataBase
{
public:
vector mvDataBase = vector(MAX_DB_SIZE);
int mIni=0;
int mFin=0;
int mNumElem = 0;
bool IsFull();
void InsertFrame2DB(const ORB_SLAM2::Frame ¤tFrame);
};
```
比较关键的是这几个函数：
```cpp
void GeometricModelCorrection(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &imDepth, cv::Mat &mask);
void InpaintFrames(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &imGray, cv::Mat &imDepth, cv::Mat &imRGB, cv::Mat &mask);
void GeometricModelUpdateDB(const ORB_SLAM2::Frame &mCurrentFrame);
```
## GeometricModelUpdateDB
```cpp
void Geometry::GeometricModelUpdateDB(const ORB_SLAM2::Frame ¤tFrame){
if (currentFrame.mIsKeyFrame)
{
mDB.InsertFrame2DB(currentFrame);
}
}
```
这一步就是将关键帧插入到几何数据库之中
```cpp
void Geometry::DataBase::InsertFrame2DB(const ORB_SLAM2::Frame ¤tFrame){
if (!IsFull()){
mvDataBase[mFin] = currentFrame;
mFin = (mFin + 1) % MAX_DB_SIZE;
mNumElem += 1;
}
else {
mvDataBase[mIni] = currentFrame;
mFin = mIni;
mIni = (mIni + 1) % MAX_DB_SIZE;
}
}
```
猜测一下这是用的什么数据结构？else后面的总感觉不大对劲， mIni比mFin要超前一步，这样不就一直都是Full了。
```cpp
#define MAX_DB_SIZE 20
bool Geometry::DataBase::IsFull(){
return (mIni == (mFin+1) % MAX_DB_SIZE);
}
```
后面那一句是不是应该改成：
```cpp
mFin = (mIni + 1) % MAX_DB_SIZE;
```
感觉mFin应该是队尾指针，从队尾插入新的数据。mIni应该是队头指针，初始值为0. 暂且这么理解吧。
## GeometricModelCorrection
```cpp
void Geometry::GeometricModelCorrection(const ORB_SLAM2::Frame ¤tFrame,
cv::Mat &imDepth, cv::Mat &mask){
if(currentFrame.mTcw.empty()){
std::cout << "Geometry not working." << std::endl;
}
else if (mDB.mNumElem >= ELEM_INITIAL_MAP){
vector vRefFrames = GetRefFrames(currentFrame);
vector vDynPoints = ExtractDynPoints(vRefFrames,currentFrame);
mask = DepthRegionGrowing(vDynPoints,imDepth);
CombineMasks(currentFrame,mask);
}
}
```
输入参数： 当前帧， 深度图 与 掩码图像
如果当前帧跟踪失败的话，就退出 。所以当我们看到 ...not working的提示时，还以为我们的开发环境没编译好，其实不是的，这是正常现像。
```cpp
if(currentFrame.mTcw.empty()){
std::cout << "Geometry not working." << std::endl;
}
```
如果数据库中的项目数大于最低要求5. 那么就继续。
- 获得当前帧的参考帧
- 提取动态特征点
- 通过深度区域增长算法来生成mask
- 将生成的mask整合到当前帧中
## GetRefFrames
```cpp
vector Geometry::GetRefFrames(const ORB_SLAM2::Frame ¤tFrame）
```
从几何模块数据库中去搜索参考帧，返回的是一个数组，所以说找到的参考帧可能有多个。
大体方法是比较平移与旋转的二范数距离差，位姿相近的帧看作是参考帧。
TODO: 细节暂且略过，因为不妨碍继续阅读。
## ExtractDynPoints
```cpp
vector Geometry::ExtractDynPoints(const vector &vRefFrames,
const ORB_SLAM2::Frame ¤tFrame)
```
这段程序好长啊。
```cpp
mDepthThreshold = 0.6;
cv::Mat matDepthDifference = matProjDepth - matDepthCurrentFrame;
for (int i(0); i < matCurrentFrame.rows; i++)
{
if (matDepthDifference.at(i,0) > mDepthThreshold)
{}
}
```
动态点的判定条件： mDepthThreshold
matDepthDifference： 投影深度$z_{proj}$与当前帧中的观测得到的深度$z\prime$之差。对应论文中的图3.
A pixel is labeled as dynamic if the difference $\Delta z = z_{proj} - z \prime$ is greater than a threshold $\tau_z$.
Youtube demo: https://www.youtube.com/embed/aiqLlyiriXI
# 程序流程分析
## 主程序流程
入口程序：rgbd_tum.cc
- 读取数据集 LoadImages
- 创建实例分割的实例 SegmentDynObject
SegmentDynObject 这个类是负责实例分割的。MaskRCNN是基于Python开发的，在此用CPP来调用Python接口。由于ROS是只支持Python2.7的，所以此处的MaskRCNN是使用的Python2.
在类中，最关键的函数是：
```cpp
cv::Mat GetSegmentation(cv::Mat &image, std::string dir = "no_save", std::string rgb_name = "no_file");
```
- 初使化SLAM (RGB-D mode)
```cpp
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
```
- 设置中间结果保存路径， 结果将保存到 rgb, depth, mask, label 文件夹中
- 开始主循环，遂帧读取图像 (nImages 为数据集中图像的个数):
```cpp
for (int ni = 0; ni < nImages; ni++)
{
//LOOP
}
```
- 形态学滤波, 设置核函数
```cpp
// Dilation settings
int dilation_size = 15;
cv::Mat kernel = getStructuringElement(
cv::MORPH_ELLIPSE, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
cv::Point(dilation_size, dilation_size));
```
- 实例分割
```cpp
cv::Mat maskRCNN;
maskRCNN = MaskNet->GetSegmentation(imRGB, string(argv[5]), vstrImageFilenamesRGB[ni].replace(0, 4, ""));
cv::Mat maskRCNNdil = maskRCNN.clone();
cv::dilate(maskRCNN, maskRCNNdil, kernel);
mask = mask - maskRCNNdil; //inverse
```
得到分割后的图像maskRCNN，
进行形态学滤波， 之后求逆 （考滤：为什么要这么操作呢？）
- Tracking线程
分两种情况，
一种是使用语义分割：
```cpp
SLAM.TrackRGBD(imRGB, imD, mask, tframe, imRGBOut, imDOut, maskOut);
```
另一种是不使用语义分割，全依靠几何信息：
```cpp
SLAM.TrackRGBD(imRGB, imD, mask, tframe);
```
考滤：第二种情况与传统的ORB SLAM相比有何异同？
- 收尾工作
- 计算时间消耗
- 保存轨迹
```cpp
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
```
## Tracking分析
- TrackRGBD的跟踪主函数入口
Tracking是一个单独的线程。
```cpp
cv::Mat System::TrackRGBD(const cv::Mat &im,
const cv::Mat &depthmap,
cv::Mat &mask,
const double ×tamp,
cv::Mat &imRGBOut,
cv::Mat &imDOut,
cv::Mat &maskOut)
```
输入参数有四个：RGB图像，Depth图像，Mask 语义Label， 时间戳。
输出有三个：处理后的RGB图像，处理后的深度图像，处理后的Mask 。
考虑：进行了哪些处理？
- GrabImageRGBD (Tracking.cc)
在TrackRGBD中最主要的函数就是GrabImageRGBD。参数与上面相同，程序返回当前帧的位姿$Tcw$.
```cpp
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,
const cv::Mat &imD,
cv::Mat &mask,
const double ×tamp,
cv::Mat &imRGBOut,
cv::Mat &imDOut,
cv::Mat &maskOut)
```
- 将RGB图像转为灰度图像
```cpp
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
```
- 对深度图像进行预处理
```cpp
if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
```
convertTo的原型为：https://docs.opencv.org/trunk/d3/d63/classcv_1_1Mat.html
```cpp
void cv::Mat::convertTo (OutputArray m,
int rtype,
double alpha = 1,
double beta = 0
) const
```
m(x,y)=saturate_cast
No comments