Cet article détaille la procédure de mise en œuvre d'ORB-SLAM2 en utilisant une caméra Intel RealSense D435I pour la loaclisation et la cartographie simultanées (SLAM) en temps réel.
Configuration Initiale
Pour commencer, lancez le nœud de la caméra RealSense. Assurez-vous que la résolution et la fréquence d'images sont définies de manière appropriée pour votre application.
roslaunch realsense2_camera rs_camera.launch color_width:=640 color_height:=480 color_fps:=30
Fichier de Configuration et Lancement
Afin de simplifier le lancement et la configuration, un fichier .launch personnalisé est créé. Ce fichier spécifie les chemins vers le vocabulaire ORB et les paramètres de la caméra, ainsi que les remappages des sujets ROS nécessaires pour la synchronisation des flux d'images couleur et de profondeur.
<launch>
<arg name="path_to_vocabulary" default="/path/to/ORBvoc.txt" />
<arg name="path_to_settings" default="/path/to/D435i_settings.yaml" />
<node name="orb_slam2_rgbd" pkg="ORB_SLAM2" type="D435i_RGBD" output="screen"
args="$(arg path_to_vocabulary) $(arg path_to_settings)">
<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" />
<remap from="/camera/depth/image" to="/camera/aligned_depth_to_color/image_raw" />
</node>
</launch>
Pour exécuter le système, lancez d'abord le maître ROS, puis le fichier de lancemant personnalisé :
roscore
roslaunch your_package_name d435i_orbslam.launch
Paramètres de la Caméra (Fichier YAML)
La performance d'ORB-SLAM2 dépend fortement de la précision des paramètres intrinsèques de la caméra. Le fichier de configuration YAML doit contenir les valeurs correctes pour la focale (fx, fy), le centre optique (cx, cy), les coefficients de distorsion (k1, k2, p1, p2), ainsi que les dimensions de l'image. Un paramètre crucial pour les systèmes RGB-D est le facteur de l'échelle de profondeur (DepthMapFactor), qui doit être ajusté en fonction de l'unité de mesure de la profondeur fournie par la caméra (souvent 1000.0 si la profondeur est en millimètres).
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.fx: 604.658935546875
Camera.fy: 604.419189453125
Camera.cx: 330.9848327636719
Camera.cy: 244.76495361328125
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.width: 640
Camera.height: 480
Camera.fps: 30
# Baseline times fx (approximate)
Camera.bf: 30.282
# Color order (0: BGR, 1: RGB)
Camera.RGB: 1
# Depth thresholds
ThDepth: 40.0
# Depth map values factor - crucial for RGB-D
DepthMapFactor: 1000.0
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
# Number of features per image
ORBextractor.nFeatures: 1000
# Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2
# Number of levels in the scale pyramid
ORBextractor.nLevels: 8
# FAST threshold parameters
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
Amélioration des Performances
Après avoir ajusté le DepthMapFactor à 1000.0 et corrigé le chemin vers le fichier YAML, les performances de suivi se sont considérablement améliorées, avec une réduction notable de la dérive et de la perte de suivi.
Sujets ROS Courants
L'exécution du système génère une variété de sujets ROS, notamment ceux liés aux flux d'images couleur et de profondeur alignés, aux informations de la caméra, et aux sorties d'ORB-SLAM2 telles que la pose de la caméra et la trajectoire.
Code Source Modifié (Extrait)
Le code source C++ du nœud ORB-SLAM2 a été adapté pour intégrer les fonctionnalités ROS. Il utilise les bibliothèques cv_bridge et message_filters pour gérer la synchronisation des images couleur et de profondeur. Le code gère également la publication des poses de caméra et de la trajectoire via des messages ROS et la diffusion des transformations TF.
#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>
#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Path.h>
#include <tf/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <opencv2/core/core.hpp>
#include "../../../include/System.h" // Assurez-vous que le chemin est correct
// Déclaration de la classe ImageGrabber
class ImageGrabber
{
public:
ros::NodeHandle nh;
ros::Publisher pub_pose, pub_path;
tf::TransformBroadcaster tf_broadcaster;
nav_msgs::Path camera_path_msg;
std::string camera_frame_id;
std::string map_frame_id;
ImageGrabber(ORB_SLAM2::System* pSLAM); // Constructeur
void GrabRGBD(const sensor_msgs::ImageConstPtr& msg_rgb, const sensor_msgs::ImageConstPtr& msg_depth);
private:
ORB_SLAM2::System* mpSLAM;
};
// Implémentation du constructeur
ImageGrabber::ImageGrabber(ORB_SLAM2::System* pSLAM) : mpSLAM(pSLAM), nh("~")
{
pub_pose = nh.advertise<geometry_msgs::PoseStamped>("CameraPose", 10);
pub_path = nh.advertise<nav_msgs::Path>("Path", 10);
// Lecture des IDs de trame depuis le serveur de paramètres
nh.param<std::string>("camera_frame_id", camera_frame_id, "camera_link");
nh.param<std::string>("pointcloud_frame_id", map_frame_id, "map");
}
// Point d'entrée principal
int main(int argc, char **argv)
{
ros::init(argc, argv, "orb_slam2_rgbd_node");
ros::start();
if (argc != 3)
{
ROS_ERROR("Usage: rosrun ORB_SLAM2 D435i_RGBD path_to_vocabulary path_to_settings");
ros::shutdown();
return 1;
}
// Initialisation du système ORB-SLAM2
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
ImageGrabber grabber(&SLAM);
ros::NodeHandle node_handle;
// Configuration des abonnés synchronisés pour les images RGB et de profondeur
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(node_handle, "/camera/color/image_raw", 10);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(node_handle, "/camera/aligned_depth_to_color/image_raw", 10);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicy;
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), rgb_sub, depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &grabber, _1, _2));
ros::spin(); // Maintient le nœud en marche
// Arrêt des threads SLAM et sauvegarde de la trajectoire
SLAM.Shutdown();
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
ros::shutdown();
return 0;
}
// Implémentation de la fonction de callback pour les images RGB-D
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msg_rgb, const sensor_msgs::ImageConstPtr& msg_depth)
{
cv_bridge::CvImageConstPtr cv_ptr_rgb, cv_ptr_depth;
try
{
cv_ptr_rgb = cv_bridge::toCvShare(msg_rgb);
cv_ptr_depth = cv_bridge::toCvShare(msg_depth);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Vérification de la validité des images
if (cv_ptr_rgb->image.empty() || cv_ptr_depth->image.empty())
{
ROS_WARN("Received empty image(s).");
return;
}
// Traitement de l'image par ORB-SLAM2
cv::Mat pose_matrix_cw;
pose_matrix_cw = mpSLAM->TrackRGBD(cv_ptr_rgb->image, cv_ptr_depth->image, cv_ptr_rgb->header.stamp.toSec());
if (!pose_matrix_cw.empty())
{
// Extraction de la rotation et de la translation
cv::Mat rotation_matrix = pose_matrix_cw.rowRange(0, 3).colRange(0, 3);
cv::Mat translation_vector = pose_matrix_cw.rowRange(0, 3).col(3);
// Conversion en message TF
tf::Matrix3x3 rotation_tf(rotation_matrix.at<float>(0, 0), rotation_matrix.at<float>(0, 1), rotation_matrix.at<float>(0, 2),
rotation_matrix.at<float>(1, 0), rotation_matrix.at<float>(1, 1), rotation_matrix.at<float>(1, 2),
rotation_matrix.at<float>(2, 0), rotation_matrix.at<float>(2, 1), rotation_matrix.at<float>(2, 2));
tf::Vector3 translation_tf(translation_vector.at<float>(0), translation_vector.at<float>(1), translation_vector.at<float>(2));
tf::Quaternion rotation_quat;
rotation_tf.getRotation(rotation_quat);
// Création du message de pose
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.stamp = msg_rgb->header.stamp;
pose_msg.header.frame_id = map_frame_id;
tf::poseTFToMsg(tf::Pose(rotation_quat, translation_tf), pose_msg.pose);
// Mise à jour et publication de la trajectoire
camera_path_msg.header.stamp = msg_rgb->header.stamp;
camera_path_msg.header.frame_id = map_frame_id;
camera_path_msg.poses.push_back(pose_msg);
pub_pose.publish(pose_msg);
pub_path.publish(camera_path_msg);
// Publication de la transformation TF (map -> camera_link)
tf::StampedTransform transform(tf::Pose(rotation_quat, translation_tf), msg_rgb->header.stamp, map_frame_id, camera_frame_id);
tf_broadcaster.sendTransform(transform);
}
else
{
// Gestion des échecs de suivi
int tracking_state = mpSLAM->GetTrackingState();
ROS_WARN_THROTTLE(1.0, "ORB-SLAM2 tracking failed or not initialized. State: %d", tracking_state);
if (tracking_state == 1) { // NOT_INITIALIZED
ROS_WARN_THROTTLE(5.0, "Initialization is required. Check depth data and camera parameters.");
} else if (tracking_state == 3) { // LOST
ROS_WARN_THROTTLE(5.0, "Tracking lost. Relocation needed.");
}
}
}