Ubuntu18.04+Android phone IMU+ROS Melodic running ORB-SLAM2

foreword

The formation of this blog is inseparable from the blog of Tsinghua University, and it is published here https://www.cnblogs.com/MingruiYu/p/12404730.html

1. Installation of ROS Melodic on version 18.04 of ubuntu system

Please move to my previous blog ROS Melodic installation on ubuntu system version 18.04 (perfect pit avoidance)

2. Installation, configuration, and operation of SLAM monocular instance based on ROS, ORB_SLAM2

1. Early SLAM environment configuration

Please move to my previous blog Ubuntu18.04 to build a SLAM environment (perfect to avoid pitfalls, the version corresponds to no error)

2. Install ORB_SLAM2 under ROS

To run ORB_SLAM in the ROS environment, it is best to put it in the src folder of the workspace (workspace directory: catkin_ws/src)
and enter the src folder of the ROS workspace.

cd ~/catkin_ws/src/

Download and install ORB_SLAM2

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

Go into the ORB_SLAM2 folder.

cd ORB_SLAM2

Give permission to build.sh file.

chmod +x build.sh

Compile the build.sh file

./build.sh

An error may be reported here
Question 1:
ORB_SLAM2/src/System.cc: error: 'usleep' was not declared in this scope usleep(5000);
Solution:
Find the header of the corresponding System.cc file and add it to the header file

#include<unistd.h>

According to the actual situation, if there is a problem with the file usleep, add this header file.
The files that need to add unistd.h are:
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
In addition, if there are self-modification
reference links https://blog.csdn.net/ qq_15698613/article/details/98453592
Because you need to run ORB_SLAM2 in the ROS environment, you need to execute the following command

chmod +x build_ros.sh

edit bash file

sudo  gedit ~/.bashrc

Add the path containing Examples/ROS/ORB_SLAM2 to the ROS_PACKAGE_PATH environment variable. Open the .bashrc file and add the content at the end as shown in the figure

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

./build_ros.sh

An error may be reported during compilation.
Question 1:

The reason for this problem is that the version of Pangolin is too high, so the version needs to be reduced to 0.5.
Network disk Pangolin
https://pan.baidu.com/s/1yEQvArzHz35CUJc-BEmNiw
Extraction code: n6r8
Reference link https://blog.csdn.net/qq_33950926/article/details/121129028
Question 2:
CMakeFiles/RGBD.dir/ build.make:197: recipe for target '.../RGBD' failed

Solution:
Modify the CMakeLists.txt file in the home/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/ folder.
Add the following line of code after set(LIBS xxxxx.

-lboost_system

Just recompile.
Refer to the link https://blog.csdn.net/weixin_44436677/article/details/105587986
So far, ORB_SLAM2 has been installed.

3. Run the monocular SLAM instance

After the compilation is complete, various executable files will be generated under the ORB_SLAM2/Examples folder. Here we take the monocular case as an example to show how to run the ORB_SLAM2 program.

(1) Download the dataset

There are three datasets: TUM, KITTI, and EuRoC. The TUM dataset is used here, and the sequence is downloaded from http://vision.in.tum.de/data/datasets/rgbd-dataset/download and decompressed. I downloaded the first one, and the downloaded file name is: rgbd-dataset_freiburg_xyz.

(2) compile

The official command format is as follows:
PATH_TO_SEQUENCE_FOLDER is the storage path of the dataset.
tumx.yaml corresponds to the dataset, for example, TUM1.yaml, TUM2.yaml, and TUM3.yaml correspond to freiburg1, freiburg2, and freiburg3 respectively.

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

Modify appropriately according to the different save paths, mine is in the main directory

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

(3) Results

It is enough to display the feature point extraction diagram. I forgot to take a screenshot here. The network diagram is as follows

to prove that the configuration is correct.

3. Establish communication transmission between Android phone camera and PC

1. Android tool download

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 版本

Be sure to git the complete folder, which will be used later. Copy the compiled apk in the cloned folder to the Android phone and install it on the phone.

2. Connect the hotspot to put the PC and the Android phone under the same LAN

Operation mode:
Terminal 1: roscore
Android mobile phone: enter your computer ip (the IP of the PC can be checked by entering ifconfig on the PC terminal), and then click Connect. If the connection is successful, it will enter the camera interface.
Terminal 2:

cd Android_Camera-IMU
roslaunch android_cam-imu.launch

Then a Rviz interface will pop up:
if you want to display the image in real time, you need to Add - By topic - add /camera/image_raw/image.
If you want to display imu, you need to Add - By topic - add imu, and change map to imu in Fix Frame.
Reference link https://www.cnblogs.com/MingruiYu/p/12404730.html

4. Camera parameter calibration of Android phone camera

Calibration board

(1) Collect and save pictures

Here we refer to an automatic capture and save code written by Tsinghua University, and modify it directly on the basis of the ros_mono.cc code of ORB-SLAM2, and write a ros_camera_capture.cc in the same directory as ros_mono.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;
    }
}

Pay attention to modify the directory where the image is saved.
In addition, add the following content to CMakeLists.txt in the ORB_SLAM2/Examples/ROS/ORB_SLAM2 directory (just add it above # 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}
)

Recompile the ORB_SLAM2 project

cd PATH/ORB_SLAM2
./build_ros.sh

Open terminal 1: enter the app on
the roscore
phone and run the camera
Open terminal 2: in the Android_Camera-IMU directory

roslaunch android_cam-imu.launch

(You can turn off Rviz)
Open terminal 3:

rosrun ORB_SLAM2 CameraCapture

Select the image frame corresponding to the mobile phone in the computer with the mouse, and press the q key to save the image.

(2) Perform calibration and create a new working directory

The main directory (folder) camera_calibration_opencv,
copy the contents of the samples/cpp/tutorial_code/calib3d/camera_calibration folder in the OpenCV installation directory to this directory.
Modify VID5.xml
VID5.xml stores the path of the calibration image, so add the path of all calibration images in VID.xml (previous step)

<?xml version="1.0"?>
<opencv_storage>
<images>
PATH/img_1.jpg
PATH/img_2.jpg
PATH/img_3.jpg
</images>
</opencv_storage>

Modify the width and height of the checkerboard. Note that the width and height here refer to the number of internal intersections, not the number of squares. As shown above, the data of the chessboard is 9 and 6

<BoardSize_Width> 9</BoardSize_Width>
<BoardSize_Height>6</BoardSize_Height>

Modify the side length (mm) of each grid, measure with a ruler

<Square_Size>20</Square_Size>

Modify the path of VID5.xml

<Input>"VID5.xml"</Input>

Modify and add tangential distortion parameters

<Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>

(3) Compile and run, calibrate

Create a new CMakeLists.txt in the working directory camera_calibration_opencv:

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

calibration

cd camera_calibration_opencv
./build/Camera_Calibration in_VID5.xml

After the program starts, it will display the corner point extraction of the calibrated image, and then it will display the corrected image, and the calibration parameters will be saved to out_camera_data.xml after all are closed one by one.

(4) Fill in the parameters in the configuration file of ORB-SLAM2

The parameter output is in out_camera_data.xml:

<camera_matrix type_id="opencv-matrix"> is the camera internal reference matrix, the order is fx, 0, cx; 0, fy, cy; 0, 0, 1.
<distortion_coefficients type_id="opencv-matrix"> is the distortion parameter, its order is k1, k2, p1, p2, k3.
Then create a new configuration file AndroidPhone.yaml in ORB_SLAM2 (TUM1.yaml is placed in a directory), copy the contents of TUM1.yaml, and modify the Camera parameter according to out_camera_data.xml.

Five, run ORB_SLAM2

Open terminal 1: enter the app on
the roscore
phone and run the camera
Open terminal 2: in the Android_Camera-IMU directory

roslaunch android_cam-imu.launch

(Rviz can be turned off)

Open terminal 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

Modify the path by yourself
Run the graph

Six, reference blog

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
Other parts of the blog are in the text.