Ubuntu18.04+Android手机IMU+ROS Melodic跑ORB-SLAM2

前言

这篇博客的形成与清华大佬的博客密不可分，在此公开https://www.cnblogs.com/MingruiYu/p/12404730.html

一、ROS Melodic在ubuntu系统18.04版本上的安装

请移步我之前的博客ROS Melodic在ubuntu系统18.04版本上的安装（完美避坑）

二、基于ROS，ORB_SLAM2的安装、配置、运行SLAM单目实例

1、前期SLAM环境配置

请移步我之前的博客Ubuntu18.04搭建 SLAM环境（完美避坑，版本对应不报错）

2、ROS下安装ORB_SLAM2

ROS环境下运行ORB_SLAM，最好放在工作区的src文件夹下（工作区目录：catkin_ws/src）
进入ROS工作区的src文件夹。

cd ~/catkin_ws/src/

下载安装ORB_SLAM2

git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2

进入ORB_SLAM2文件夹。

cd ORB_SLAM2

给build.sh文件权限。

chmod +x build.sh

编译build.sh文件

./build.sh

这里可能报出错误
问题1：
ORB_SLAM2/src/System.cc: error: ‘usleep’ was not declared in this scope usleep(5000);
解决方案：
找到对应的System.cc文件的首部加入 头文件

#include

根据实际情况，提示哪个文件usleep有问题，就去加这个头文件。
需要增加unistd.h的文件还有：
Examples/Monocular/mono_euroc.cc
Examples/Monocular/mono_kitti.cc
Examples/Monocular/mono_tum.cc
Examples/RGB-D/rgbd_tum.cc
Examples/Stereo/stereo_euroc.cc
Examples/Stereo/stereo_kitti.cc
src/LocalMapping.cc
src/LoopClosing.cc
src/System.cc
src/Tracking.cc
src/Viewer.cc
除此之外如果还有自行修改
参考链接https://blog.csdn.net/qq_15698613/article/details/98453592
因为需要在ROS环境下运行ORB_SLAM2所以要执行下面命令

chmod +x build_ros.sh

编辑bash文件

sudo  gedit ~/.bashrc

将包含Examples/ROS/ORB_SLAM2的路径添加到ROS_PACKAGE_PATH环境变量中。打开.bashrc文件并在最后添加的内容如图所示

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:（PATH）/catkin_ws/src/ORB_SLAM2/Examples/ROS

./build_ros.sh

编译的时候可能报错
问题1：

这个问题发生的原因是，Pangolin的版本过高，所以需要降低版本到0.5的。
网盘Pangolin
https://pan.baidu.com/s/1yEQvArzHz35CUJc-BEmNiw
提取码: n6r8
参考链接https://blog.csdn.net/qq_33950926/article/details/121129028
问题2：
CMakeFiles/RGBD.dir/build.make:197: recipe for target ‘…/RGBD’ failed

解决方案：
修改home/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/文件夹下的CMakeLists.txt文件。
在set（LIBS xxxxx 的后面加上下列这一句代码。

-lboost_system

重新编译即可
参考链接https://blog.csdn.net/weixin_44436677/article/details/105587986
至此，ORB_SLAM2已经安装好。

3、运行单目SLAM实例

编译完成后会在ORB_SLAM2/Examples文件夹下生成各种可执行文件。这里以单目情况为例，展示如何运行ORB_SLAM2程序。

（1）下载数据集

有TUM、KITTI、EuRoC三种数据集，这儿使用TUM数据集，从http://vision.in.tum.de/data/datasets/rgbd-dataset/download下载序列并解压缩。我下载的是第一个的，下载下来的文件名是：rgbd-dataset_freiburg_xyz。

（2）编译

官方给出的命令格式如下：
PATH_TO_SEQUENCE_FOLDER为数据集的存储路径。
tumx.yaml与数据集对应，比如TUM1.yaml、TUM2.yaml 、TUM3.yaml 分别对应 freiburg1、freiburg2 、 freiburg3。

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txtExamples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER

根据自己保存路径不一样适当修改,我的在主目录

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml /home/wei/rgbd_dataset_freiburg1_xyz

（3）结果

出现特征点提取图就可以了，我这里忘了截图，网络图如下

证明配置正确。

三、Android 手机摄像头与 PC建立通信传输

1、Android 工具下载

https://github.com/hitcm/Android_Camera-IMU

git clone https://github.com/hitcm/Android_Camera-IMU.git
sudo apt-get install ros-melodic-imu-tools  # 修改对应自己的 ROS 版本

一定要git完整文件夹，后面要用到。将 clone 下来的文件夹中已经编译好的 apk 拷到 Android 手机上，在手机上安装。

2、连接热点将 PC 和 Android 手机 置于同一局域网下

运行方式：
终端1: roscore
Android手机:输入自己电脑ip（PC 的 IP 可在 PC 终端输入 ifconfig 查看），之后点击 Connect，连接成功则进入相机界面。
终端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。
参考链接 https://www.cnblogs.com/MingruiYu/p/12404730.html

四、Android 手机摄像头相机参数标定

标定板

（1）采集并保存图片

这里参考清华大佬写的一个自动采集保存代码，直接在 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
#include
#include
#include

#include
#include 

#include

#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

打开终端1:
roscore
手机进入 app 运行摄像头
打开终端2: 在 Android_Camera-IMU 目录

roslaunch android_cam-imu.launch

（可以关掉 Rviz）
打开终端3:

rosrun ORB_SLAM2 CameraCapture

鼠标选中电脑里对应手机拍摄的图像框，按下 q 键保存图像。

（2）进行标定新建一个工作目录

主目录（文件夹）camera_calibration_opencv，
将 OpenCV 安装目录中的 samples/cpp/tutorial_code/calib3d/camera_calibration 文件夹内的内容拷贝至该目录。
修改 VID5.xml
VID5.xml 中存储着标定图像的路径，所以要在 VID.xml 中添加所有标定图像的路径（上一步）

PATH/img_1.jpg
PATH/img_2.jpg
PATH/img_3.jpg

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

 9
6

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

20

修改 VID5.xml 的路径

"VID5.xml"

修改添加切向畸变参数

 1 

（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

标定

cd camera_calibration_opencv
./build/Camera_Calibration in_VID5.xml

程序启动后会显示标定图像的角点提取情况，之后会显示校正后图像，一个一个全部关闭后才会保存标定参数至 out_camera_data.xml

（4）参数填入 ORB-SLAM2 的配置文件

参数输出在 out_camera_data.xml 中:

是相机内参矩阵，顺序为 fx, 0, cx; 0, fy, cy; 0, 0, 1。
是畸变参数，其顺序为 k1, k2, p1, p2, k3。
之后在 ORB_SLAM2 中新建一个配置文件 AndroidPhone.yaml（TUM1.yaml 放在一个目录下），将 TUM1.yaml 的内容拷贝过来，并把其中的 Camera 参数按照out_camera_data.xml 得到的进行修改。

五、跑ORB_SLAM2

打开终端 1:
roscore
手机进入 app 运行摄像头
打开终端2: 在 Android_Camera-IMU 目录

roslaunch android_cam-imu.launch

（可以关掉 Rviz）

打开终端 3:

rosrun ORB_SLAM2 Mono /home/wei/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/wei/catkin_ws/src/ORB_SLAM2/Examples/Monocular/AndroidPhone.yaml

自行修改路径
运行图

六、参考博客

https://www.cnblogs.com/MingruiYu/p/12404730.html
https://www.jianshu.com/p/967a35dbb56a
https://blog.csdn.net/qq_29796781/article/details/80008643
https://blog.csdn.net/weixin_44436677/article/details/105587986
其他部分博客在文中。