【记录】ORB_SLAM2 例程（README文件里的单双目RDB-D、ROS、AR、外接摄像头、点云）

建议结合另一篇一起看：【收获】第一次跑 ORB_SLAM2 例程的收获

需要仔细读一读官方提供的README，有很多有用的信息

一、普通环境

0.build.sh 遇到的问题

（1）usleep

https://zhuanlan.zhihu.com/p/411027681

解决方案：在报错的文件里添加 #include
我的是这两个：

/home/qwe/ROS_WS/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc

/home/qwe/ROS_WS/src/ORB_SLAM2/src/LoopClosing.cc

（2）AlignedBit

https://blog.csdn.net/chenzz444/article/details/121406471

查看eigen版本

pkg-config --modversion eigen3

解决方案：重装eigen 3.2.1

sudo rm -rf /usr/include/eigen3

# 进入到下载完的eigen3.2.1目录下执行
mkdir build
cd build
sudo make install

# 进入到原来的Pangolin-0.6目录下执行
sudo rm -rf ./build
mkdir build
cd build
cmake ..
sudo make install
# 这样Pangolin也重新编译好了

1.单目、TUM数据集

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER
其中PATH_TO_SEQUENCE_FOLDER为数据集的存储路径，并将tumx.yaml与下载的数据集对应，比如TUM1.yaml,TUM2.yaml和TUM3.yaml 分别对应 freiburg1, freiburg2 和 freiburg3。

这句话是说，你需要改的部分是TUMX.yaml的X以及最后的数据集路径。
X与后面的freiburgX对应。

例如：

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml ./Examples/data/rgbd_dataset_freiburg1_desk

2.双目、KITTI数据集

./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt Examples/Stereo/KITTI00-02.yaml ./Examples/data/00

3.RGB-D、TUM数据集

https://blog.csdn.net/xyt723916/article/details/89374201

这里官方提供了关联文件，所以不用自己再关联一遍

  ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml ./Examples/data/rgbd_dataset_freiburg1_desk ./Examples/RGB-D/associations/fr1_desk.txt

二、ROS

ORB_SLAM2 ROS运行KITTI数据集
https://blog.csdn.net/learning_tortosie/article/details/79881165

1.使用ROS创建workspace

mkdir -p ~/catkin_orb_slam2_ws/src
cd ~/catkin_orb_slam2_ws/
catkin_make

2.将ORB-SLAM2项目移动到catkin_orb_slam2_ws/src

3.将包含Examples/ROS/ORB_SLAM2的路径添加到ROS_PACKAGE_PATH环境变量中。

sudo gedit ~/.bashrc

打开.bashrc文件并在最后添加以下行(注意改路径):

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:路径/ORB_SLAM2/Examples/ROS
source ~/catkin_orb_slam2_ws/devel/setup.bash

使生效

source ~/.bashrc

4.编译ROS下的ORB-SLAM2

cd ORB_SLAM2
chmod +x build_ros.sh
./build_ros.sh

建议先./build.sh
再./build_ros.sh

0.build_ros.sh 遇到的问题

（1）rospack found package “ORB_SLAM2” at “”, but the current directory is “xxxx”

报以下错误：大概是软链接的问题
我当时的解决方案是重建了文件夹，然后重新clone了一遍SLAM
现在我觉得是source的问题，bashrc里面配置的路径没有生效

source ~/catkin_orb_slam2_ws/devel/setup.bash
source ~/.bashrc

（2）the rosdep view is empty

按图中提示，运行

sudo rosdep init
rosdep update

（3）…/…/config.h：没有那个文件或目录

g2o没有编译好，不建议用下面的方法，能解决当下的问题，但是还会有别的问题
建议先./build.sh，再./build_ros.sh

https://blog.csdn.net/yu_2054/article/details/88700874

echo "Configuring and building Thirdparty/DBoW2 ..."

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

cd ../../g2o

echo "Configuring and building Thirdparty/g2o ..."

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

cd ../../../

echo "Building ROS nodes"

cd Examples/ROS/ORB_SLAM2
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j

（4）CMake Error:The current CMakeCache.txt directory xxxx is different than the directory xxx

删除如下build文件

（5）recipe for target ‘CMakeFiles/xxx.dir/all’ failed

make[2]: *** 没有规则可制作目标“../../../../lib/libORB_SLAM2.so”，由“../RGBD” 需求。 停止。
make[2]: *** 正在等待未完成的任务....
[ 11%] Building CXX object CMakeFiles/RGBD.dir/src/ros_rgbd.cc.o
make[2]: *** 没有规则可制作目标“../../../../lib/libORB_SLAM2.so”，由“../Mono” 需求。 停止。
make[2]: *** 没有规则可制作目标“../../../../lib/libORB_SLAM2.so”，由“../Stereo” 需求。 停止。
make[2]: *** 正在等待未完成的任务....
make[2]: *** 正在等待未完成的任务....
[ 33%] Building CXX object CMakeFiles/Mono.dir/src/ros_mono.cc.o
[ 33%] Building CXX object CMakeFiles/Stereo.dir/src/ros_stereo.cc.o
make[2]: *** 没有规则可制作目标“../../../../lib/libORB_SLAM2.so”，由“../MonoAR” 需求。 停止。
make[2]: *** 正在等待未完成的任务....
[ 44%] Building CXX object CMakeFiles/MonoAR.dir/src/AR/ros_mono_ar.cc.o
[ 55%] Building CXX object CMakeFiles/MonoAR.dir/src/AR/ViewerAR.cc.o
CMakeFiles/Makefile2:397: recipe for target 'CMakeFiles/Mono.dir/all' failed
make[1]: *** [CMakeFiles/Mono.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
CMakeFiles/Makefile2:594: recipe for target 'CMakeFiles/MonoAR.dir/all' failed
make[1]: *** [CMakeFiles/MonoAR.dir/all] Error 2
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/RGBD.dir/all' failed
make[1]: *** [CMakeFiles/RGBD.dir/all] Error 2
CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/Stereo.dir/all' failed
make[1]: *** [CMakeFiles/Stereo.dir/all] Error 2
Makefile:129: recipe for target 'all' failed
make: *** [all] Error 2

网上的解决分方法都是在Examples/ROS/ORB_SLAM2/文件夹下的CMakeLists.txt文件加-lboost_system，然而我加了却解决不了，这里卡了很长时间：
https://blog.csdn.net/YiYeZhiNian/article/details/108270337

报错的意思是libORB_SLAM2.so文件找不到，所以需要这个文件的那些都失败了
解决方法：在ORB_SLAM2文件夹下直接运行编译命令，这时候会生成一个lib文件夹，libORB_SLAM2.so就在这个文件夹下。

与问题（3）中的建议相同，不建议单个编译，而是先./build.sh再./build_ros.sh，一劳永逸。

最后结论是，只要先./build.sh
再./build_ros.sh 就解决了大部分问题。

1.双目、EuRoC 数据集对应的 rosbag

ORB SLAM 2 demo 复现（普通模式 + ROS 模式）

EuRoC 数据集对应的 rosbag，包含如下的 topic：

/cam0/image_raw    3682 msgs    : sensor_msgs/Image         
/cam1/image_raw    3682 msgs    : sensor_msgs/Image          /imu0    
36820 msgs    : sensor_msgs/Imu            /leica/position    3099
msgs    : geometry_msgs/PointStamped 

其中关键的 topic 是左右两个摄像头的数据
/cam0/image_raw 和 /cam1/image_raw

而双目 rosnode Stereo 接收的 topic 分别为
/camera/left/image_raw 和 /camera/right/image_raw
因此在运行时需要转换一下 topic 名称：/cam0/image_raw:=/camera/left/image_raw。

完整命令如下：

roscore
rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true
rosbag play --pause MH_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw

(1)如何知道数据集的topic？

roscore
rosbag info xxx.bag

(2)如何知道rosnode Stereo 接收的 topic ？

2.单目、AR

ORB SLAM 2 demo 复现（普通模式 + ROS 模式）
实现增强现实效果，可以向摄像头中的世界插入一个虚拟立方体，通过摄像头观察，这个虚拟立方体与实际物体相仿。

roscore
rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml 
rosbag play datasets/rgbd_dataset_freiburg1_xyz.bag /camera/rgb/image_color:=/camera/image_raw

这里是跑数据集，然后放置虚拟立方体，就不运行这个了，我在下下部分实现连接手机摄像头，实时放置

3.笔记本摄像头、AR

[ROS] 安装 USB Camera 驱动并调用

ROS中使用摄像头的问题

如果出现报错“sh: 1: v4l2-ctl: not found”则需要安装v4l2
运行：sudo apt-get install v4l-utils

以下括号里的是我当时遇到的情况

1.在ROS工作区src目录下（1.在roscore启动时不能gitclone，2.把后面的.git去掉才行）

git clone https://github.com/bosch-ros-pkg/usb_cam.git

2.退出 src 目录运行 catkin_make 并配置运行环境

cd ..
catkin_make
source devel/setup.bash

（source之后，可以在任意目录使用roslaunch usb_cam usb_cam.launch，不然需要cd到launch目录）
3.使用自定义 launch 文件设置摄像头
（我下载的里边有usb_cam.launch和usb_cam-test.launch）
（usb_cam-test.launch是usb_cam.launch的链接文件，usb_cam.launch被改变的话usb_cam-test.launch也会改变）
（我下载后的文件和以下内容不太一样）

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

4.运行
（我用rosrun usb_cam usb_cam_node运行，不出现画面那个弹窗，但是摄像头是开启状态，也可以跑monoAR（monoAR自已有弹窗））

roslaunch usb_cam usb_cam.launch

此时摄像头开启

5.编译ROS下的ORB-SLAM2

ROS(三)：使用ROS跑通ORB_SLAM2

ORB-SLAM默认订阅的话题为/camera/image_raw，而usb_cam节点发布的话题为/usb_cam/image_raw，因此需要在ros_mono_ar.cc中修改订阅的话题。

文件路径：/home/qwe/ROS_WS/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc

ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 
//根据rostopic,应该改成下面的代码 
ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb); 

./build_ros.sh

roslaunch usb_cam usb_cam-test.launch
rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

刚开始的初始化好难，找不到规律，有时候一直不初始化，有时候晃一晃就成功了，太奇怪了。

因为电脑太重了，搬着对着桌子实在太累，所以下一部分，连接手机摄像头，再试试实现monoAR

试完回来发现，初始化并不是很难，难是因为有问题，我之后成功的几次都是几十秒就ok的，猜测一下这里的问题：应该是我笔记本摄像头的分辨率太低了，硬件不给力

另外，在开始时，选择特征纹理丰富的区域，多上下左右平移相机，有利于初始化。

4.连接手机摄像头、AR

1.连接手机摄像头

Android_Camera-IMU原作者的：ROS实时采集Android的图像和IMU数据
这个写的很详细，强烈推荐看一看，我的就略写了

git clone https://github.com/hitcm/Android_Camera-IMU.git
sudo apt-get install ros-melodic-imu-tools  # 按实际安装indigo、Kinetic 等安装对应的工具包
apk安装到手机
ip a 
查看电脑ip，填到app首页locolhost，点击connect

roscore
cd Android_Camera-IMU      
roslaunch android_cam-imu.launch

会弹出一个Rviz界面，如果要实时显示则：
如果要实时显示 image，需要 Add - By topic -添加/camera/image_raw/image。
如果要显示 imu，则需要 Add - By topic - 添加 imu，且在 Fix Frame 中 将 map 改为 imu。

2.标定之前在slam14讲里做过，这里省略
(建议不想做这一步的先跳过这一步，我标定完反而不能运行demo（貌似是因为畸变参数有问题）)
2.1采集并保存摄像头图像
2.1.1在 ros_mono.cc 同一目录下写了个 ros_camera_capture.cc
2.1.2在 ORB_SLAM2/Examples/ROS/ORB_SLAM2 目录中的 CMakeLists.txt 中添加如下内容
2.1.3编译运行，按q保存图片

./build_ros.sh
roscore
roslaunch android_cam-imu.launch（可以关掉 Rviz）
rosrun ORB_SLAM2 CameraCapture

2.2标定
2.2.1修改 VID5.xml 图片路径
2.2.2修改 in_VID5.xml 配置文件
2.2.3编译运行

mkdir build
cd build
cmake ..
make
./Camera_Calibration ../in_VID5.xml

2.3标定结果：

是相机内参矩阵，顺序为 fx, 0, cx; 0, fy, cy; 0, 0, 1。
是畸变参数，其顺序为 k1, k2, p1, p2, k3。

3.改yaml文件，自己新建一个mycam.yaml，复制TUM1.yaml的内容，并修改相机参数和畸变参数。

4.改 topic，手机摄像头的是/android/image_raw/compressed（不用改）

在运行时，一个关键的设置是将 slam node 接收的 ros topic 和 rosbag 发布的 ros topic
匹配起来，也就是收、发双方的 ros topic 名字必须相同.

topic 名字的转化既可以在运行 slam node 时设置，格式为
rosrun original_topic:=new_topic
也可以在play rosbag 的时候设置，格式为
rosbag play original_topic:=new_topic
https://www.jianshu.com/p/31c95d9a5f97

之前用电脑摄像头的时候是在文件里改的，我这次在文件里改，不知道为啥，没有弹窗出来，运行卡住了，所以采用了上面这种方法，使用

roslaunch android_cam-imu.launch /android/image_raw/compressed:=/camera/image_raw

后来试了试直接这样也行（注意这里我已经把ros_mono_ar.cc里的topic改回去了，没改过的就不用管）

roslaunch android_cam-imu.launch

仔细想了想，发现手机摄像头也有/camera/image_raw这个topic，所以不用改

5.运行demo，注意改最后的yaml文件

rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/mycam.yaml

心好累，标定了两次，以为是我标定的有问题，但是两次都不能正确初始化（SLAM NOT INITIALIZED），然后就试了试不用自己的标定，一下子就成功了 ORZ

自己标定矫正后的图太畸形了，猜测是畸变系数有问题，所以把畸变系数都置0，也能正确运行出结果

Asus是自带的内参文件，需要修改，为什么只改相机参数，不改畸变参数呢？

所以总结一下这部分，虽然我写的长（说明我当时很懵），但其实归根到底，只用下面这些，就可以跑出来效果

git clone https://github.com/hitcm/Android_Camera-IMU.git
sudo apt-get install ros-melodic-imu-tools  # 按实际安装indigo、Kinetic 等安装对应的工具包
apk安装到手机
ip a # 查看电脑ip，填到app首页locolhost，点击connect

roscore #终端 1
roslaunch android_cam-imu.launch # 终端 2 ，gitclone的目录，运行后有 Rviz 界面
rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml # 终端3，ORB_SLAM2

三、点云

最后再跑一跑高博修改的ORB_SLAM2，增加了点云输出

压缩包里的没跑出来，g2o的问题

之后参照这个跑了ORB_SLAM2_modified
高翔ORB-SLAM2稠密建图编译