文章目录
- [0 引言](#0 引言)
- [1 D435i相机配置](#1 D435i相机配置)
- [2 新增发布双目位姿功能](#2 新增发布双目位姿功能)
-
- [2.1 新增d435i_stereo.cc代码](#2.1 新增d435i_stereo.cc代码)
- [2.2 修改CMakeLists.txt](#2.2 修改CMakeLists.txt)
- [2.3 新增配置文件D435i.yaml](#2.3 新增配置文件D435i.yaml)
- [3 编译运行和结果](#3 编译运行和结果)
-
- [3.1 编译运行](#3.1 编译运行)
- [3.2 结果](#3.2 结果)
- [3.3 可能出现的问题](#3.3 可能出现的问题)
0 引言
ORB-SLAM2学习笔记1已成功编译安装ROS
版本ORB-SLAM2
到本地,以及ORB-SLAM2学习笔记5成功用EuRoc、TUM、KITTI
开源数据来运行ROS
版ORB-SLAM2
,并生成轨迹。但实际ROS
视觉SLAM
工程落地时,一般搭配传感器实时发出位姿pose
的rostopic
,本篇就以D435i
相机的双目IR
相机作为输入,运行ROS
版ORB-SLAM2
,最后发出pose
的rostopic
。
👉 ORB-SLAM2 github: https://github.com/raulmur/ORB_SLAM2
本文系统环境:
- Ubuntu18.04
- ROS-melodic
- ROS版ORB-SLAM2
- D435i相机和驱动
1 D435i相机配置
默认已在Ubuntu18.04
系统上安装ROS
版的D435i
相机驱动,比如本文驱动安装目录~/catkin_rs/src/realsense-ros
安装后,默认是不开双目IR
相机,需要自行修改配置:
bash
# 激活环境
source /catkin_rs/devel/setup.bash
# roscd 进入到配置文件目录下
roscd realsense2_camera/launch/
# 打开 rs_camera.launch 配置文件进行修改
vim rs_camera.launch
打开后,主要是如下的字段需要修改成 true
,这样就能打开双目IR
相机,分辨率也可自行修改。
xml
<arg name="infra_width" default="848"/>
<arg name="infra_height" default="480"/>
<arg name="enable_infra" default="true"/>
<arg name="enable_infra1" default="true"/>
<arg name="enable_infra2" default="true"/>
...
2 新增发布双目位姿功能
2.1 新增d435i_stereo.cc代码
在ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/
目录下新增d435i_stereo.cc
代码文件,如下代码片段来增加:
cpp
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<tf/transform_broadcaster.h>
#include "../../../include/Converter.h"
#include <nav_msgs/Path.h>
#include <ros/ros.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"
using namespace std;
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);
ORB_SLAM2::System* mpSLAM;
bool do_rectify;
cv::Mat M1l,M2l,M1r,M2r;
};
ros::Publisher pose_pub;
nav_msgs::Path stereo_path;
ros::Publisher stereo_path_pub;
int main(int argc, char **argv)
{
ros::init(argc, argv, "RGBD");
ros::start();
if(argc != 4)
{
cerr << endl << "Usage: rosrun ORB_SLAM2 Stereo path_to_vocabulary path_to_settings do_rectify" << endl;
ros::shutdown();
return 1;
}
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);
ImageGrabber igb(&SLAM);
stringstream ss(argv[3]);
ss >> boolalpha >> igb.do_rectify;
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);
}
ros::NodeHandle nh;
//message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);
//message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "camera/right/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/infra1/image_rect_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/infra2/image_rect_raw", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));
pose_pub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5);
stereo_path_pub = nh.advertise<nav_msgs::Path>("ORB_SLAM/path",10);
ros::spin();
// Stop all threads
SLAM.Shutdown();
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");
ros::shutdown();
return 0;
}
void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptrLeft;
try
{
cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv_bridge::CvImageConstPtr cv_ptrRight;
try
{
cv_ptrRight = cv_bridge::toCvShare(msgRight);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
if(do_rectify)
{
cv::Mat imLeft, imRight;
cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR);
mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()).clone();
}
else
{
cv::Mat Tcw;
Tcw = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id ="path";
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);
tf::Transform new_transform;
new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
new_transform.setRotation(quaternion);
tf::poseTFToMsg(new_transform, pose.pose);
pose_pub.publish(pose);
stereo_path.header.frame_id="path";
stereo_path.header.stamp=ros::Time::now();
stereo_path.poses.push_back(pose);
stereo_path_pub.publish(stereo_path);
}
}
上述代码已经写入了D435i
相机双目IR
相机发出的topic
,分别是左目/camera/infra1/image_rect_raw
,右目/camera/infra2/image_rect_raw
;发布的位姿pose
的topic
是ORB_SLAM/pose
,如果用的不是D435i
,比如zed双目相机,可以自行修改。
2.2 修改CMakeLists.txt
由于新增了发布功能的代码文件,那对应的CMakeLists.txt也需要新增对应的编译和链接的设置,如下所示,在ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt
文件的结尾新增:
cmake
# Node for d435i_stereo camera
# 设置了编译的代码文件`d435i_stereo.cc`和可执行文件的名字
rosbuild_add_executable(D435i_Stereo
src/d435i_stereo.cc
)
target_link_libraries(D435i_Stereo
${LIBS}
)
2.3 新增配置文件D435i.yaml
同时也要新增对应的配置文件D435i.yaml
,可新增到ORB_SLAM2/Examples/Stereo/D435i.yaml
,文件类似ORB_SLAM2/Examples/Stereo/EuRoC.yaml
,如下所示,主要修改第一部分的内参部分(fx,fy,cx,cy
)即可,相机的内参获取方法,可通过roslaunch realsense2_camera rs_camera.launch
启动相机后,再通过rostopic echo /camera/infra1/camera_info
来获取。
yaml
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 427.03680419921875
Camera.fy: 427.03680419921875
Camera.cx: 427.3993835449219
Camera.cy: 236.4639129638672
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.width: 848
Camera.height: 480
# Camera frames per second
Camera.fps: 15.0
# stereo baseline times fx
Camera.bf: 50.0
# 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: 35
#--------------------------------------------------------------------------------------------
# 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: 480
LEFT.width: 752
LEFT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
LEFT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
LEFT.R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]
RIGHT.height: 480
RIGHT.width: 752
RIGHT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
RIGHT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
RIGHT.R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 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
3 编译运行和结果
3.1 编译运行
全部修改后,可回到ORB_SLAM2
工程目录下,重新执行命令进行编译:
bash
# chmod 之前执行过可忽略
chmod +x build_ros.sh
./build_ros.sh
编译完成后,首先连接D435i
相机到电脑上(USB3.0
),然后执行命令启动D435i
相机:
bash
source /catkin_rs/devel/setup.bash
roslaunch realsense2_camera rs_camera.launch
然后再新开终端,执行D435i_Stereo
:
bash
# ORB_SLAM2工程目录下
rosrun ORB_SLAM2 D435i_Stereo Vocabulary/ORBvoc.txt Examples/Stereo/D435i.yaml false
3.2 结果
执行上述命令后,在加载完词袋后,会自动打开两个可视化界面:
ORB-SLAM2: Current Frame
ORB-SLAM2: Map Viewer
可以用rostopic list
可查看到已经发出的位姿topic
:
bash
/ORB_SLAM/path
/ORB_SLAM/pose
也可以用rostopic echo /ORB_SLAM/pose
查看具体的位姿信息:
bash
header:
seq: 3287
stamp:
secs: 0
nsecs: 0
frame_id: "path"
pose:
position:
x: 0.0335485860705
y: -0.0102641582489
z: -0.0411500893533
orientation:
x: -0.042415473676
y: -0.00852415898276
z: -0.015283392766
w: 0.998946787478
至此,成功用D435i
相机的双目IR
相机作为输入,运行ROS
版ORB-SLAM2
,最后发出pose
的rostopic
。
3.3 可能出现的问题
问题1:
如果如下所示的问题,启动后很快自动关闭,可能是特征点太少的原因,调整相机的朝向,保证相机视野范围内多一点特征:
bash
terminate called after throwing an instance of 'cv::Exception'
what(): /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matrix.cpp:483: error: (-215) 0 <= _rowRange.start && _rowRange.start <= _rowRange.end && _rowRange.end <= m.rows in function Mat
Aborted (core dumped)
Reference:
须知少时凌云志,曾许人间第一流。
⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔