目前我们已经搭建了完整的环境
现在我们的目标是：利用zed2在ros平台上跑通ORB-SLAM3
但是ORB-SLAM3中并没有提供zed2的配置文件，在ORB-SLAM3的官方github上ros的示例文件中没有相关的操作。执行./build_ros.sh也无效，并没有生成ros下的可执行文件。我也做过把ORB-SLAM3完整的库完全放在ros的工作空间中，并没有什么卵用。
这个原因是因为作者ORB_SLAM3/下的CMakeLists.txt中并没有ros包可执行文件生成的操作。作者把ros的相关配置都放在了/Examples_old/ROS/ORB_SLAM3文件下了
步入正文：一步一步脚印来
- 添加zed的配置文件
cd ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3/src
touch zed2_stereo_inertial.cc
touch zed2_stereo_inertial.yaml
在zed2_stereo_inertial.cc文件中添加以下信息
/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<vector>
#include<queue>
#include<thread>
#include<mutex>
#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/Imu.h>
#include<opencv2/core/core.hpp>
#include"../../../include/System.h"
#include"include/ImuTypes.h"
using namespace std;
class ImuGrabber
{
public:
ImuGrabber(){};
void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);
queue<sensor_msgs::ImuConstPtr> imuBuf;
std::mutex mBufMutex;
};
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe){}
void GrabImageLeft(const sensor_msgs::ImageConstPtr& msg);
void GrabImageRight(const sensor_msgs::ImageConstPtr& msg);
cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
void SyncWithImu();
queue<sensor_msgs::ImageConstPtr> imgLeftBuf, imgRightBuf;
std::mutex mBufMutexLeft,mBufMutexRight;
ORB_SLAM3::System* mpSLAM;
ImuGrabber *mpImuGb;
const bool do_rectify;
cv::Mat M1l,M2l,M1r,M2r;
const bool mbClahe;
cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "Stereo_Inertial");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
bool bEqual = false;
if(argc < 4 || argc > 5)
{
cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl;
ros::shutdown();
return 1;
}
std::string sbRect(argv[3]);
if(argc==5)
{
std::string sbEqual(argv[4]);
if(sbEqual == "true")
bEqual = true;
}
// 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::IMU_STEREO,true);
ImuGrabber imugb;
ImageGrabber igb(&SLAM,&imugb,sbRect == "true",bEqual);
if(igb.do_rectify)
{
// Load settings related to stereo calibration
cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "ERROR: Wrong path to settings" << endl;
return -1;
}
cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
{
cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
return -1;
}
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);
}
// Maximum delay, 5 seconds
//ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb);
//ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb);
//ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb);
ros::Subscriber sub_imu = n.subscribe("/zed/zed_node/imu/data", 1000, &ImuGrabber::GrabImu, &imugb);
ros::Subscriber sub_img_left = n.subscribe("/zed/zed_node/left/image_rect_color", 100, &ImageGrabber::GrabImageLeft,&igb);
ros::Subscriber sub_img_right = n.subscribe("/zed/zed_node/right/image_rect_color", 100, &ImageGrabber::GrabImageRight,&igb);
std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);
ros::spin();
return 0;
}
void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg)
{
mBufMutexLeft.lock();
if (!imgLeftBuf.empty())
imgLeftBuf.pop();
imgLeftBuf.push(img_msg);
mBufMutexLeft.unlock();
}
void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg)
{
mBufMutexRight.lock();
if (!imgRightBuf.empty())
imgRightBuf.pop();
imgRightBuf.push(img_msg);
mBufMutexRight.unlock();
}
cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
if(cv_ptr->image.type()==0)
{
return cv_ptr->image.clone();
}
else
{
std::cout << "Error type" << std::endl;
return cv_ptr->image.clone();
}
}
void ImageGrabber::SyncWithImu()
{
const double maxTimeDiff = 0.01;
while(1)
{
cv::Mat imLeft, imRight;
double tImLeft = 0, tImRight = 0;
if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!mpImuGb->imuBuf.empty())
{
tImLeft = imgLeftBuf.front()->header.stamp.toSec();
tImRight = imgRightBuf.front()->header.stamp.toSec();
this->mBufMutexRight.lock();
while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1)
{
imgRightBuf.pop();
tImRight = imgRightBuf.front()->header.stamp.toSec();
}
this->mBufMutexRight.unlock();
this->mBufMutexLeft.lock();
while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1)
{
imgLeftBuf.pop();
tImLeft = imgLeftBuf.front()->header.stamp.toSec();
}
this->mBufMutexLeft.unlock();
if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff)
{
// std::cout << "big time difference" << std::endl;
continue;
}
if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec())
continue;
this->mBufMutexLeft.lock();
imLeft = GetImage(imgLeftBuf.front());
imgLeftBuf.pop();
this->mBufMutexLeft.unlock();
this->mBufMutexRight.lock();
imRight = GetImage(imgRightBuf.front());
imgRightBuf.pop();
this->mBufMutexRight.unlock();
vector<ORB_SLAM3::IMU::Point> vImuMeas;
mpImuGb->mBufMutex.lock();
if(!mpImuGb->imuBuf.empty())
{
// Load imu measurements from buffer
vImuMeas.clear();
while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tImLeft)
{
double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));
mpImuGb->imuBuf.pop();
}
}
mpImuGb->mBufMutex.unlock();
if(mbClahe)
{
mClahe->apply(imLeft,imLeft);
mClahe->apply(imRight,imRight);
}
if(do_rectify)
{
cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR);
}
mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas);
std::chrono::milliseconds tSleep(1);
std::this_thread::sleep_for(tSleep);
}
}
}
void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
{
mBufMutex.lock();
imuBuf.push(imu_msg);
mBufMutex.unlock();
return;
}
在zed2_stereo_inertial.yaml中添加以下信息
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"
# Camera calibration and distortion parameters (OpenCV) (equal for both cameras after stereo rectification)
Camera.fx: 528.3009033203125
Camera.fy: 528.3009033203125
Camera.cx: 632.7931518554688
Camera.cy: 372.5525817871094
# 用的是校正过的节点，所以畸变参数设置为0
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.width: 1280
Camera.height: 720
# Camera frames per second
Camera.fps: 15.0
# stereo baseline times fx
Camera.bf: 63.396108984375
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Close/Far threshold. Baseline times.
ThDepth: 40.0 # 35
# Transformation from camera 0 to body-frame (imu)
# 从左目转换到IMU坐标系
Tbc: !!opencv-matrix
rows: 4
cols: 4
dt: f
data: [ 0.0055827285742915, 0.0128040922714603, 0.9999024394223516, 0.0285440762197234,
-0.9999801332587812, 0.0029981004108222, 0.0055447706603969, -0.1038871459045697,
-0.0029268121592544, -0.9999135295689473, 0.0128205754767047, -0.0063514683297355,
0.0000000000000000, -0.0000000000000000, -0.0000000000000000, 1.0000000000000000]
# IMU noise
# get it from Project of **zed-examples/tutorials/tutorial 7 - sensor data/**.
IMU.NoiseGyro: 0.007 # 1.6968e-04
IMU.NoiseAcc: 0.0016 # 2.0000e-3
IMU.GyroWalk: 0.0019474
IMU.AccWalk: 0.0002509 # 3.0000e-3
IMU.Frequency: 400
#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 720
LEFT.width: 1280
LEFT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data: [0, 0, 0, 0, 0]
LEFT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [528.3009033203125, 0.0, 632.7931518554688, 0.0, 528.3009033203125, 372.5525817871094, 0.0, 0.0, 1.0]
LEFT.R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
LEFT.Rf: !!opencv-matrix
rows: 3
cols: 3
dt: f
data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
LEFT.P: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [528.3009033203125, 0.0, 632.7931518554688, 0.0, 0.0, 528.3009033203125, 372.5525817871094, 0.0, 0.0, 0.0, 1.0, 0.0]
RIGHT.height: 720
RIGHT.width: 1280
RIGHT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data: [0, 0, 0, 0, 0]
RIGHT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [528.3009033203125, 0.0, 632.7931518554688, 0.0, 528.3009033203125, 372.5525817871094, 0.0, 0.0, 1.0]
RIGHT.R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
RIGHT.P: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [528.3009033203125, 0.0, 632.7931518554688, -63.47084045410156, 0.0, 528.3009033203125, 372.5525817871094, 0.0, 0.0, 0.0, 1.0, 0.0]
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1200
# 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
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
说明这里的zed2_stereo_inertial.yaml文件是zed2相机的参数文件，但是每一个相机或多或少会有一点差异，所以这里的参数应该改成自己的相机参数，这一部分我暂时还有做，准备在下一片博客中跟大家一起交流
在zed2_stereo_inertial.cc中修改以下内容：大约在145行左右，
将以下的内容
ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb);
ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb);
ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb);
变成
ros::Subscriber sub_imu = n.subscribe("/zed/zed_node/imu/data", 1000, &ImuGrabber::GrabImu, &imugb);
ros::Subscriber sub_img_left = n.subscribe("/zed/zed_node/left/image_rect_color", 100, &ImageGrabber::GrabImageLeft,&igb);
ros::Subscriber sub_img_right = n.subscribe("/zed/zed_node/right/image_rect_color", 100, &ImageGrabber::GrabImageRight,&igb);
注意：这里的topic要对应
如果不确定可以运行三个终端
第一个终端运行：
cd catkin_ws
source ./devel/setup.bash
roscore
第二个终端运行
cd catkin_ws
source ./devel/setup.bash
roslaunch zed_wrapper zed2.launch
第三个终端运行
rostopic list
在第三个终端下面查看对应的topic到底是什么，然后输入进去
2.编译CMakeLists.txt，生成可执行文件
首先编辑CMakeLists.txt文件
cd ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3
gedit CMakeLists.txt
在最下面添加
rosbuild_add_executable(zed2_stereo_inertial
src/zed2_stereo_inertial.cc
)
target_link_libraries(zed2_stereo_inertial
${LIBS}
)
然后编译
cd ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3
mkdir build
cd build
cmake ..
make -j8
sudo make install
这时候会发现ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3下面多了一个可执行文件zed2_stereo_inertial
3.添加ROS下面的ORB-SLAM3的默认工作路径
gedit ~/.bashrc
在最下面添加
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:{ 你的路径 }/ORB_SLAM3-master/Examples_old/ROS
4.运行ORB-SLAM3
打开第一个终端
cd ~/catkin_ws
source ./devel/setup.bash
roscore
打开第二个终端
cd ~/catkin_ws
source ./devel/setup.bash //不知道为什么不执行这一步就无法找到zed.launch
roslaunch zed_wrapper zed.launch
打开第三个终端
cd ~/catkin_ws/src/ORB_SLAM3
rosrun ORB_SLAM3 zed2_stereo_inertial Vocabulary/ORBvoc.txt Examples_old/ROS/ORB_SLAM3/src/zed2_stereo_inertial.yaml false
建议
Vocabulary/ORBvoc.txt Examples_old/ROS/ORB_SLAM3/src/zed2_stereo_inertial.yaml 写成自己的绝对路径
我自己的是：
rosrun ORB_SLAM3 ZED_Stereo_IMU ~/Downloads/ORB_SLAM3-master/Vocabulary/ORBvoc.txt ~/Downloads/ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3/src/zed2.yaml false
运行效果：
https://www.bilibili.com/video/BV1U44y1g78B?spm_id_from=333.999.0.0
如果遇到not enough accelerator 试试走起来试试，不要在原地不动就好了
如果遇到问题，欢迎大家和我交流
下一篇我将跟大家交流一下
跑丢，标定，not imu meas等问题。
文章中参考了以下资料:
https://editor.csdn.net/md/?articleId=124301452
https://github.com/stereolabs/zed-ros-wrapper/issues/620
https://blog.csdn.net/slender_1031/article/details/115030053
747
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
