ORB-SLAM2 運行 —— ROS + Android 手機攝像頭

轉載請註明出處，謝謝
原創做者：Mingrui
原創連接：http://www.javashuo.com/article/p-rjvhykbc-bo.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 出錯

安裝步驟中 sudo rosdep 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.git

sudo 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
./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 Mono

Terminal 1:

roscore

手機進入 app 運行

Terminal 2: 在 Android_Camera-IMU 目錄

roslaunch android_cam-imu.launch

（能夠關掉 rvis）

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。參考資料

參考資料

• 在Ubuntu 18.04 LTS安裝ROS Melodic
• sudo rosdep init出錯的解決方案
• sudo rosdep init 錯誤
• ERROR while running ./build_ros.sh #535
• ubuntu16.04下用筆記本攝像頭和ROS編譯運行ORB_SLAM2的單目AR例程
• ROS實時採集Android的圖像和IMU數據
• OpenCV 相機參數標定（Camera Calibration
• 採集Android手機攝像頭運行ORB_SLAM2（ubuntu16.04+ROS kinetic
• ubuntu下經過命令打開多個終端並在相應終端執指令
• gnome-terminal

ORB-SLAM2 系列博文

ORB-SLAM2 系列博文