【ORB-SLAM3】使用安卓手机的摄像头运行ORB3

1.前言

最近在学习ORBSLAM2/3，找到了一个安卓手机跑ORB2的帖子，我照着帖子成功运行了安卓手机跑ORB3，本帖来记录一下里面的细节。

参考帖子：ORB-SLAM2 运行 —— ROS + Android 手机摄像头

本文环境：

• Ubuntu20.04
• ROSNoetic
• Android手机（红米note9 pro）

2.ros noetic安装

当你使用较新的电脑时，安装老版本的ubuntu会出现各种硬件驱动不匹配的问题，所以我ubuntu直接安装了20.04版本，对应的ros版本是noetic。

安装参考：

官网（https://wiki.ros.org/noetic/Installation/Ubuntu）

错误：

sudo rosdep init连接不上

解决：

# 1.pip安装rosdepc
sudo pip install rosdepc
# 如果显示没有pip可以试试pip3
sudo pip3 install rosdepc
# 如果pip3还没有
sudo apt-get install python3-pip 
sudo pip install rosdepc

# 2.用rosdepc替换原来的rosdep安装
sudo rosdepc init
rosdepc update

3.ORB-SLAM3安装

本来是想先跑ORB2的，ORB2支持opencv2/3，但ROS-Noetic安装默认安装了opencv4，而且cv_bridge默认绑定了opencv4，导致直接编译ORB2会出现opencv版本不匹配的问题，我尝试安装Opencv3，且卸载ros的cv_bridge，源码编译一个链接opencv3的cv_bridge，结果编译下来还是去找opencv4，捣鼓一天也没有解决，只好去安装ORB3了，ORB3支持opencv3/4，安装过程比较顺利。下面的安装步骤：

（1）安装依赖

需要 Pangolinv0.5版本、opencv3/4版本、Eigen3、g2o、ROS

（2）下载编译

git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
cd ORB_SLAM3
# 修改ORB_SLAM3/CMakeLists.txt
# 把opencv版本改为自己的版本，如 find_package(OpenCV 4 QUIET)
chmod +x build.sh
./build.sh

Q1：

CMake Error at CMakeLists.txt:91 (ADD_LIBRARY): Cannot find source
file:
g2o/stuff/timeutil.cpp

A1：

如果是直接复制XTDrone中的ORB3源码，会报这个错误，因为它在/Thirdparty/g2o/g2o/stuff/缺少了文件timeutil.h、timeutil.cpp。需要自己到官网下载相应文件。

Q2：

ORB SLAM2 找不到 libDBoW2.so / libg2o.so

A2：

XTDrone中的ORB3源码的ORB_SLAM3/Thirdparty/DBoW2缺少lib文件夹，但我在现在的ORB官网也没有找到相应的lib文件夹，有一个教程是如何自己编译缺少的库文件。不过我是直接在别人安装好的电脑上找打了缺少的库文件解决的

（3）编译到ROS中

# 路径需要自己修改
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/catkin_ws/src/ORB_SLAM3/Examples/ROS 
# 修改ORB_SLAM3/Examples/ROS/ORB_SLAM3/CMakeLists.txt
# 把opencv版本改为自己的版本，注意要和前面修改的CMakeLists.txt中的opencv版本一致
# 如 find_package(OpenCV 4 QUIET)
# -----------------
chmod +x build_ros.sh
./build_ros.sh

（4）运行ORB3

ORB3中支持运行单目、双目、深度以及结合IMU，由于手机摄像头是单目，所以说一下ORB3运行单目的命令：

rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

PATH_TO_VOCABULARY是离线预训练的词袋模型路径，在源文件的ORB_SLAM3/Vocabulary下，是一个txt文件；

PATH_TO_SETTINGS_FILE是摄像头配置文件，里面记录了单目相机的相机内参，相机内参我们后面需要重新标定计算，ORB提供了参考的配置文件，路径在ORB_SLAM3/Examples/Monocular/TUM1.yaml

4.Android 手机摄像头与 PC 进行基于 ROS 的通信

我们ORB3是跑在PC的ROS上的，而相机传感器是在手机上，因此我们需要建立手机摄像头图片和PC端ROS的通信，将手机摄像头图片以ROS话题形式发布出去，这里基于 GitHub 上的一个项目：hitcm/Android_Camera-IMU，作者实现了将手机的摄像头信息和 IMU 信息传给 PC（可参考作者博文 ROS实时采集Android的图像和IMU数据）。本文中，我们只需使用摄像头信息。

（1）安装

# 电脑PC端
git clone https://github.com/hitcm/Android_Camera-IMU.git
sudo apt-get install ros-noetic-imu-tools  # 修改对应自己的 ROS 版本
# 安卓手机端
# 将git下载的源文件中的apk安装包发给手机并安装

（2）运行

# 1.PC和手机处于同一个网段，例如PC和手机连同一个wifi，然后查看PC端ip
ifconfig   # 记录一下PC端的ip地址

# 2.运行roscore
roscore

# 3.手机上运行安装好的软件，在 IP Port 中修改 IP 地址为 PC的 IP地址，port不需要修改，之后点击 Connect，连接成功则进入相机界面。
# 例如 http://192.168.31.192:11311/

# 4.运行roslaunch文件，复制出一个跑orb3需要的摄像头话题
cd pathToAndroid_Camera-IMU  # 就是cd到刚才git下载的源文件
roslaunch android_cam-imu.launch
# 没报错的话，会弹出一个rviz，直接关掉就可以了

# 5.查看图片话题
rqt_image_view   # 如果成功看到有图形，那么就成功了

5.手机摄像头标定

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

1.采集摄像头标定图片

（1）打印标定板

打印下面的标定板图片

（2）编写采集图片的代码

目前没有找到直接保存的方法，所以我们选择写一个 ROS node 来接收手机传来的图像，再通过 OpenCV 进行显示和保存。

为了方便，我们选择直接在 ORB-SLAM3的 ORB_SLAM3/Examples/ROS/ORB_SLAM3/srcros_mono.cc 的代码基础上进行修改，在 ros_mono.cc 同一目录下写了个 ros_camera_capture.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 = "~/"; // 修改为自己保存图片的路径，注意路径末尾带上/
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;
    }
}

（3）编译代码

在 ORB_SLAM3/Examples/ROS/ORB_SLAM3 目录中的 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_SLAM3项目

cd PATH/ORB_SLAM3
./build_ros.sh

（4）运行代码采集图片

# 终端1
roscore

# 手机
运行应用软件并连接

# 终端2
cd pathToAndroid_Camera-IMU  # 就是cd到刚才git下载的源文件
roslaunch android_cam-imu.launch
# 可以关掉rviz

# 终端3
rosrun ORB_SLAM3 CameraCapture
# 会显示手机摄像头图片，按下 q 键保存图像

注意拍摄时多角度拍标定板，由于图像比较模糊，所以拍的时候离近一些

2.标定相机

相机标定程序我们使用opencv实例程序里面的。将 OpenCV 安装目录中的 samples/cpp/tutorial_code/calib3d/camera_calibration 文件夹复制一份到自定义路径下，在里面新建一个子文件夹img，把前面保存的图片都放到img文件夹中，文件结构为：

camera_calibration_opencv

├── camera_calibration.cpp
├── CMakeLists.txt
├── img
│ ├── img_10.jpg
│ ├── img_11.jpg
│ ├── img_1.jpg
│ ├── img_2.jpg
│ ├── img_3.jpg
│ ├── img_4.jpg
│ ├── img_5.jpg
│ ├── img_6.jpg
│ ├── img_7.jpg
│ ├── img_8.jpg
│ └── img_9.jpg
├── in_VID5.xml
├── out_camera_data.yml
└── VID5.xml

（1）修改 VID5.xml

VID5.xml 中存储着标定图像的路径，所以要在 VID.xml 中添加所有标定图像的路径，例如：

<?xml version="1.0"?>
<opencv_storage>
<images>
../img/img_1.jpg
../img/img_2.jpg
../img/img_3.jpg
../img/img_4.jpg
../img/img_5.jpg
../img/img_6.jpg
../img/img_7.jpg
../img/img_8.jpg
../img/img_9.jpg
../img/img_10.jpg
../img/img_11.jpg
</images>
</opencv_storage>

注意，里面图像的路径必须得是相对路径，不能是绝对路径，否则会报错。

（2）修改 in_VID5.xml

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

# 修改为每格的边长 (mm)，拿尺子量。
<Square_Size>19</Square_Size>

# 修改 VID5.xml 的路径，这里路径是相对路径
<Input>"../VID5.xml"</Input>

# 此处原来是0，需要改为1，表示引入切向畸变参数（因为 ORB-SLAM2 中也引入了切向畸变参数），否则只有径向畸变参数
<Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>

（3）编译项目

在工作目录 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

（4）运行程序进行标定

cd camera_calibration_opencv/build
./Camera_Calibration ../in_VID5.xml

前面在修改VID5.xml和 in_VID5.xml时说了得用相对路径而非绝对路径，否则会出现错误：
Input does not exist: VID5.xmlInvalid input detected

那么用相对路径就需要知道当前路径是什么，当前路径就是你终端运行可执行文件Camera_Calibration的路径，例如：

# 运行方法一:
cd camera_calibration_opencv/build
camera_calibration_opencv/build$./Camera_Calibration ../in_VID5.xml
# 终端当前路径就是$号前面的路径，也就是camera_calibration_opencv/build，所以in_VID5.xml的相对路径就是../in_VID5.xml

# 运行方法二:
cd camera_calibration_opencv
camera_calibration_opencv$./build/Camera_Calibration in_VID5.xml
# 终端当前路径就是camera_calibration_opencv，所以in_VID5.xml的相对路径就是in_VID5.xml

成功运行之后，会依次显示标定图片，并在终端里面输出重投影误差，误差越小越好，我这里是0.3，如果太大就重新拍照。

Re-projection error reported by calibrateCamera: 0.299492
Calibration succeeded. avg re projection error = 0.299491

（5）填写ORB-SLAM3的配置文件SETTINGS_FILE

当你成功运行标定程序后，会在同目录下生成相机内参结果文件camera_calibration_opencv/build/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_SLAM3/Examples/Monocular路径下复制TUM1.yaml，命名为AndroidPhone.yaml，填写里面的相机内参：

Camera1.fx: 454.608394
Camera1.fy: 454.608394
Camera1.cx: 319.500000
Camera1.cy: 239.500000

Camera1.k1: 0.180868
Camera1.k2: -1.037955
Camera1.p1: 0.00
Camera1.p2: 0.00
Camera1.k3: 2.046420

6.运行ORB-SLAM3

# 1.
roscore

# 2.
手机运行应用程序并连接

# 3.
cd pathToAndroid_Camera-IMU  
roslaunch android_cam-imu.launch # 可以关掉rviz

# 4.
rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
# PATH_TO_VOCABULARY就是ORB_SLAM3/Vocabulary/ORBvoc.txt的绝对路径
# PATH_TO_SETTINGS_FILE就是AndroidPhone.yaml的绝对路径

运行效果展示：

注意： ORB-SLAM3 Mono 还是比较难以初始化的（其设置的初始化条件相对苛刻），在开始时，选择特征纹理丰富的区域，不要纯旋转，多上下左右平移相机，有利于初始化。