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

阿新 • • 發佈：2020-03-04

轉載請註明出處，謝謝
原創作者：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 出錯

安裝步驟中 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 系列博