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<unistd.h>
根据实际情况，提示哪个文件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<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
打开终端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 中添加所有标定图像的路径（上一步）
<?xml version="1.0"?>
<opencv_storage>
<images>
PATH/img_1.jpg
PATH/img_2.jpg
PATH/img_3.jpg
</images>
</opencv_storage>
修改棋盘格的宽和高，注意，这里的宽度和高度是指内部交叉点的个数，而不是方形格的个数。如上图棋盘的数据就是9和6
<BoardSize_Width> 9</BoardSize_Width>
<BoardSize_Height>6</BoardSize_Height>
修改每格的边长 (mm)，拿尺子量
<Square_Size>20</Square_Size>
修改 VID5.xml 的路径
<Input>"VID5.xml"</Input>
修改添加切向畸变参数
<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
标定
cd camera_calibration_opencv
./build/Camera_Calibration in_VID5.xml
程序启动后会显示标定图像的角点提取情况，之后会显示校正后图像，一个一个全部关闭后才会保存标定参数至 out_camera_data.xml
（4）参数填入 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 参数按照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
其他部分博客在文中。
