本文有点长，可以根据目录跳转到想看的部分。因为仿真和应用环境不同，可能例程的运行方式（输入话题等）有所不同，但第三部分有关ORB_SLAM3相机仿真标定、第四部分有关ORB_SLAM3源码修改的部分是通用的。
目录
一、仿真环境配置
1.双系统安装
① 工具准备
② 启动盘制作
i. 格式化U盘
ii. 制作启动盘
③ 系统安装
2. XTDrone仿真系统安装
二、ORB_SLAM3源码下载及编译
1. 源码下载
2. 编译
① ORB_SLAM3编译
② ROS例程编译
3. 测试
① realsense2安装
② realsense_ros安装
③ 重新编译ORB_SLAM3
④ 运行例程
三、ROS例程的运行与修改
1. 相机标定
① 制作标定世界
②制作launch文件
③ 运行世界并完成标定
i. 启动Gazebo仿真
ii. 启动XTDrone无人机控制模块
iii. 启动标定节点完成标定
iv.标定数据转换为orb_slam3的相机标定数据
2. 修改源码
3. 运行ROS例程
四、修改ORB_SLAM3源码实现点云、图像的ROS消息输出
1. 源码分析
① 初始化过程
② 显示输出过程
③ 应用方法
2. 利用pcl与ros实现点云转存与ROS消息发布
① 改写ORB_SLAM3
i.修改MapDrawer.cc
a.添加PCL头文件
b.修改函数void MapDrawer::DrawMapPoints()
c.修改函数申明
ii.修改Viewer.cc
a.添加PCL头文件，引用外部定义的全局变量
b.修改 void Viewer::Run() 函数
iii.修改system.h
a.添加PCL头文件
b.定义一个结构体变量，包含点云、图像、追踪信息
c.申明新定义的函数
iv. 修改system.cc
a.添加PCL头文件，定义全局变量
b.修改TrackMonocular函数，形成新函数
c.修改CMakeList.txt
② 改写ROS应用层
i. 修改ros_mono.cc
ii. 修改CMakeList.txt
3. 运行
五、附录
附录一：测试使用的无人机模型文件
附录二：无人机测试环境及launch文件
一、仿真环境配置
采用XTDrone实现无人机Gazebo仿真。
1.双系统安装
① 工具准备
Rufus软件
点击链接下载安装Rufus软件，选择图示版本。
Usb3.0接口U盘一个，推荐容量大于16GUbuntu18.04桌面版镜像文件
点击链接下载安装Ubuntu18.04系统镜像，选择桌面版镜像，并设置好下载路径确保能找到镜像。
② 启动盘制作
i. 格式化U盘
ii. 制作启动盘
使用Rufus软件制作启动盘。选择对应的U盘，选择下载好的Ubuntu镜像文件，其余设置与图示相同。等待制作完成关闭即可。
③ 系统安装
若为双系统安装，需压缩硬盘留出系统安装空间，单系统安装可跳过。需要注意的是，最好预留出90GB以上的空间，否则如果后期空间不够很麻烦。
将U盘插上电脑、重启，进入boot模式，选择启动介质。（可自行搜索自己电脑进入boot的方法）
选择启动盘后，选择安装Ubuntu。
选择好语言，点击下一步（建议选择English(US)）。
选择Normal installation 选项，如下图所示。
选择Something else 选项，如下图所示。
设置分区根据电脑内存和存储不同而不同，具体参见Ubuntu分区，勾选后点击install now。
最后设置好用户名和密码，等待安装完成后，重启。
2. XTDrone仿真系统安装
仿真平台的所有配置根据XTDrone文档进行即可。
仿真平台基础配置 · 语雀
二、ORB_SLAM3源码下载及编译
1. 源码下载
原始代码下载地址：GitHub - UZ-SLAMLab/ORB_SLAM3: ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAMORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM - UZ-SLAMLab/ORB_SLAM3https://github.com/UZ-SLAMLab/ORB_SLAM3 建议使用注释版本，利于代码阅读（第三方提供中文注释）：GitHub - electech6/ORB_SLAM3_detailed_comments: Detailed comments for ORB-SLAM3Detailed comments for ORB-SLAM3. Contribute to electech6/ORB_SLAM3_detailed_comments development by creating an account on GitHub.https://github.com/electech6/ORB_SLAM3_detailed_comments 因为一些原因git clone可能网速很慢，可以下载.zip文件后解压。
2. 编译
① ORB_SLAM3编译
在编译之前，需要安装OpenCV（建议安装3.2.0版本，这样与cv_bridge使用的OpenCV版对应，不容易出现版本冲突问题）、Eigen3（官方要求高于3.1.0版本）。
OpenCV与Eigen3安装教程可以自行搜索。其他依赖在项目wiki中有写明。
配置完依赖后，执行以下代码进行编译。
cd ORB_SLAM3
chmod +x build.sh
./build.sh
成功编译完成后，会在/ORB_SLAM3/lib/目录下生成libORB_SLAM.so文件。
② ROS例程编译
首先将以下代码加入~/.bashrc中。
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
随后运行：
chmod +x build_ros.sh
./build_ros.sh
3. 测试
可以使用数据集测试，我们这边用了T265 ROS版本测试。
① realsense2安装
安装参考：T265驱动安装_英特尔t265驱动-CSDN博客
值得注意的是，因为T265已经停产，最新版本的realsense2已经不支持T265。建议使用2.50.0版本（librealsense-2.50.0.zip），ROS驱动对应的是2.3.2版本（realsense-ros-2.3.2.zip）
下载完成后，先配置依赖后编译realsense2：
# 依赖
sudo apt-get install libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libusb-1.0-0-dev pkg-config
sudo apt-get install libglfw3-dev
sudo apt-get install libssl-dev
./scripts/setup_udev_rules.sh
./scripts/patch-realsense-ubuntu-lts.sh
# 编译
mkdir build
cd build
cmake ../ -DBUILD_EXAMPLES=true
make
sudo make install
编译过程中，由于网络问题，curl可能无法下载安装。可以从github上下载.zip压缩包，随后在CmakeList中修改源即可。参考：realsense git libcurl 失败解决的方法（亲测）git失败 改本地压缩包_realsense2安装中curl失败-CSDN博客
② realsense_ros安装
ROS驱动对应的是2.3.2版本（realsense-ros-2.3.2.zip），编译过程如下：
mkdir -p realsense_ros_ws/src
cd realsense_ros_ws/src # 复制解压好的realsense-ros-2.3.2到此目录下
catkin_init_workspace
cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
echo "source ~/YOUR_WORKSAPCE/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
# 测试运行
roslaunch realsense2_camera rs_t265.launch
③ 重新编译ORB_SLAM3
因为ORB_SLAM3在编译时检测到未安装realsense2，则不会编译相关例程。所以，安装测试号realsense2后，重新编译一次即可。
④ 运行例程
# T265 ORBSLAM3
cd
./Examples/Stereo-InInertial/stereo_inertial_realsense_t265 Vocabulary/ORBvoc.txt ./Examples/Stereo-InInertial/RealSense_T265.yaml
等待初始化完成后 ，弹出两个弹窗，一个是点云显示与相机追踪，还有一个是双目相机的图像与特征点标注。
T265的特征点识别效果一般，也可能是测试环境比较简单。
三、ROS例程的运行与修改
1. 相机标定
ROS有一个标定模块camera_calibration，用起来非常方便。具体可以参考：相机标定原理 用ROS camera_calibration 功能包 在gazebo中进行 相机校准_gazebo相机标定世界-CSDN博客
① 制作标定世界
首先需要制作Gazebo世界，需要有无人机（装有需要标定的相机）及标定板。世界文件calibration.world如下：
②制作launch文件
launch文件的制作比较简单，可以直接参考XTDrone的其他启动节点。修改后的calibration.launch文件如下：
其中第七行为上一步calibration.world的存储地址。
③ 运行世界并完成标定
i. 启动Gazebo仿真
cd ~/user_ws/modules/slam/Gazebo_Calibration # 进入到存储launch文件的目录下
roslaunch ./calibration.launch
ii. 启动XTDrone无人机控制模块
# 无人机通讯控制节点
cd ~/XTDrone/communication/
python multirotor_communication.py iris 0
# 用键盘控制无人机飞行
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 0 vel
启动后Gazebo界面如图所示，键盘控制可以控制无人机解锁、起飞等。解锁后，切换到offboard模式，并起飞无人机。
iii. 启动标定节点完成标定
运行cameracalibrator.py。其中，--size参数是标定板点数，设横向黑白格共n个，纵向黑白格共m个，则参数为(n-1)x(m-1)，这里我们是7x7；--square参数是标定板每个小格子的边长，单位是米，这里模型每个小格边长0.25m；image:=是相机图像消息名称；camera:=是相机服务名称。
rosrun camera_calibration cameracalibrator.py --size 7x7 --square 0.25 image:=/iris_0/realsense/depth_camera/color/image_raw camera:=/iris_0/realsense/depth_camera
启动后如图：
此时，运动无人机使得X,Y,Size,Skew均达到绿色（横向、纵向、前后、旋转），calibrate按钮亮起，即可完成标定。
点击calibrate按钮，等待完成计算，save按钮会亮起。
点击save按钮，会存储标定数据到“/tmp/calibrationdata.tar.gz”中。点击commit退出。
iv.标定数据转换为orb_slam3的相机标定数据
参考：ROS+Opencv的双目相机标定和orbslam双目参数匹配_opencvsharp 双目相机-CSDN博客
标定数据解压后找到ost.yaml，我标定完数据如下：
image_width: 1280
image_height: 720
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [1109.51051, 0. , 637.897 ,
0. , 1109.55019, 358.77075,
0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.000544, -0.003422, -0.000323, -0.000315, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [1109.35156, 0. , 637.53416, 0. ,
0. , 1109.77466, 358.61387, 0. ,
0. , 0. , 1. , 0. ]
复制/ORB_SLAM3/Example/ROS/ORB_SLAM3/Asus.yaml文件，并对其进行修改，修改方法如下：
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"
Camera.type: "PinHole" # 针孔相机
# Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 1109.35156 # 对应projection_matrix.data的第1个参数
Camera1.fy: 1109.77466 # 对应projection_matrix.data的第6个参数
Camera1.cx: 637.53416 # 对应projection_matrix.data的第3个参数
Camera1.cy: 358.61387 # 对应projection_matrix.data的第7个参数
# 畸变纠正，对应distortion_coefficients矩阵
Camera1.k1: 0.000544
Camera1.k2: -0.003422
Camera1.p1: -0.000323
Camera1.p2: -0.000315
Camera1.k3: 0.000000
# Camera frames per second
Camera.fps: 30
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 0
# Camera resolution
Camera.width: 1280 # 对应image_width
Camera.height: 720 # 对应image_height
#--------------------------------------------------------------------------------------------
# ORB Parameters 以下为ORB的参数，基本不需要修改
#--------------------------------------------------------------------------------------------
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000
# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2
# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8
# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500.0
2. 修改源码
找到/ORB_SLAM3/Examples/ROS/ORB_SLAM3/src/ros_mono.cc，将第62行中订阅的消息名称修改为自己的消息名称。
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
例如XTDrone例程中无人机的RGB-D相机的RGB图像作为单目输入源，消息名称为：“/iris_0/realsense/depth_camera/color/image_raw”：
#define cfg_ROS_IMG_INPUT_TOPIC "/iris_0/realsense/depth_camera/color/image_raw"
ros::Subscriber sub = nodeHandler.subscribe(cfg_ROS_IMG_INPUT_TOPIC, 1, &ImageGrabber::GrabImage, &igb);
3. 运行ROS例程
重新编译后，运行（其中depth_camera.yaml是上一步标定完成后的标定文件）：
# 启动一个无人机飞行场景
roslaunch px4 zhihang2.launch
# 启动控制及通讯节点
cd ~/user_ws/modules/gui/scripts
python multirotor_communication.py iris 0
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 0 vel
# 启动ORB_SLAM3
rosrun ORB_SLAM3 Mono ~/user_ws/modules/slam/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/user_ws/modules/slam/Gazebo_Calibration/data/depth_camera.yaml
运行效果如图：
四、修改ORB_SLAM3源码实现点云、图像的ROS消息输出
通过观察节点代码可以发现，在ros_mono.cc的倒数第二行调用ORB_SLAM3算法：
mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
此函数返回值只有相机追踪后的估计位置，并不能输出点云与图像。而算法内部则已经全部封装到了/ORB_SLAM3/lib/libORB_SLAM3.so中。因此，如果需要将点云及标注过特征点的图像发布到ROS消息以供其他处理，则需要修改源码。
1. 源码分析
① 初始化过程
第一步是初始化ORB_SLAM3系统：
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);
这一步会调用/src/system.cc下的初始化函数：
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer, const int initFr, const string &strSequence):
mSensor(sensor), mpViewer(static_cast
mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)
{ ... }
在这个函数中，会完成导入数据、开启Tracking线程、LocalMapping线程、LoopClosing线程，开启显示线程。其中，我使用的方法是通过显示线程将数据导出。
在该函数的最后，有创建显示线程的代码：
//Initialize the Viewer thread and launch 创建并开启显示线程
if(bUseViewer)
//if(false) // TODO
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
mpLoopCloser->mpViewer = mpViewer;
mpViewer->both = mpFrameDrawer->both;
}
② 显示输出过程
显而易见，算法通过mptViewer线程运行Viewer::Run函数实现显示功能。因此，找到/src/Viewer.cc文件，其中定义了Viewer::Run函数：
void Viewer::Run() { ... }
其中，以下这段代码是处理标注过特征点的图像。显然，toShow变量就是存储了处理完成的图像。
cv::Mat toShow;
cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
if(both){
cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
cv::hconcat(im,imRight,toShow);
}
else{
toShow = im;
}
if(mImageViewerScale != 1.f) {
int width = toShow.cols * mImageViewerScale;
int height = toShow.rows * mImageViewerScale;
cv::resize(toShow, toShow, cv::Size(width, height));
}
cv::imshow("ORB-SLAM3: Current Frame",toShow);
cv::waitKey(mT);
以下这段代码是处理点云图显示的。然而，我们并不能从这里获取点云数据，因为算法利用了DrawMapPoints函数完成了点云的绘制。
d_cam.Activate(s_cam);
glClearColor(1.0f,1.0f,1.0f,1.0f);
mpMapDrawer->DrawCurrentCamera(Twc);
if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph || menuShowOptLba)
mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph, menuShowOptLba);
if(menuShowPoints)
mpMapDrawer->DrawMapPoints(); // 画出点云
因此，找到/src/MapDrawer.cc文件，找到DrawMapPoints函数：
void MapDrawer::DrawMapPoints() { ... }
其中，这一部分实现了点云的绘制：
for(set
{
if((*sit)->isBad())
continue;
Eigen::Matrix
glVertex3f(pos(0),pos(1),pos(2));
}
③ 应用方法
当一帧图像输入到节点后，会调用TrackMonocular函数完成视觉SLAM（以ros_mono.cc为例）：
mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());
由于ORB_SLAM编译后生成的是lib文件，因此难以在其中嵌入ROS。考虑到需要将数据输出，可以考虑改写TrackMonocular函数，使得函数能够返回点云、图像等数据。
2. 利用pcl与ros实现点云转存与ROS消息发布
① 改写ORB_SLAM3
将点云数据及图像数据存储到一个全局变量中，然后用修改后的TrackMonocular函数（TrackMonocularOutput函数）返回结构体，结构体中包含相机追踪数据、点云数据、图像数据。
i.修改MapDrawer.cc
a.添加PCL头文件
#include
#include
#include
#include
b.修改函数void MapDrawer::DrawMapPoints()
修改函数中以下for循环：
// 修改前
for(set
{
if((*sit)->isBad())
continue;
Eigen::Matrix
glVertex3f(pos(0),pos(1),pos(2));
}
在遍历点云中的点时，存入pcl点云变量。修改后如下：
// 创建点云图
pcl::PointCloud
for(set
{
if((*sit)->isBad())
continue;
Eigen::Matrix
glVertex3f(pos(0),pos(1),pos(2));
// 将点插入点云图
pcl::PointXYZ point;
point.x = pos(0);
point.y = pos(1);
point.z = pos(2);
point_cloud->points.push_back(point);
}
// TEST 保存点云图像到本地
//if(point_cloud->points.size())
// pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);
这里，可以直接将point_cloud变量存储到全局变量中。这里我的处理方法是，修改函数定义，让其返回 pcl::PointCloud
pcl::PointCloud
{
// 创建点云图
pcl::PointCloud
Map* pActiveMap = mpAtlas->GetCurrentMap();
if(!pActiveMap)
return point_cloud;
const vector
const vector
set
if(vpMPs.empty())
return point_cloud;
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(0.0,0.0,0.0);
for(size_t i=0, iend=vpMPs.size(); i { if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])) continue; Eigen::Matrix glVertex3f(pos(0),pos(1),pos(2)); } glEnd(); glPointSize(mPointSize); glBegin(GL_POINTS); glColor3f(1.0,0.0,0.0); for(set { if((*sit)->isBad()) continue; Eigen::Matrix glVertex3f(pos(0),pos(1),pos(2)); // 将点插入点云图 pcl::PointXYZ point; point.x = pos(0); point.y = pos(1); point.z = pos(2); point_cloud->points.push_back(point); } // 保存点云图像到本地 //if(point_cloud->points.size()) // pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud); glEnd(); return point_cloud; } c.修改函数申明 在修改函数返回值后，需要在/include/MapDrawer.h文件中修改函数申明： pcl::PointCloud （如果直接将变量存入全局变量则无需这一步操作） ii.修改Viewer.cc a.添加PCL头文件，引用外部定义的全局变量 #include #include #include #include // 全局变量定义在/src/system.cc // 本文 <四.2.iv.b> 中介绍 extern pcl::PointCloud extern cv::Mat G_imToShow; b.修改 void Viewer::Run() 函数 原始点云显示代码如下，原始的DrawMapPoints函数没有返回值。 if(menuShowPoints) mpMapDrawer->DrawMapPoints(); 将其修改为以下样式，用全局变量G_pcToShow接收返回的点云数据 if(menuShowPoints) G_pcToShow = mpMapDrawer->DrawMapPoints(); // 处理后的点云显示 // TEST //if(G_pcToShow->points.size()) // pcl::io::savePCDFileBinary("orb_slam3.pcd", *G_pcToShow); 原始图片显示代码如下： cv::Mat toShow; cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale); if(both){ cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale); cv::hconcat(im,imRight,toShow); } else{ toShow = im; } if(mImageViewerScale != 1.f){ int width = toShow.cols * mImageViewerScale; int height = toShow.rows * mImageViewerScale; cv::resize(toShow, toShow, cv::Size(width, height)); } cv::imshow("ORB-SLAM3: Current Frame",toShow); cv::waitKey(mT); 修改将其接受图片的变量换为全局变量G_imToShow，修改后如下： cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale); if(both){ cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale); cv::hconcat(im,imRight,G_imToShow); } else{ G_imToShow = im; } if(mImageViewerScale != 1.f){ int width = G_imToShow.cols * mImageViewerScale; int height = G_imToShow.rows * mImageViewerScale; cv::resize(G_imToShow, G_imToShow, cv::Size(width, height)); } cv::imshow("ORB-SLAM3: Current Frame",G_imToShow); // 处理后的视频流显示 cv::waitKey(mT); iii.修改system.h a.添加PCL头文件 #include #include #include #include b.定义一个结构体变量，包含点云、图像、追踪信息 // 利用结构体返回位置、点云、图像（在 namespace ORB_SLAM3 下） typedef struct { pcl::PointCloud cv::Mat image; Sophus::SE3f pos; }SLAM_Output; c.申明新定义的函数 // 返回相机追踪信息、点云数据、图像数据（位置在原TrackMonocular函数之后即可） SLAM_Output TrackMonocularOutput(const cv::Mat &im, const double ×tamp, const vector iv. 修改system.cc a.添加PCL头文件，定义全局变量 #include #include #include #include // 全局变量（用于传出输出点云与输出图像） pcl::PointCloud cv::Mat G_imToShow; b.修改TrackMonocular函数，形成新函数 SLAM_Output System::TrackMonocularOutput(const cv::Mat &im, const double ×tamp, const vector { SLAM_Output data; { unique_lock if(mbShutDown) return data; } // 确保是单目或单目VIO模式 if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR) { cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl; exit(-1); } cv::Mat imToFeed = im.clone(); if(settings_ && settings_->needToResize()){ cv::Mat resizedIm; cv::resize(im,resizedIm,settings_->newImSize()); imToFeed = resizedIm; } // Check mode change { // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱 unique_lock // mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式 if(mbActivateLocalizationMode) { mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper->isStopped()) { usleep(1000); } // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新 mpTracker->InformOnlyTracking(true); // 关闭线程可以使得别的线程得到更多的资源 mbActivateLocalizationMode = false; } if(mbDeactivateLocalizationMode) { mpTracker->InformOnlyTracking(false); mpLocalMapper->Release(); mbDeactivateLocalizationMode = false; } } // Check reset { unique_lock if(mbReset) { mpTracker->Reset(); mbReset = false; mbResetActiveMap = false; } // 如果检测到重置活动地图的标志为true,将重置地图 else if(mbResetActiveMap) { cout << "SYSTEM -> Reseting active map in monocular case" << endl; mpTracker->ResetActiveMap(); mbResetActiveMap = false; } } // 如果是单目VIO模式，把IMU数据存储到队列mlQueueImuData if (mSensor == System::IMU_MONOCULAR) for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) mpTracker->GrabImuData(vImuMeas[i_imu]); // 计算相机位姿 Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename); // 更新跟踪状态和参数 unique_lock mTrackingState = mpTracker->mState; mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; data.pointcloud = G_pcToShow; data.image = G_imToShow; data.pos = Tcw; return data; } c.修改CMakeList.txt 需要再其中添加PCL库，在CMakeList中添加如下三行： ... find_package(PCL REQUIRED) # 添加行 include_directories( ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/include ${PROJECT_SOURCE_DIR}/include/CameraModels ${PROJECT_SOURCE_DIR}/Thirdparty/Sophus ${EIGEN3_INCLUDE_DIR} ${Pangolin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} # 添加行 ) ... target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} ${PCL_LIBRARIES} # 添加行 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lboost_serialization -lcrypto ) 完成后以上步骤后，可以重新编译ORB_SLAM3。 ② 改写ROS应用层 i. 修改ros_mono.cc ROS节点改写后如下： #include #include #include #include #include #include #include #include #include #include #include"../../../include/System.h" #include #include #include #include using namespace std; /***************** #define cfg_NODE_NAME "mono" #define cfg_ROS_IMG_INPUT_TOPIC "/iris_0/realsense/depth_camera/color/image_raw" #define cfg_ROS_IMG_OUTPUT_TOPIC "/orb_slam3/output/image" #define cfg_ROS_PC_OUTPUT_TOPIC "/orb_slam3/output/pointcloud2" #define RED "\e[1;31m" #define YELLOW "\e[1;33m" #define GREEN "\e[1;32m" #define BLUE "\e[1;34m" #define COLOR_TILE "\e[0m" ros::Publisher img_pub; ros::Publisher pc_pub; sensor_msgs::ImagePtr img_msg; sensor_msgs::PointCloud2 pc_msg; /*************** class ImageGrabber { public: ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){} void GrabImage(const sensor_msgs::ImageConstPtr& msg); ORB_SLAM3::System* mpSLAM; // 创建SLMA系统指针 }; int main(int argc, char **argv) { ros::init(argc, argv, cfg_NODE_NAME); ros::start(); if(argc != 3) { cerr << endl << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings" << endl; ros::shutdown(); return 1; } // Create SLAM system. It initializes all system threads and gets ready to process frames. ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true); ImageGrabber igb(&SLAM); ros::NodeHandle nodeHandler; ros::Subscriber sub = nodeHandler.subscribe(cfg_ROS_IMG_INPUT_TOPIC, 1, &ImageGrabber::GrabImage, &igb); img_pub = nodeHandler.advertise pc_pub = nodeHandler.advertise cout << GREEN << "\n\n[INFO] cout << "---------------------------------------------------------------------" << endl; ros::spin(); // Stop all threads SLAM.Shutdown(); // Save camera trajectory SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); ros::shutdown(); return 0; } // ROS接收消息回调函数 void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) { // Copy the ros image message to cv::Mat. cv_bridge::CvImageConstPtr cv_ptr; try { cv_ptr = cv_bridge::toCvShare(msg); } catch (cv_bridge::Exception& e) { ROS_ERROR("[ROS]<%s> cv_bridge r2c exception: %s", cfg_NODE_NAME, e.what()); return; } //mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec()); // SLAM系统追踪单目相机 返回相机位置数据 ORB_SLAM3::SLAM_Output data; data = mpSLAM->TrackMonocularOutput(cv_ptr->image, cv_ptr->header.stamp.toSec()); // 发布IMG到ROS消息 std_msgs::Header header; header.frame_id = "camera"; header.stamp = ros::Time::now(); try { img_msg = cv_bridge::CvImage(header, "rgb8", data.image).toImageMsg(); img_pub.publish(*img_msg); } catch (cv_bridge::Exception& e) { ROS_ERROR("[ROS]<%s> cv_bridge c2r exception: %s", cfg_NODE_NAME, e.what()); return; } // 发布PC到ROS消息 pcl::toROSMsg(*(data.pointcloud), pc_msg); pc_msg.header = header; pc_pub.publish(pc_msg); //cout << data.pos.matrix() << endl << endl; //if((data.pointcloud)->points.size()) //pcl::io::savePCDFileBinary("orb_slam3.pcd", *(data.pointcloud)); //此行会莫名其妙导致运行 } 应用层中，主要是调用了上一步已经修改好的TrackMonocularOutput函数替换原来使用的TrackMonocular函数，并将返回值通过ROS发布。 ii. 修改CMakeList.txt 需要在其中添加PCL库，在CMakeList中添加如下2行： ... find_package(PCL REQUIRED) # 添加行 include_directories( ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/../../../ ${PROJECT_SOURCE_DIR}/../../../include ${PROJECT_SOURCE_DIR}/../../../include/CameraModels ${PROJECT_SOURCE_DIR}/../../../Thirdparty/Sophus ${Pangolin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} # 添加行 ) 一些参考资料： PCL转ROSMSG PointCloud2：ROS与PCL中各种点云数据格式之间的转换（大总结）_pcl::torosmsg-CSDN博客文章浏览阅读2.3k次，点赞11次，收藏63次。ROS与PCL中各种点云数据格式之间的转换（大总结）三种常用点云数据格式：pcl::PointCloud< PointT>pcl::PCLPointCloud2snesor_msgs::PointCloud21.sensor_msgs::PointCloud2转pcl::PCLPointCloud2pcl_conversion::toPCl(sensor_msgs::PointCloud2,pcl::PCLPointCloud2)2.sensor_msgs::PointClo_pcl::torosmsghttps://blog.csdn.net/m0_45388819/article/details/113794706OpenCV转ROSMSG Image：利用opencv将本地图片转换成ROS格式_opencv 图像类型转换为rosmsg类型-CSDN博客文章浏览阅读884次。转自:http://blog.csdn.net/yake827/article/details/44593621本文主要讲解如何将本地的图片通过ROS来显示出来。主要利用了OpenCV库，一样是来源于ROS官网.创建一个ROS工作区工作区还是存放和编译我们的文件[plain] view plain copy$ mkdi_opencv 图像类型转换为rosmsg类型https://blog.csdn.net/mxgsgtc/article/details/72143054将ORBSLAM中的Map转换为点云：ORB SLAM3 点云地图保存_orbslam3 点云-CSDN博客文章浏览阅读5.1k次，点赞17次，收藏90次。简单修改ORB_SLAM3源码， 使其能保存pcd格式的点云地图。_orbslam3 点云https://blog.csdn.net/qq_43591054/article/details/125693886 3. 运行 使用 <三、3> 介绍的步骤运行即可，可以在其他节点接收点云消息和图像消息。 正常运行后，可以通过rqt_image_view来查看图像数据是否正确发布： 可以通过 rostopic echo / rostopic hz 来查看点云数据是否正确发布。如需要通过rviz来查看点云数据是否正确，则需要新增一个tf转换（上面代码中，点云的frame_id为camera）。 改写双目与RGB-D方法与单目相同，因此不再赘述。 五、附录 附录一：测试使用的无人机模型文件 iris_all_sensor.sdf 0.1
0
10.0
0
10.0
0
model.config
This is a model of the 3DR Iris Quadrotor for Zhihangcup competition.
附录二：无人机测试环境及launch文件
map_maze_target.world
map_maze_target.launch
参考文章
您阅读本篇文章共花了：
发表评论