ORB-SLAM2 运行 —— ROS + Android 手机摄像头 _ORB-SLAM2

知识为进步之母,而进步又为富强之源泉。这篇文章主要讲述ORB-SLAM2 运行 —— ROS + Android 手机摄像头相关的知识，希望能为你提供帮助。
本文介绍了如何实现以 android 手机摄像头为输入，运行 ORB-SLAM2 ROS Mono。1. 介绍了 ROS 的配置安装和 ORB-SLAM2 ROS 的配置安装；2.介绍了如何实现Android 手机摄像头与 PC 进行基于 ROS 的通信；3. 进行手机摄像头标定；4. 使用 Android 手机摄像头，运行 ORB-SLAM2 ROS Mono；5. 简化启动，一个脚本运行多个终端。转载请注明出处，谢谢
原创作者：Mingrui
原创链接：https://www.cnblogs.com/MingruiYu/p/12404730.html
本文要点：

ROS 配置安装
- 解决 sudo rosdep init 报错 Website may be down.
ORB-SLAM2 ROS 配置安装
- 解决报错 DSO missing from command line
Android 手机摄像头与 PC 进行基于 ROS 的通信
手机摄像头标定
- 采集标定图像
- OpenCV samples 相机标定例程
使用 Android 手机摄像头，运行 ORB-SLAM2 ROS Mono
简化启动
- 使用 gnome-terminal，一个脚本运行多个终端

写在前面最近研究 ORB-SLAM2，自然是想能自己实时跑一跑。但最近因为疫情只能待在家里，身边能当摄像头的东西好像只有笔记本摄像头和手机摄像头。笔记本摄像头不方便（特别是我的 matebook 14 这个在键盘上的弹出摄像头，如想实现可参考），所以选择使用手机摄像头。ORB-SLAM2 官方提供了 ROS 的支持，再结合网上各路大佬提供的工具，最终实现了以 Android 手机摄像头为输入，基于 ROS 在 PC 上实时运行 ORB-SLAM2 Mono。本文将从零开始，介绍如何实现这一目标。
本文环境为：

Ubuntu 18.04
ROS Melodic
Android 手机（MI 9 SE）

ROS 配置安装首先是 ROS 的配置安装，参照 ROS 官方安装教程，其中第一步使用国内镜像：

sudo sh -c \'. /etc/lsb-release & & echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ $DISTRIB_CODENAME main" > /etc/apt/sources.list.d/ros-latest.list\'

sudo rosdep init 出错
安装步骤中 sudorosdep init 报错：

ERROR: cannot download default sources list from: https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/sources.list.d/20-default.list Website may be down.

首先试一试在浏览器中能不能打开，如果打不开的话，说明该网站需要翻。
因为在终端中安装，所以光浏览器能翻不够，还得配置终端翻。如果使用的是 ss 的话，终端还需要额外配置。配置方法可自行 google。
成功配置终端后，如果还报这个错，则需要：

sudo c_rehash /etc/ssl/certs sudo -E rosdep init

之后再 rosdep update 就可以了。
学习教程
ROS-Tutorials
ORB-SLAM2 ROS 配置安装编译ORB-SLAM2 的配置安装可见 raulmur/ORB_SLAM2。之前的博文 ORB-SLAM2 初体验 —— 配置安装中介绍了不包括 ROS 支持的 ORB-SLAM2 配置安装。包括 ROS 支持的配置安装可见 raulmur/ORB_SLAM2#7-ros-examples：
在 ~/.bashrc 中添加 ORB-SLAM2 path 至 ROS_PACKAGE_PATH

# 打开 ~/.bashrc sudo gedit ~/.bashrc# 添加 export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS # （注意修改 PATH 为自己 ORB-SLAM2 的目录）

NOTICE:

${ROS_PACKAGE_PATH}:和PATH之间不能有空格。
添加的位置要在之前添加的其它的 source 命令之后。

之后进行编译：

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

会报错：DSO missing from command line
解决方法：ERROR while running ./build_ros.sh #535
运行例如运行单目 ORB-SLAM2：

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

下文会详细介绍如何运行。
Android 手机摄像头与 PC 进行基于 ROS 的通信该实现基于 GitHub 上的一个项目：hitcm/Android_Camera-IMU，作者实现了将手机的摄像头信息和 IMU 信息传给 PC（可参考作者博文 ROS实时采集Android的图像和IMU数据）。本文中，我们只需使用摄像头信息。

git clone https://github.com/hitcm/Android_Camera-IMU.gitsudo apt-get install ros-melodic-imu-tools# 修改对应自己的 ROS 版本（本文中其实不需要）

将 clone 下来的文件夹中已经编译好的 apk 拷到 Android 手机上，在手机上安装。并将 PC 和 Android 手机置于同一局域网下。
运行方式：
PC Terminal 1: roscore
Android: 打开应用，在在 IP Port 中修改 IP 地址为 PC的 IP地址，port不需要修改（PC 的 IP 可在 PC 终端输入 ifconfig 查看），之后点击 Connect，连接成功则进入相机界面。
PC Terminal 2:

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。

手机摄像头标定为了 ORB-SLAM2 准确运行，需要对手机摄像头进行标定。标定方式为：对棋盘格标定板进行各个方向的拍照，之后基于 OpenCV 进行标定。注意这里采集的图片需要和 ORB-SLAM2 程序读取到的一致，所以不能直接使用手机自带相机 app 拍照，因为手机会自动通过算法进行校正，而上述通信传输的是 raw images。因此，首先我们需要完成的任务是：采集并保存摄像头图像。
使用下图作为标定板（参考资料），可直接在电脑屏幕上显示，对其拍照即可。

文章图片
注意：

实验发现，使用长宽格数不一样的棋盘标定板效果更好。
实验发现，标定板周围要是白色的才行，黑色的提取不出角点来（在电脑屏幕上显示标定板时尤其需要注意）。
摄像头需要从不同方向拍摄棋盘格，可参考 OpenCV 安装目录下 samples/data 中的 left0x.jpg 系列标定图片。

采集并保存图片目前没有找到直接保存的方法，所以我们选择写一个 ROS node 来接收手机传来的图像，再通过 OpenCV 进行显示和保存。
为了方便，我们选择直接在 ORB-SLAM2 的 ros_mono.cc 的代码基础上进行修改，在 ros_mono.cc 同一目录下写了个 ros_camera_capture.cc：

/** * This file is to capture images from Android phone, for camera calibration * This file is used with Android_Camera-IMU */#include< iostream> #include< algorithm> #include< fstream> #include< chrono> #include< ros/ros.h> #include < cv_bridge/cv_bridge.h> #include< opencv2/core/core.hpp> #include"../../../include/System.h"using namespace std; string save_dir = "PATH"; // 修改为自己保存图片的路径 int imgId = 0; void GrabImage(const sensor_msgs::ImageConstPtr& msg); int main(int argc, char **argv) {std::cout < < "To save the current frame, please press \'Q\' or \'q\' " < < std::endl; std::cout < < "The images will be saved to " < < save_dir < < std::endl; ros::init(argc, argv, "PClistener"); ros::start(); ros::NodeHandle nodeHandler; ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, GrabImage); ros::spin(); ros::shutdown(); return 0; }void GrabImage(const sensor_msgs::ImageConstPtr& msg) { string imgname; cv_bridge::CvImageConstPtr cv_ptr; try { cv_ptr = cv_bridge::toCvShare(msg); cv::Mat img = cv_ptr-> image; cv::imshow("img_name", img); char key = cv::waitKey(1); // press "q" to save the image if(key == \'q\' || key == \'Q\'){ imgId++; imgname = "img_" + to_string(imgId) + ".jpg"; cv::imwrite(save_dir + imgname, img); std::cout < < "has saved image "< < imgId < < " to " < < save_dir < < std::endl; } } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } }

注意修改其中保存图像的目录。
另外，在 ORB_SLAM2/Examples/ROS/ORB_SLAM2 目录中的 CMakeLists.txt 中添加如下内容（添加在 # Node for monocular camera 上方即可）：

# Node for capture images for camera calibration rosbuild_add_executable(CameraCapture src/ros_camera_capture.cc )target_link_libraries(CameraCapture ${LIBS} )

之后重新编译 ORB_SLAM2 项目。

cd PATH/ORB_SLAM2 ./build_ros.sh

使用方法：
Terminal 1:

roscore

手机进入 app 运行
Terminal 2: 在 Android_Camera-IMU 目录

roslaunch android_cam-imu.launch

（可以关掉 Rviz）
Terminal 3:

rosrun ORB_SLAM2 CameraCapture

鼠标选中图像框，按下 q 键保存图像。
进行标定使用 OpenCV samples 中的代码实现。参考资料
标定例程
新建一个工作目录（文件夹）camera_calibration_opencv，将 OpenCV 安装目录中的 samples/cpp/tutorial_code/calib3d/camera_calibration 文件夹内的内容拷贝至该目录。
修改 VID5.xml
VID5.xml 中存储着标定图像的路径，所以要在 VID.xml 中添加所有标定图像的路径，eg：

< ?xml version="1.0"?> < opencv_storage> < images> PATH/img_1.jpg PATH/img_2.jpg PATH/img_3.jpg < /images> < /opencv_storage>

修改 in_VID5.xml

< BoardSize_Width> 9< /BoardSize_Width> < BoardSize_Height> 6< /BoardSize_Height>

表示棋盘格的宽和高，注意，这里的宽度和高度是指内部交叉点的个数，而不是方形格的个数。如上图棋盘的数据就是9和6。

< Square_Size> 20< /Square_Size>

修改为每格的边长 (mm)，拿尺子量。

< Input> "VID5.xml"< /Input>

修改 VID5.xml 的路径。

< Calibrate_FixPrincipalPointAtTheCenter> 1 < /Calibrate_FixPrincipalPointAtTheCenter>

此处原来是0，需要改为1，表示引入切向畸变参数（因为 ORB-SLAM2 中也引入了切向畸变参数），否则只有径向畸变参数。
其它地方应该不需要改动，想进一步了解可看其中的注释。
编译
在工作目录 camera_calibration_opencv 中新建 CMakeLists.txt:

project(Camera_Calibration) set(CMAKE_CXX_STANDARD 11)find_package(OpenCV 3.0 QUIET) if(NOT OpenCV_FOUND) find_package(OpenCV 2.4.3 QUIET) if(NOT OpenCV_FOUND) message(FATAL_ERROR "OpenCV > 2.4.3 not found.") endif() endif()include_directories(${OpenCV_INCLUDE_DIR}) add_executable(Camera_Calibration camera_calibration.cpp) target_link_libraries(Camera_Calibration ${OpenCV_LIBS})

之后编译：

cd camera_calibration_opencv mkdir build cd build cmake .. make

运行，标定

cd camera_calibration_opencv ./build/Camera_Calibration in_VID5.xml

程序启动后会显示标定图像的角点提取情况，之后会显示校正后图像，一个一个全部关闭后才会保存标定参数至 out_camera_data.xml。
参数填入 ORB-SLAM2 的配置文件
参数输出在 out_camera_data.xml 中:

< camera_matrix type_id="opencv-matrix"> 是相机内参矩阵，顺序为 fx, 0, cx; 0, fy, cy; 0, 0, 1。
< distortion_coefficients type_id="opencv-matrix"> 是畸变参数，其顺序为 k1, k2, p1, p2, k3。

之后在 ORB_SLAM2 中新建一个配置文件 AndroidPhone.yaml（建哪儿都行，我为了方便就和 TUM1.yaml 放在了一个目录下），将 TUM1.yaml 的内容拷贝过来，并把其中的 Camera 参数进行修改。
注意：相机参数对 ORB-SLAM2 的运行效果有极大影响（尤其是初始化），所以标定过程须认真完成。
运行 ORB-SLAM2 MonoTerminal 1:

roscore

手机进入 app 运行
Terminal 2: 在 Android_Camera-IMU 目录

roslaunch android_cam-imu.launch

（可以关掉 Rviz）
Terminal 3:

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

运行效果展示：

文章图片
注意： ORB-SLAM2 Mono 还是比较难以初始化的（其设置的初始化条件相对苛刻），在开始时，选择特征纹理丰富的区域，多上下左右平移相机，有利于初始化。
简化启动上述启动步骤需要启动3个终端，挺麻烦的，所以可以选择写一个脚本来自动启动这3个终端。参考资料
新建 ORB_SLAM2_with_AndroidPhone.sh，在其中填入：

gnome-terminal --title="roscore" -x bash -c "roscore" # 暂停 2s，保证几个不同终端的启动顺序 sleep 2s; gnome-terminal --title="AndroidPhone" -x bash -c "cd PATH/Android_Camera-IMU; roslaunch android_cam-imu.launch" sleep 2s; gnome-terminal --title="ORB-SLAM2" -x bash -c "rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE"

之后赋予权限（仅需一次）：

chmod +x ORB_SLAM2_with_AndroidPhone.sh

运行：

./ORB_SLAM2_with_AndroidPhone.sh

即可一次性打开3个终端，并运行相关命令。之后手机再打开 app 就可以了。
注意：此时终端运行结束后会自动退出，如果不想自动退出，可在terminal点右键，选择Profiles-> Profile Preferences然后找到Title and Command，里面有一项When command exits，后面选择为Hold the terminal open。参考资料
参考资料