Orbslam2 稠密点云 +D435i实现（Ubuntu18.04）

系统：Ubuntu18.04
设备：Realsense D435i

一、安装相关依赖库

毕设后写的一篇通俗一些的，适合没接触过或不太会ubuntu的同学，前半部分都是一样的，
tips：需要安装一个[百度网盘]，以便下载需要的文件。
→ → → orbslam2+小觅相机

相关依赖库

sudo  apt-get  install  cmake gcc g++ git vim
sudo  apt-get  install  libglew-dev
sudo  apt-get  install  libboost-dev libboost-thread-dev 
sudo  apt-get  install  libboost-filesystem-dev
sudo  apt-get  install  libpython2.7-dev
sudo  apt-get  install  build-essential

安Pangolin和Eigen
这两个库，建议是0.5和3.2版本，报错可能性小，其他版本可能会出现不兼容等各种问题，目前从git下载已经是新版，这里提供一下0.5和3.2版本

pangolin：pangolin_v0.5 提取码: edam

Eigen3：Eigen_v3.2.10 提取码: hnw9

安装：

cd  Pangolin  
mkdir  build
cd  build
cmake ..
make 
sudo make install

cd  eigen
mkdir  build
cd  build
cmake ..
make 
sudo make install

二、安装ROS Ubuntu18.04对应melodic

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

sudo apt update
sudo apt install ros-melodic-desktop-full（时间略长，需等待一会）
！复制全了别丢东西！
------------------------------------------------------------------------
sudo rosdep init
出现错误：找不到命令
解决：sudo apt install python-rosdep2
------------------------------------------------------------------------

---

rosdep update
出现 timed out  出现超时问题

解决：解决方法
涉及到的rosdistro文件
rosdistro 提取码: b3hb

---

sudo apt-get install python-rosinstall
source /opt/ros/melodic/setup.bash

创建并初始化工作目录ROS使用一个名为catkin的ROS专用构建系统。为了使用它，用户需要创建并初始化catkin工作目录，如下所示。除非用户创建新的工作目录，否则此设置只需设置一次。

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ~/catkin_ws/
catkin_make
sudo apt install net-tools
gedit ~/.bashrc
在弹出的文本中最后面添加如下两句
source /opt/ros/melodic/setup.bash
source ~/catkin_ws/devel/setup.bash
保存
source ~/.bashrc

进行测试：

roscore

出现错误：没发现roscore
解决：
sudo apt-get install ros-melodic-desktop

重新打开三个终端：

roscore

rosrun turtlesim turtlesim_node

rosrun turtlesim turtle_teleop_key

鼠标点击最后一个终端，按方向键小海龟可以移动，即是配置成功

三、编译orbslam

1.安装pcl点云库，大致按这个教程安装就可以
点云安装

2.下载 高翔博士修改后的orbslam2

git clone https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git

我按照网上教程稍微自己稍微修改了的

复制这段内容后打开百度网盘手机App，操作更方便哦
链接：https://pan.baidu.com/s/18T6kNhHFRxoxiKYo3o3rEg 
提取码：i3CB --来自百度网盘超级会员V5的分享

如果下载高翔原版代码，解压zip，将第一个文件夹中文件，复制替换进 解压后 文件夹中 的ORB_SLAM2_modified

3.编译g2o
解压后找到g2o_with_orbslam2文件夹
如果下载我的应该不需要修改，高翔原版需要改动一下
1）分别对文件夹下cmakelist和g20下的cmakelist对相应语句进行注释
主文件夹下

g20下的

注释后保存

2）修改g2o/types/slam2d/edge_se2_pointxy_bearing.cpp

原代码：t.setRotation(t.rotation().angle()+_measurement);

修改后：t.setRotation((Eigen::Rotation2Dd)(t.rotation().angle()+_measurement));  

3）修改/g2o_with_orbslam2/g2o/solvers/eigen/linear_solver_eigen.h

原代码：typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, SparseMatrix::Index> PermutationMatrix;

修改后：typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, SparseMatrix::StorageIndex> PermutationMatrix;

保存后，回到g2o_with_orbslam2主文件夹下

mkdir build
cmake ..
make -j8
sudo make install

4.进入ORB_SLAM2_modified文件夹
高翔原版没有ORBvoc.txt.tar.gz需要自行下载，而后将其放置于Vocabulary文件夹下，顺手解压一下

ORBvoc.txt.tar.gz

修改文件夹中的cmakelist，将opencv和eigen3修改为自己电脑安装的版本，我前边提供的eigen3为3.2.10，我安装时ros自带的3.2的opencv。如果不清楚可以进行查询

pkg-config --modversion opencv

修改cmakelist为对应版本

来到/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt
查看这 5 处是否正确，并也将对应的库版本号修改为自己安装的版本

修改后，删除
/ORB_SLAM2_modified/build,
/ORB_SLAM2_modified/Thirdparty/DBoW2/build
/ORB_SLAM2_modified/Thirdparty/g2o/build
删掉三个 build 文件夹

在ORB_SLAM2_modified下右键打开终端

cd ORB_SLAM2_modified/Thirdparty/DBoW2
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make 

从新在ORB_SLAM2_modified下右键打开终端

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8

数据集测试，数据集需自行下载，我提供的代码中dataset中有两个下载好直接能用的数据集
如果自己下载需要对rgbd和depth进行关联

python associate.py rgb.txt depth.txt > associations.txt

示例命令
./Example/RGB-D/rgbd_tum　Vocabulary/ORBvoc.txt　path_to_settings　path_to_sequence path_to_association
在ORB_SLAM2_modified打开终端执行

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml datasets/rgbd_dataset_freiburg1_xyz  datasets/rgbd_dataset_freiburg1_xyz/association.txt

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml datasets/rgbd_dataset_freiburg1_room  datasets/rgbd_dataset_freiburg1_room/association.txt

高翔原版似乎没有地图保存和彩色，如果需要保存地图，或者地图不是彩色，请参考文章中间部分
保存地图，地图不是彩色

保存的点云在主文件夹下，名称vslam.pcd，当前文件夹打开终端

pcl_viewer vslam.pcd  (查看点云)

四、安装D435i SDK

Ubuntu18.04 + ROS melodic 安装使用 RealSenseD435i

Ubuntu18.04下安装RealSense D435i相机的SDK和ROS包

五、编译orbslam +ROS

1.来到ORB_SLAM2_modified文件夹
在打开的文本最后添加你的ROS路径，保存

gedit ~/.bashrc
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/你的目录/ORB_SLAM2_modified/Examples/ROS

保存后在终端输入

source ~/.bashrc

chmod +x build_ros.sh
./build_ros.sh

2.修改ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc文件，把rgb_topic和depth_topic订阅话题修改为

 "/camera/color/image_raw";
 "/camera/aligned_depth_to_color/image_raw";

ORB_SLAM2_modified下从新打开终端

chmod +x build.sh 
./build.sh
chmod +x build_ros.sh
./build_ros.sh

从新打开终端，查看内参

roscore
roslaunch realsense2_camera rs_rgbd.launch 
rostopic echo /camera/color/camera_info

没有rs_rgbd.lunch
解决：sudo apt-get install ros-melodic-rgbd-launch
终端中显示的K,为参数，其中
K = [fx 0 cx 0 fy cy 0 0 1 ] 基线50mm

修改参数，得到一个新的D435i.yaml

%YAML:1.0
 
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
 
# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 909.559875
Camera.fy: 909.740478
Camera.cx: 645.366455
Camera.cy: 366.805908

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0

Camera.width: 640
Camera.height: 480
# Camera frames per second 
Camera.fps: 30.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: 500
 
PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0

运行，打开三个终端

roscore
roslaunch realsense2_camera rs_rgbd.launch
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/D435i.yaml

运行视频b站：【视觉slam Orbslam2+D435i 稠密点云，记录-哔哩哔哩

过程中会遇到各种问题，需要大家自己解决喽，因为问题各不相同，所以大家就自行搜索解决，我只记录了一下自己从配置到运行的大致过程。

我遇到最多的问题是这个：
核心转储：核心转储解决