DynaSLAM代码详解(1) — RGBD模式DynaSLAM运行流程

论文翻译地址：动态SLAM论文(2) — DynaSLAM: Tracking, Mapping and Inpainting in Dynamic Scenes_几度春风里的博客-CSDN博客

1.1 DynaSLAM和ORB-SLAM2文件对比

DynaSLAM是一个建立在ORB-SLAM2基础上的视觉SLAM系统，它增加了动态物体检测和背景修复的能力。DynaSLAM在单目、立体和RGB-D配置下对动态场景非常稳健。能够通过多视角几何、深度学习或两者结合来检测移动物体。拥有场景的静态地图允许修复被这些动态物体遮挡的帧背景。

ORB-SLAM2和DynaSLAM的文件对比如下，红色为DynaSLAM相对于ORB-SLAM2多出的文件。

文件目录	文件	功能
	python	Mask-RCNN目标检测文件
	Conversion.cc	包含一些数据类型之间的转换函数，用于在不同类型之间进行数据转换
	Converter.cc	包含将动态点云数据转换为Dynaslam数据格式的函数
	Frame.cc	用于表示相机的一帧图像，其中包含了图像的各种属性和特征点
	FrameDrawer.cc	用于在图像上绘制特征点、相机轨迹等信息
	Geometry.cc	包含一些几何计算的函数，例如计算两条线段的交点等
	Initializer.cc	用于初始化相机的位置和姿态
	KeyFramer.cc	用于管理关键帧的生成和选取
	KeyFrameDatabase.cc	用于管理关键帧的数据库，用于地图匹配和回环检测
	LocalMapping.cc	用于局部地图建立和更新
	loopClosing.cc	用于检测并闭合回环
	Map.cc	用于管理地图的点云和关键帧
	MapPoint.cc	用于表示地图中的点云
	MaskNet.cc	用于进行目标检测和语义分割
	MaskNetStereo.cc	用于进行立体目标检测和语义分割
	Optimizer.cc	用于对地图中的点云进行优化
	ORBextractor.cc	用于提取图像的ORB特征点
	ORBmatcher.cc	用于进行ORB特征点的匹配
	PnPsolver.cc	用于求解相机的位置和姿态
	RegionProps.cc	用于提取图像中的目标区域
	Sim3Solver.cc	于求解相机的相似变换矩阵
	System.cc	是整个系统的入口，包含了主要的函数和流程
	Tracking.cc	用于跟踪相机的运动和定位
	Viewer.cc	用于可视化显示地图和相机的运动轨迹

1.2 RGBD模式运行流程

笔记将以RGBD模式运行为基础，讲解整个DynaSLAM的运行流程，RGBD模式的运行在 /Example/RGB-D/rgbd_tum.cc文件下。

检查和加载相关的配置文件，首先判断传入的参数数是否符合要求，然后进行变量的声明用于存放彩色图像、深度图像的路径，以及对应的时间戳的变量。

int main(int argc, char **argv)
{
    // 判断传入的参数数是否符合要求
    if(argc != 5 && argc != 6 && argc != 7)
    {
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association (path_to_masks) (path_to_output)" << endl;
        return 1;
    }

    // Retrieve paths to images
    //按顺序存放需要读取的彩色图像、深度图像的路径，以及对应的时间戳的变量
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    ...
}

然后加载关联文件，从文件中加载rgb图像路径、时间戳、深度图像路径。

/*
 * 从关联文件中提取这些需要加载的图像的路径和时间戳 
*/
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    //输入文件流
    ifstream fAssociation;
    //打开关联文件
    fAssociation.open(strAssociationFilename.c_str());
    //一直读取,直到文件结束
    while(!fAssociation.eof())
    {
        string s;
        //读取一行的内容到字符串s中
        getline(fAssociation,s);
        //如果不是空行就可以分析数据了
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            //字符串格式:  时间戳rgb图像路径 时间戳 深度图像路径
            double t;
            string sRGB, sD;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);
            ss >> t;
            ss >> sD;
            vstrImageFilenamesD.push_back(sD);
        }
    }
}

随后开始初始化MaskRCNN网络和ORB-SLAM2系统

    // Initialize Mask R-CNN
    // 初始化Mask R-CNN
    DynaSLAM::SegmentDynObject *MaskNet;
    if (argc==6 || argc==7)
    {
        cout << "Loading Mask R-CNN. This could take a while..." << endl;
        MaskNet = new DynaSLAM::SegmentDynObject();
        cout << "Mask R-CNN loaded!" << endl;
    }
    //初始化ORB-SLAM2系统
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

与ORB-SLAM2的不同：因为有关于图像分割的操作，因此在设置了图像膨胀的相关参数，后面也创建了相关的的文件路径用于TrackRGB中的输出imRGBOut,imDOut,maskOut。

    // Dilation settings
    // 设置图像膨胀（dilation）的参数，并创建一个膨胀操作的核（kernel）
    int dilation_size = 15;     // 膨胀的大小为15
    // 使用getStructuringElement函数创建了一个膨胀操作的核
    cv::Mat kernel = getStructuringElement(cv::MORPH_ELLIPSE,   //表示椭圆形
                                           cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ),  //表示核的大小
                                           cv::Point( dilation_size, dilation_size ) );     //表示核的锚点位置
    
    // 创建一系列的文件目录用于存放rgb、depth和mask
    if (argc==7)
    {
        std::string dir = string(argv[6]);
        mkdir(dir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
        dir = string(argv[6]) + "/rgb/";
        mkdir(dir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
        dir = string(argv[6]) + "/depth/";
        mkdir(dir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
        dir = string(argv[6]) + "/mask/";
        mkdir(dir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
    }

随后开始正式的处理，对图像序列中的每张图像展开遍历，首先从文件中读取RGB、depth和时间戳并检查图像的合法性；随后开始进行图像分割，对分割的结果利用膨胀操作；最后根据不同的参数利用不同的TrackRGBD函数开启跟踪线程，并将跟踪线程输出的结果imRGBOut，imDOut, maskOut 保存到创建的文件路径中。

    //对图像序列中的每张图像展开遍历
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image and depthmap from file
        // 从文件中读取RGB、depth和时间戳
        imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];
        
        // 检查图像的合法性
        if(imRGB.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
            return 1;
        }

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Segment out the images
        // 开始进行图像分割
        cv::Mat mask = cv::Mat::ones(480,640,CV_8U);
        if (argc == 6 || argc == 7)
        {
            cv::Mat maskRCNN;
            // 利用GetSegmentation()函数进行图像分割
            maskRCNN = MaskNet->GetSegmentation(imRGB,string(argv[5]),vstrImageFilenamesRGB[ni].replace(0,4,""));
            // 将分割的结果 maskRCNN 复制到 maskRCNNdil
            cv::Mat maskRCNNdil = maskRCNN.clone();
            // 对 maskRCNN 应用膨胀操作，使用 kernel 作为内核
            cv::dilate(maskRCNN,maskRCNNdil, kernel);
            // 将 maskRCNNdil 从 mask 中减去,得到最终的mask结果
            mask = mask - maskRCNNdil;
        }

        // Pass the image to the SLAM system
        // 根据不同的参数利用不同的TrackRGBD函数开启跟踪线程
        if (argc == 7){SLAM.TrackRGBD(imRGB,imD,mask,tframe,imRGBOut,imDOut,maskOut);}
        else {SLAM.TrackRGBD(imRGB,imD,mask,tframe);}

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
        // 将跟踪线程输出的结果imRGBOut,imDOut,maskOut保存到创建的文件路径中
        if (argc == 7)
        {
            cv::imwrite(string(argv[6]) + "/rgb/" + vstrImageFilenamesRGB[ni],imRGBOut);
            vstrImageFilenamesD[ni].replace(0,6,"");
            cv::imwrite(string(argv[6]) + "/depth/" + vstrImageFilenamesD[ni],imDOut);
            cv::imwrite(string(argv[6]) + "/mask/" + vstrImageFilenamesRGB[ni],maskOut);
        }

最后等待所有的图像处理完成后终止SLAM过程，统计分析追踪耗时和保存最终的相机轨迹。

    // Stop all threads
    //终止SLAM过程
    SLAM.Shutdown();

    // Tracking time statistics
    //统计分析追踪耗时
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    //保存最终的相机轨迹
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");