文章目录
- 0 引言
- 1 安装依赖
- 1.1 其他库安装
- 1.2 pcl库安装
- 2 编译ORB-SLAM2_RGBD_DENSE_MAP
- 2.1 build.sh
- 2.2 build_ros.sh
- 3 运行ORB-SLAM2_RGBD_DENSE_MAP
- 3.1 build.sh编译版本
- 3.2 build_ros.sh编译版本
0 引言
ORB-SLAM2_RGBD_DENSE_MAP
是基于ORB-SLAM2
框架的一种RGB-D
稠密地图构建算法。ORB-SLAM2
是一种基于单目、双目和RGB-D
相机的实时定位与建图(SLAM
)系统,旨在通过计算机视觉技术实现机器人和自主驾驶汽车等设备的自主定位和地图构建。
ORB-SLAM2_RGBD_DENSE_MAP
的主要思想是结合ORB-SLAM2
框架的稠密重建算法和体素网格(Voxel Grid
)滤波器,对RGB-D
相机采集到的图像数据进行处理,从而得到一个稠密的3D
点云表示,进而构建出稠密的地图。该算法通过对相邻帧之间的视差图进行计算,利用三角测量法估计相机到场景中每个像素的深度值,并通过体素网格滤波器对深度图进行降采样,从而减少计算量和存储空间。最终,该算法可以生成一个具有高精度和高分辨率的稠密地图。
ORB-SLAM2_RGBD_DENSE_MAP
算法的优点是具有较高的重建精度和鲁棒性,在各种环境和光照条件下均表现良好。然而,由于该算法需要进行大量的计算和存储,因此它的实时性和运行效率可能受到一定的影响。
本文系统环境:
- Ubuntu18.04
- ROS-melodic
- OpenCV3.2.0
- Eigen 3.3.4
- Pangolin-0.6
- PCL 1.8.1
- realsense-ros (若用硬件D435i)
1 安装依赖
1.1 其他库安装
ORB-SLAM2算法1已针对Ubuntu20.04
安装ORB-SLAM2
所需的依赖库,安装 OpenCV
,Eigen
,Pangolin
可以参考下。
1.2 pcl库安装
但ORB-SLAM2_RGBD_DENSE_MAP
因为是稠密建图,还需要安装pcl1.7
以上版本,本文是安装了pcl1.8
大版本,可先下载 pcl 1.8.1
版本(点击Source code(zip)
先安装pcl1.8.1
所需的依赖库:
sudo apt-get update
sudo apt-get install git build-essential linux-libc-dev
sudo apt-get install cmake cmake-gui
sudo apt-get install libusb-1.0-0-dev libusb-dev libudev-dev
sudo apt-get install mpi-default-dev openmpi-bin openmpi-common
sudo apt-get install libflann1.9 libflann-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libboost-all-dev
sudo apt-get install libvtk7.1-qt libvtk7.1
sudo apt-get install libqhull* libgtest-dev
sudo apt-get install freeglut3-dev pkg-config
sudo apt-get install libxmu-dev libxi-dev
sudo apt-get install mono-complete
sudo apt-get install openjdk-8-jdk openjdk-8-jre
然后解压下载的pcl1.8.1
源码,并进入到pcl1.8.1
文件夹中
mkdir build
cd build
cmake ..
make -j4
sudo make install
测试pcl是否安装成功的话,可用pcl_viewer xxx.pcd
;如果没有pcd文件,可去点云库PCL(Point Cloud Library)的学习资源汇总下载rabbit.pcd
pcl_viewer rabbit.pcd
2 编译ORB-SLAM2_RGBD_DENSE_MAP
编译类似ORB-SLAM2
的安装,但可能会出现不一样的问题。首先下载ORB-SLAM2_RGBD_DENSE_MAP
git clone https://github.com/xiaobainixi/ORB-SLAM2_RGBD_DENSE_MAP.git
由于安装的是pcl1.8
,首先将CMakeLists.txt
中find_package( PCL 1.7 REQUIRED )
改成了find_package( PCL 1.8 REQUIRED )
2.1 build.sh
# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
chmod +x build.sh
./build.sh
tips: ./build.sh > build_log.txt 2>&1
可打印整个编译log
到文件
- 编译问题1
make: *** [all] Error 1
Converting vocabulary to binary
./build.sh: line 30: ./tools/bin_vocabulary: Permission denied
原因是./tools/bin_vocabulary
词袋文件没有执行权限,如下给其权限
# ORB-SLAM2_RGBD_DENSE_MAP 目录下执行
sudo chmod +x tools/bin_vocabulary
- 编译问题2
../Thirdparty/DBoW2/lib/libDBoW2.so: undefined reference to `cv::_OutputArray::_OutputArray(cv::Mat&)'
collect2: error: ld returned 1 exit status
CMakeFiles/rgbd_tum.dir/build.make:391: recipe for target '../bin/rgbd_tum' failed
make[2]: *** [../bin/rgbd_tum] Error 1
CMakeFiles/Makefile2:124: recipe for target 'CMakeFiles/rgbd_tum.dir/all' failed
make[1]: *** [CMakeFiles/rgbd_tum.dir/all] Error 2
Makefile:90: recipe for target 'all' failed
make: *** [all] Error 2
Converting vocabulary to binary
./tools/bin_vocabulary: error while loading shared libraries: libopencv_core3.so.3.3: cannot open shared object file: No such file or directory
Error1
一开始以为OpenCV
的问题,但后来排查到我这个是由于之前尝试编译时,第三方库DBoW2
和g2o
没有完全编译,删除两个第三方库中的build
文件夹,重新编译即可。
Error2
是tools
中的bin_vocabulary
词袋找不到libopencv_core3.so.3.3
,而且ORB-SLAM2_RGBD_DENSE_MAP
的lib
也没有该版本的opencv
动态链接库,而本文系统安装的是OpenCV
3.2.0,版本也不符合,所以临时解决办法是不用bin_vocabulary
词袋可执行文件,改用ORB-SLAM2
工程中的词袋文件ORBvoc.txt
# 首先注释掉 build.sh 中的bin_vocabulary部分
# 最后几行注释掉# cd ..
# echo "Converting vocabulary to binary"
# ./tools/bin_vocabulary
最后复制ORB-SLAM2
中的词袋文件夹Vocabulary
到ORB-SLAM2_RGBD_DENSE_MAP
工程目录下。
重新编译,即可编译成功
/usr/include/eigen3/Eigen/src/Core/util/Constants.h:162:37: note: declared hereEIGEN_DEPRECATED const unsigned int AlignedBit = 0x80;^~~~~~~~~~
[ 94%] Linking CXX executable ../bin/mono_tum
[ 97%] Linking CXX executable ../bin/mono_kitti
[100%] Linking CXX executable ../bin/mono_euroc
[100%] Built target mono_tum
[100%] Built target mono_kitti
[100%] Built target mono_euroc
2.2 build_ros.sh
首先添加该工程到ROS_PACKAGE_PATH
gedit ~/.bashrc
# 最后一行新增,其中冒号后的PATH是自己ORB-SLAM2_RGBD_DENSE_MAP工程存放目录
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB-SLAM2_RGBD_DENSE_MAP/Examples/ROS
# 保存后source
source ~/.bashrc
然后执行如下命令进行编译
# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
chmod +x build_ros.sh
./build_ros.sh
3 运行ORB-SLAM2_RGBD_DENSE_MAP
3.1 build.sh编译版本
由于用到rgb
和depth
图,所以参考ORB-SLAM2算法2下载TUM
的rgbd_dataset_freiburg1_desk
数据集,准备好数据集后,可执行以下命令,其中PATH
是rgbd_dataset_freiburg1_desk
文件夹的存放目录
# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml PATH/rgbd_dataset_freiburg1_desk PATH/rgbd_dataset_freiburg1_desk/associations.txt
- 运行问题1
Depth Threshold (Close/Far Points): 3.09294-------
Start processing sequence ...
Images in the sequence: 573New map created with 945 points
Segmentation fault (core dumped)
这种 段错误(核心已转储)
的问题本文暂未完全解决(如有,欢迎分享交流),先用以下的临时解决办法(该问题仍会偶发):
- 在
CMakeLists.txt
和Thirdparty/g2o/CMakeLists.txt
中删除-march=native
# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")
- 在有定义
Eigen
的头文件中添加预编译命令EIGEN_MAKE_ALIGNED_OPERATOR_NEW
,分别是include
路径下Converter.h、LoopClosing.h、PointCloude.h
这三个文件
Converter.h
class Converter
{public:# 添加预编译命令EIGEN_MAKE_ALIGNED_OPERATOR_NEWstatic std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
LoopClosing.h
(两次修改,第一个public
中添加,第二个public
中的注释掉)
class LoopClosing
{public:# 添加预编译命令EIGEN_MAKE_ALIGNED_OPERATOR_NEWtypedef pair<set<KeyFrame*>,int> ConsistentGroup;typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;public:LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, shared_ptr<PointCloudMapping> pPointCloud);void SetTracker(Tracking* pTracker);void SetLocalMapper(LocalMapping* pLocalMapper);// Main functionvoid Run();void InsertKeyFrame(KeyFrame *pKF);void RequestReset();// This function will run in a separate threadvoid RunGlobalBundleAdjustment(unsigned long nLoopKF);shared_ptr<PointCloudMapping> mpPointCloudMapping;bool isRunningGBA(){unique_lock<std::mutex> lock(mMutexGBA);return mbRunningGBA;}bool isFinishedGBA(){unique_lock<std::mutex> lock(mMutexGBA);return mbFinishedGBA;}void RequestFinish();bool isFinished();# 注释掉预编译命令// EIGEN_MAKE_ALIGNED_OPERATOR_NEWint loopcount = 0;
PointCloude.h
class PointCloude
{typedef pcl::PointXYZRGBA PointT;typedef pcl::PointCloud<PointT> PointCloud;
public:PointCloud::Ptr pcE;
public:# 添加预编译命令EIGEN_MAKE_ALIGNED_OPERATOR_NEWEigen::Isometry3d T;int pcID;
- 运行问题2
viewer视图中没有点云,只有坐标系
改动include
文件夹中的 pointcloudmapping.h
头文件
# bool loopbusy;
bool loopbusy = false;
- 运行问题3
确认TUM1.yaml
文件里有没有点云地图的参数,如果没有,就把以下几行加在最后面
PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0
重新执行./build.sh
,编译完成后,重新执行:
# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml PATH/rgbd_dataset_freiburg1_desk PATH/rgbd_dataset_freiburg1_desk/associations.txt
运行结果:
median tracking time: 0.0200984
mean tracking time: 0.0206714Saving camera trajectory to CameraTrajectory1.txt ...trajectory saved!Saving keyframe trajectory to CameraTrajectory2.txt ...trajectory saved!
globalMap save finished
Current Frame
关键帧可视化界面:
Map Viewer
轨迹地图可视化界面:
viewer
稠密地图可视化界面:
保存的稠密地图文件result.pcd
可用已安装的pcl
打开:
pcl_viewer result.pcd
至此,成功用TUM
数据包运行非ROS
版本的ORB-SLAM2_RGBD_DENSE_MAP
,并可视化生成的稠密地图。
3.2 build_ros.sh编译版本
本文这里就不用TUM
的开源数据测试了,因为恰好有D435i
相机可以实时发出深度图和rgb
图来测试(录制的一小段数据ORB-SLAM2-RGBD-DENSE-MAP-data.tar),所以以下就是以此为例。
默认已经安装realsense-ros
的相机驱动,首先是相机启动文件(rs_camera.launch
)的配置,保证发出与rgb
图对齐的深度图:false
修改成true
<arg name="enable_depth" default="true"/>
然后插上D435i
相机后(USB3.0
),新开终端启动D435i
相机:
# source 激活realsense-ros的相机驱动后执行
roslaunch realsense2_camera rs_camera.launch
启动后,通过rostopic list
可查的所需的rgb
图和对齐的深度图的topic
# 与rgb图对齐的深度图
/camera/aligned_depth_to_color/image_raw
# rgb图
/camera/color/image_raw
然后修改代码中的两个topic
,在ORB-SLAM2_RGBD_DENSE_MAP/Examples/ROS/ORB_SLAM21/src/ros_rgbd.cc
文件中修改:
// message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/kinect2/qhd/image_color_rect", 1);// message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/kinect2/qhd/image_depth_rect", 1);message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);
当然,还有对应的配置文件,在ORB-SLAM2_RGBD_DENSE_MAP/Examples/ROS/ORB_SLAM21/
目录下新建配置文件d435i.yaml
,主要是fx, fy, cx, cy
、分辨率,帧率,baseline等要修改成所用的D435i
相机对应的,好在D435i
相机发出的rgb
图已经是去畸变的,而且给出了内参,可通过rostopic echo /camera/color/camera_info
命令查询,具体如下:
查询结果:其中K
即是所需的内参
header: seq: 0stamp: secs: 1691576284nsecs: 247732162frame_id: "camera_color_optical_frame"
height: 540
width: 960
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [682.6236572265625, 0.0, 490.54339599609375, 0.0, 682.521240234375, 275.81976318359375, 0.0, 0.0,
1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [682.6236572265625, 0.0, 490.54339599609375, 0.0, 0.0, 682.521240234375, 275.81976318359375, 0.0,
0.0, 0.0, 1.0, 0.0]
d435i.yaml
配置文件信息:
%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 682.6236572265625
Camera.fy: 682.521240234375
Camera.cx: 490.54339599609375
Camera.cy: 275.81976318359375Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0Camera.width: 960
Camera.height: 540
# Camera frames per second
Camera.fps: 15.0# IR projector baseline times fx (aprox.)
# bf = baseline (in meters) * fx, D435i的 baseline = 50 mm
Camera.bf: 50.0# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1# Close/Far threshold. Baseline times.
ThDepth: 40.0# Deptmap values factor
DepthMapFactor: 1000.0#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0
然后重新执行./build_ros.sh
编译,编译成功后执行如下命令运行:
# ORB-SLAM2_RGBD_DENSE_MAP 目录下执行
rosrun ORB_SLAM21 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM21/d435i.yaml
运行后相比原版的ORB-SLAM2
多生成一个viewer
的稠密建图的可视化界面:
Current Frame可视化界面:
Map Viewer可视化界面:
viewer可视化界面:
至此,成功用在线加载D435i
相机发出的rgb
图和对齐的深度图,运行ROS
版本的ORB-SLAM2_RGBD_DENSE_MAP
,并可视化生成的稠密地图。
Reference:
- ORB-SLAM2_RGBD_DENSE_MAP
- ORB-SLAM2算法1之Ubuntu20.04+ROS-noetic安装ORB-SLAM2及各种问题解决
- 点云库PCL(Point Cloud Library)的学习资源汇总
- ORB-SLAM2算法2之TUM开源数据运行ORB-SLAM2生成轨迹并用evo工具评估轨迹
- ORB-SLAM2-RGBD-DENSE-MAP-data.tar
⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔