【ORB-SLAM3】Utiliser l'appareil photo d'un téléphone Android pour exécuter ORB3

1. Introduction

J'apprenais ORBSLAM2/3 récemment, et j'ai trouvé un article sur l'exécution d'ORB2 sur un téléphone Android. J'ai suivi l'article et j'ai réussi à exécuter ORB3 sur un téléphone Android. Cet article enregistrera les détails.

Poste de référence : ORB-SLAM2 en cours d'exécution - ROS + caméra de téléphone Android

L'environnement de cet article :

• Ubuntu 20.04
• ROSNoétique
• Téléphone Android (Redmi note9 pro)

2.ros installation noétique

Lorsque vous utilisez un ordinateur plus récent, l'installation de l'ancienne version d'ubuntu entraînera divers problèmes d'incompatibilité de pilotes matériels. J'ai donc installé la version 20.04 directement sur ubuntu et la version ros correspondante est noétique.

Référence d'installation :

Site officiel (https://wiki.ros.org/noetic/Installation/Ubuntu)

erreur:

sudo rosdep init ne peut pas se connecter

résoudre:

# 1.pip安装rosdepc
sudo pip install rosdepc
# 如果显示没有pip可以试试pip3
sudo pip3 install rosdepc
# 如果pip3还没有
sudo apt-get install python3-pip 
sudo pip install rosdepc

# 2.用rosdepc替换原来的rosdep安装
sudo rosdepc init
rosdepc update

3. Installation de l'ORB-SLAM3

À l'origine, je voulais d'abord exécuter ORB2.ORB2 prend en charge opencv2/3, mais ROS-Noetic installe opencv4 par défaut, et cv_bridge lie opencv4 par défaut, ce qui pose le problème que la version opencv ne correspond pas lors de la compilation ORB2 directement.J'essaie pour installer Opencv3. Et désinstallez le cv_bridge de ros, compilez un cv_bridge reliant opencv3 à partir du code source, le résultat est de trouver opencv4 après compilation, et la solution n'a pas été résolue depuis un jour, je dois donc installer ORB3, ORB3 prend en charge opencv3/4, le processus d'installation est relativement fluide. Les étapes d'installation ci-dessous :

(1) Dépendances d'installation

Nécessite la version Pangolinv0.5, la version opencv3/4, Eigen3, g2o, ROS

(2) Télécharger et compiler

git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
cd ORB_SLAM3
# 修改ORB_SLAM3/CMakeLists.txt
# 把opencv版本改为自己的版本，如 find_package(OpenCV 4 QUIET)
chmod +x build.sh
./build.sh

Q1：

CMake Error at CMakeLists.txt:91 (ADD_LIBRARY): Cannot find source
file:
g2o/stuff/timeutil.cpp

A1：

Si vous copiez directement le code source ORB3 dans XTDrone, cette erreur sera signalée car il /Thirdparty/g2o/g2o/stuff/manque des fichiers timeutil.h、timeutil.cpp. Vous devez télécharger les fichiers correspondants sur le site officiel.

Q2：

ORB SLAM2 找不到 libDBoW2.so / libg2o.so

A2：

ORB_SLAM3/Thirdparty/DBoW2Le dossier manquant du code source ORB3 dans XTDrone , mais je ne trouve pas le dossier correspondant libsur le site officiel ORB actuel . Il existe un tutoriel sur la façon de compiler vous-même les fichiers de bibliothèque manquants. Cependant, je l'ai résolu en trouvant et en tapant les fichiers de bibliothèque manquants directement sur l'ordinateur installé par d'autres.lib

(3) Compiler dans ROS

# 路径需要自己修改
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/catkin_ws/src/ORB_SLAM3/Examples/ROS 
# 修改ORB_SLAM3/Examples/ROS/ORB_SLAM3/CMakeLists.txt
# 把opencv版本改为自己的版本，注意要和前面修改的CMakeLists.txt中的opencv版本一致
# 如 find_package(OpenCV 4 QUIET)
# -----------------
chmod +x build_ros.sh
./build_ros.sh

(4) Exécutez ORB3

ORB3 prend en charge l'exécution de commandes monoculaires, binoculaires, de profondeur et combinées avec l'IMU. Étant donné que la caméra du téléphone portable est monoculaire, parlons d'ORB3 exécutant des commandes monoculaires :

rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

PATH_TO_VOCABULARYIl s'agit du chemin du modèle de sac de mots pré-formé hors ligne, qui ORB_SLAM3/Vocabularyest un txtfichier sous le fichier source ;

PATH_TO_SETTINGS_FILEIl s'agit d'un fichier de configuration de caméra, qui enregistre les paramètres internes de la caméra de la caméra monoculaire. Nous devons recalibrer les paramètres internes de la caméra ultérieurement. ORB fournit un fichier de configuration de référence, et le chemin est dansORB_SLAM3/Examples/Monocular/TUM1.yaml

4. Communication basée sur ROS entre la caméra du téléphone Android et le PC

Notre ORB3 fonctionne sur le ROS du PC, et le capteur de la caméra est sur le téléphone mobile, nous devons donc établir la communication entre l'image de la caméra du téléphone mobile et le ROS côté PC, et publier l'image de la caméra du téléphone mobile en tant que ROS sujet. Ceci est basé sur un sur GitHub. Projet : hitcm/Android_Camera-IMU , l'auteur a réalisé le transfert des informations de l'appareil photo et des informations IMU du téléphone mobile vers le PC (reportez-vous au blog de l'auteur ROS pour collecter des images Android et Données IMU en temps réel ). Dans cet article, nous n'avons besoin d'utiliser que les informations de la caméra.

(1) mise en place

# 电脑PC端
git clone https://github.com/hitcm/Android_Camera-IMU.git
sudo apt-get install ros-noetic-imu-tools  # 修改对应自己的 ROS 版本
# 安卓手机端
# 将git下载的源文件中的apk安装包发给手机并安装

(2) courir

# 1.PC和手机处于同一个网段，例如PC和手机连同一个wifi，然后查看PC端ip
ifconfig   # 记录一下PC端的ip地址

# 2.运行roscore
roscore

# 3.手机上运行安装好的软件，在 IP Port 中修改 IP 地址为 PC的 IP地址，port不需要修改，之后点击 Connect，连接成功则进入相机界面。
# 例如 http://192.168.31.192:11311/

# 4.运行roslaunch文件，复制出一个跑orb3需要的摄像头话题
cd pathToAndroid_Camera-IMU  # 就是cd到刚才git下载的源文件
roslaunch android_cam-imu.launch
# 没报错的话，会弹出一个rviz，直接关掉就可以了

# 5.查看图片话题
rqt_image_view   # 如果成功看到有图形，那么就成功了

5. Calibrage de la caméra du téléphone portable

Pour que ORB-SLAM3 fonctionne avec précision, la caméra du téléphone portable doit être calibrée. La méthode d'étalonnage est la suivante : prendre des photos de la carte d'étalonnage en damier dans toutes les directions, puis effectuer un étalonnage basé sur OpenCV. Notez que les images collectées ici doivent être cohérentes avec celles lues par le programme ORB-SLAM3, vous ne pouvez donc pas utiliser directement l'application appareil photo intégrée du téléphone pour prendre des photos, car le téléphone la corrigera automatiquement via l'algorithme, et la communication ci-dessus transmet des images brutes. Par conséquent, la première tâche que nous devons accomplir est : collecter et enregistrer l'image de la caméra.

1. Recueillir des images d'étalonnage de la caméra

(1) Carte de calibrage d'impression

Imprimez l'image de la carte d'étalonnage ci-dessous

(2) Écrivez le code pour collecter des images

À l'heure actuelle, il n'y a aucun moyen de l'enregistrer directement, nous choisissons donc d'écrire un nœud ROS pour recevoir l'image du téléphone mobile, puis de l'afficher et de l'enregistrer via OpenCV.

Par commodité, on choisit de ORB_SLAM3/Examples/ROS/ORB_SLAM3/srcros_mono.ccmodifier directement sur la base du code d'ORB-SLAM3, et ros_mono.ccd'écrire un code dans le même répertoire ros_camera_capture.cc, le contenu est :

/**
* 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 = "~/"; // 修改为自己保存图片的路径，注意路径末尾带上/
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;
    }
}

(3) Compiler le code

Ajoutez le contenu suivant dans ORB_SLAM3/Examples/ROS/ORB_SLAM3le répertoire CMakeLists.txt(il suffit d'ajouter ci-dessus # Node pour caméra monoculaire):

# Node for capture images for camera calibration
rosbuild_add_executable(CameraCapture
src/ros_camera_capture.cc
)

target_link_libraries(CameraCapture
${LIBS}
)

Après cela, recompilez le projet ORB_SLAM3

cd PATH/ORB_SLAM3
./build_ros.sh

(4) Exécutez le code pour collecter des images

# 终端1
roscore

# 手机
运行应用软件并连接

# 终端2
cd pathToAndroid_Camera-IMU  # 就是cd到刚才git下载的源文件
roslaunch android_cam-imu.launch
# 可以关掉rviz

# 终端3
rosrun ORB_SLAM3 CameraCapture
# 会显示手机摄像头图片，按下 q 键保存图像

Faites attention à photographier la carte d'étalonnage sous plusieurs angles lors de la prise de vue. Étant donné que l'image est floue, vous devez être plus proche lors de la prise de vue.

2. Calibrez la caméra

Pour le programme d'étalonnage de la caméra, nous utilisons le programme d'exemple opencv. Copiez le dossier du répertoire d'installation d'OpenCV samples/cpp/tutorial_code/calib3d/camera_calibrationdans le chemin personnalisé, créez-y un nouveau sous-dossier imget placez toutes les images précédemment enregistrées imgdans le dossier. La structure du fichier est la suivante :

camera_calibration_opencv

├── camera_calibration.cpp
├── CMakeLists.txt
├── img
│ ├── img_10.jpg
│ ├── img_11.jpg
│ ├── img_1.jpg
│ ├── img_2.jpg
│ ├── img_3. jpg
│ ├── img_4.jpg
│ ├── img_5.jpg
│ ├── img_6.jpg
│ ├── img_7.jpg
│ ├── img_8.jpg
│ └── img_9.jpg
├── dans_VID5.xml
├ ── out_camera_data.yml
└── VID5.xml

(1) Modifier VID5.xml

Le chemin de l'image de calibrage est stocké dans VID5.xml, ajoutez donc le chemin de toutes les images de calibrage dans VID.xml, par exemple :

<?xml version="1.0"?>
<opencv_storage>
<images>
../img/img_1.jpg
../img/img_2.jpg
../img/img_3.jpg
../img/img_4.jpg
../img/img_5.jpg
../img/img_6.jpg
../img/img_7.jpg
../img/img_8.jpg
../img/img_9.jpg
../img/img_10.jpg
../img/img_11.jpg
</images>
</opencv_storage>

Notez que le chemin de l'image à l'intérieur doit être un chemin relatif, pas un chemin absolu, sinon une erreur sera signalée.

(2) Modifier in_VID5.xml

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

# 修改为每格的边长 (mm)，拿尺子量。
<Square_Size>19</Square_Size>

# 修改 VID5.xml 的路径，这里路径是相对路径
<Input>"../VID5.xml"</Input>

# 此处原来是0，需要改为1，表示引入切向畸变参数（因为 ORB-SLAM2 中也引入了切向畸变参数），否则只有径向畸变参数
<Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>

(3) Compiler le projet

Créez un nouveau CMakeLists.txt dans le répertoire de travail 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})

Après compilation :

cd camera_calibration_opencv
mkdir build
cd build
cmake ..
make

(4) Exécutez le programme pour l'étalonnage

cd camera_calibration_opencv/build
./Camera_Calibration ../in_VID5.xml

Comme je l'ai déjà dit lors de la modification de la somme, VID5.xmlutilisez in_VID5.xmlun chemin relatif au lieu d'un chemin absolu, sinon une erreur se produira :
Input does not exist: VID5.xmlInvalid input detected

Ensuite, vous devez savoir quel est le chemin actuel lorsque vous utilisez un chemin relatif. Le chemin actuel est Camera_Calibrationle chemin où votre terminal exécute le fichier exécutable, par exemple :

# 运行方法一:
cd camera_calibration_opencv/build
camera_calibration_opencv/build$./Camera_Calibration ../in_VID5.xml
# 终端当前路径就是$号前面的路径，也就是camera_calibration_opencv/build，所以in_VID5.xml的相对路径就是../in_VID5.xml

# 运行方法二:
cd camera_calibration_opencv
camera_calibration_opencv$./build/Camera_Calibration in_VID5.xml
# 终端当前路径就是camera_calibration_opencv，所以in_VID5.xml的相对路径就是in_VID5.xml

Après une opération réussie, les images d'étalonnage seront affichées en séquence et l'erreur de reprojection sera émise dans le terminal. Plus l'erreur est petite, mieux c'est. Ici, c'est 0,3. Si elle est trop grande, reprenez une photo.

Re-projection error reported by calibrateCamera: 0.299492
Calibration succeeded. avg re projection error = 0.299491

(5) Remplissez le fichier de configuration SETTINGS_FILE de ORB-SLAM3

Lorsque vous exécutez avec succès le programme d'étalonnage, le fichier de résultat de référence interne de la caméra sera généré dans le même répertoirecamera_calibration_opencv/build/out_camera_data.xml

Ouvrez et trouvez les paramètres internes de la caméra à l'intérieur :

• <camera_matrix type_id="opencv-matrix">est la matrice de référence interne de la caméra, l'ordre est fx, 0, cx ; 0, fy, cy ; 0, 0, 1.
• <distortion_coefficients type_id="opencv-matrix">est le paramètre de distorsion, son ordre est k1, k2, p1, p2, k3.

Copiez-le ensuite ORB_SLAM3/Examples/Monocularsous le chemin TUM1.yaml, nommez-le AndroidPhone.yaml, et renseignez les paramètres internes de la caméra :

Camera1.fx: 454.608394
Camera1.fy: 454.608394
Camera1.cx: 319.500000
Camera1.cy: 239.500000

Camera1.k1: 0.180868
Camera1.k2: -1.037955
Camera1.p1: 0.00
Camera1.p2: 0.00
Camera1.k3: 2.046420

6. Exécutez ORB-SLAM3

# 1.
roscore

# 2.
手机运行应用程序并连接

# 3.
cd pathToAndroid_Camera-IMU  
roslaunch android_cam-imu.launch # 可以关掉rviz

# 4.
rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
# PATH_TO_VOCABULARY就是ORB_SLAM3/Vocabulary/ORBvoc.txt的绝对路径
# PATH_TO_SETTINGS_FILE就是AndroidPhone.yaml的绝对路径

Affichage de l'effet de course :

Remarque : L'ORB-SLAM3 Mono est encore relativement difficile à initialiser (ses conditions d'initialisation sont relativement difficiles). Au début, sélectionnez une zone avec des textures riches en fonctionnalités, ne tournez pas purement et déplacez la caméra vers le haut, le bas, la gauche et la droite. , ce qui est propice à l'initialisation.