基于ORB-SLAM2的语义地图构建

github: ORBSLAM2_Semantic_Mapping
注：这个版本使用C++调用python环境，运行冲突比较多，很多设置了绝对路径。改进的办法是将语义分割部分写成了服务端，参考基于ORB-SLAM2的语义地图构建，分成服务端和客户端。

介绍

简单地实现了一个融合图像语义分割信息的语义地图。总体思路参考：Semanticfusion: Dense 3d semantic mapping with convolutional neural networks。使用的是COCO-Stuff数据集，语义分割模型deeplabv2。

语义分割结果展示（去除了未带语义标签的地图点）：

编译运行

仅RGBD相机适用。

依赖安装

• ORB-SLAM2
• deeplab-pytorch
• 权重文件放在deeplabv2/data/models中，可以在deeplab-pytorch中下载。当然也可以在deeplabv2/inference.py修改权重文件的路径和名称。

class SegModel():
    def __init__(self):
        config_path = "deeplabv2/configs/cocostuff164k.yaml"
        model_path = "deeplabv2/data/models/deeplabv2_resnet101_msc-cocostuff164k-100000.pth"
        ......

运行

数据集运行：参考ORB-SLAM2
ROS运行：

# 开启相机（本人用的是realsense D435i相机）
roslaunch realsense2_camera rs_rgbd.launch
# 进入到工作空间的目录下后：
source ./Examples/ROS/ORB_SLAM2/build/devel/setup.bash
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt config/Intel_D435i.yaml null

注意：运行的最后一个参数是保存数据集的根目录（按照TUM数据集的格式），不保存则设置成null。详见ros_rgbd.cc。

源码分析

CmakeLists文件

增加PCL库和python库。

include_directories(
......
${PCL_INCLUDE_DIRS}
"/home/xshen/Anaconda3/envs/deeplab-pytorch/include/python3.6m"
)

add_library(${PROJECT_NAME} SHARED
......
src/PointcloudMapping.cc
src/ProbPoint.cc
)
target_link_libraries(${PROJECT_NAME}
......
${PCL_LIBRARIES}
"/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/python3.6"
"/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/libpython3.6m.so"
)

环境配置问题

图像语义分割模型没有用C++实现，而是用python实现，所以用到了C++调用python，即CPython的使用。

本人在编译时Py_SetPythonHome(mPythonHome);和import_array();会出现问题，也没有解决（可能是由于自己系统安装的库冲突了？），最后设置成了绝对路径。

// void PointCloudMapping::runSegmantation() 

 	Py_SetPythonHome(mPythonHome);  // ??ImportError: No module named site
    Py_Initialize(); // 对python进行初始化，无返回值。使用py_IsInitialized检查系统是否初始化成功。

    // anaconda下deeplab-pytorch环境（Py_SetPythonHome的替换方案）
    // PyRun_SimpleString("import sys"); 
    // PyRun_SimpleString("sys.path = []");
    // PyRun_SimpleString("sys.path.append('/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/python36.zip')");
    // PyRun_SimpleString("sys.path.append('/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/python3.6')");
    // PyRun_SimpleString("sys.path.append('/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/python3.6/lib-dynload')");
    // PyRun_SimpleString("sys.path.append('/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/python3.6/site-packages')");
    // PyRun_SimpleString("sys.path.append('/home/xshen/Anaconda3/envs/deeplab-pytorch/lib/python3.6/site-packages/tqdm-4.7.2-py3.6.egg')");

    import_array();  // 遇到问题RuntimeError: _ARRAY_API is not PyCObject object？
    // 以下替代了import_array();
    // PyObject *numpy = PyImport_ImportModule("numpy.core.multiarray");
    // PyObject *c_api = NULL;
    // c_api = PyObject_GetAttrString(numpy, "_ARRAY_API");
    // PyArray_API = (void **)PyCObject_AsVoidPtr(c_api);

多线程

新建了两个线程，一个是用于生成稠密地图（多余的），另一个是用于生成语义地图。线程之间通过互斥锁和条件变量同步，并用队列存放图像。

数据集

使用COCO-Stuff数据集，总共有80+91+1种类别。（具体类别有所修改）

• PointCloudMapping::initColorMap()：针对不同的场景，仅选择部分类别进行识别。具体类别和对应RGB在colormap.txt文件中。
• PointCloudMapping::mergeSomeClasses()：针对一些细分类别进行了合并，例如各种不同材质的墙，统一归类为墙。相应地在colormap.txt中进行了修改。

语义标签关联并更新

• PointCloudMapping::assoiateAndUpdate()：主要思路是将已经生成的地图点投影到当前帧图像中，然后进行像素点与地图点的匹配，匹配过程中主要有两个参数 mThdepth和mThprob，前者是匹配点深度的阈值，后者是匹配点预测类别概率的阈值。
• PointCloudMapping::getProbMap()：调用assoiateAndUpdate()，生成点云。下面代码是给点云中每个点加上索引。

    for (size_t i = 0;  i < current->points.size(); i++) {  // 初始化点概率均匀分布
        mvPoint2ProbPoint[k++] = new ProbPoint(numberOfLabels);
    }