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);
}