ORB-SLAM2学习笔记6之D435i双目IR相机运行ROS版ORB-SLAM2并发布位姿pose的rostopic

文章目录

  • 0 引言
  • 1 D435i相机配置
  • 2 新增发布双目位姿功能
    • 2.1 新增d435i_stereo.cc代码
    • 2.2 修改CMakeLists.txt
    • 2.3 新增配置文件D435i.yaml
  • 3 编译运行和结果
    • 3.1 编译运行
    • 3.2 结果
    • 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相机，需要自行修改配置：

# 激活环境
source /catkin_rs/devel/setup.bash
# roscd 进入到配置文件目录下
roscd realsense2_camera/launch/
# 打开 rs_camera.launch 配置文件进行修改
vim rs_camera.launch

打开后，主要是如下的字段需要修改成 true，这样就能打开双目IR相机，分辨率也可自行修改。

  <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 代码文件，如下代码片段来增加：

#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 calibrationcv::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 threadsSLAM.Shutdown();// Save camera trajectorySLAM.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 informationcv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation informationvector<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 文件的结尾新增：

# 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: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.4639129638672Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0Camera.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-matrixrows: 1cols: 5dt: ddata:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
LEFT.K: !!opencv-matrixrows: 3cols: 3dt: ddata: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrixrows: 3cols: 3dt: ddata: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P:  !!opencv-matrixrows: 3cols: 4dt: ddata: [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-matrixrows: 1cols: 5dt: ddata:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
RIGHT.K: !!opencv-matrixrows: 3cols: 3dt: ddata: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
RIGHT.R:  !!opencv-matrixrows: 3cols: 3dt: ddata: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P:  !!opencv-matrixrows: 3cols: 4dt: ddata: [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工程目录下，重新执行命令进行编译：

# chmod 之前执行过可忽略
chmod +x build_ros.sh
./build_ros.sh

编译完成后，首先连接D435i相机到电脑上（USB3.0），然后执行命令启动D435i相机：

source /catkin_rs/devel/setup.bash
roslaunch realsense2_camera rs_camera.launch

然后再新开终端，执行D435i_Stereo：

# 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 :

/ORB_SLAM/path
/ORB_SLAM/pose

也可以用rostopic echo /ORB_SLAM/pose查看具体的位姿信息：

header: seq: 3287stamp: secs: 0nsecs:         0frame_id: "path"
pose: position: x: 0.0335485860705y: -0.0102641582489z: -0.0411500893533orientation: x: -0.042415473676y: -0.00852415898276z: -0.015283392766w: 0.998946787478

至此，成功用D435i相机的双目IR相机作为输入，运行ROS版ORB-SLAM2，最后发出pose的rostopic。

3.3 可能出现的问题

问题1：

如果如下所示的问题，启动后很快自动关闭，可能是特征点太少的原因，调整相机的朝向，保证相机视野范围内多一点特征：

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 MatAborted (core dumped)

Reference:

  • https://github.com/raulmur/ORB_SLAM2



须知少时凌云志，曾许人间第一流。



⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.hqwc.cn/news/53075.html

如若内容造成侵权/违法违规/事实不符，请联系编程知识网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

LAXCUS分布式操作系统引领科技潮流，进入百度首页

LAXCUS分布式操作系统引领科技潮流，进入百度首页

信息源自某家网络平台&#xff0c;以下原样摘抄贴出。 随着科技的飞速发展&#xff0c;分布式操作系统做为通用基础平台&#xff0c;为大数据、高性能计算、人工智能提供了强大的数据和算力支持&#xff0c;已经成为了当今计算机领域的研究热点。近日&#xff0c;一款名为LAXCU…
阅读更多...
Mybatis引出的一系列问题-JDBC 的探究

Mybatis引出的一系列问题-JDBC 的探究

1 引入对JDBC的理解-1 一般来说&#xff0c;Java应用程序访问数据库的过程是&#xff1a; 装载数据库驱动程序&#xff1b;通过jdbc建立数据库连接&#xff1b;访问数据库&#xff0c;执行sql语句&#xff1b;断开数据库连接。 Public void FindAllUsers(){//1、装载sqlserve…
阅读更多...
开放式耳机的音质不如入耳式耳机吗？开放式耳机的优缺点？

开放式耳机的音质不如入耳式耳机吗？开放式耳机的优缺点？

​开放式耳机的音质不一定不如入耳式耳机。音质取决于多种因素&#xff0c;包括耳机的设计、音频技术和材料质量等。因此&#xff0c;不能简单地将开放式耳机和入耳式耳机进行比较&#xff0c;并得出开放式耳机的音质不如入耳式的结论。不同的耳机类型都有各自的优势和劣势&…
阅读更多...
vue+element中如何设置单个el-date-picker开始时间和结束时间关联

vue+element中如何设置单个el-date-picker开始时间和结束时间关联

功能&#xff1a;选了开始时间&#xff0c;则结束时间只能选择开始时间之后的&#xff1b;选了结束时间&#xff0c;则开始时间只能选择结束时间之前的 重点是picker-options属性 图示&#xff1a; 代码展示: // body 内部<el-form-item><el-date-pickerv-model&qu…
阅读更多...
面试题总结

面试题总结

文章目录 第一阶段：网络1、osi七层模型、tcp\ip 五层模型2、三次握手四次挥手3、交换机路由器工作原理4、vlan的作用5、icmp协议Linux1、cpu、内存、io、磁盘容量、网络流量、load average2、lvm逻辑卷如何创建3、raid磁盘阵列4、开机引导过程5、软连接硬链接6、查找文件命令7…
阅读更多...
在windows下安装ruby使用gem

在windows下安装ruby使用gem

在windows下安装ruby使用gem 1.下载安装ruby环境2.使用gem3.gem换源 1.下载安装ruby环境 ruby下载地址 选择合适的版本进行下载和安装&#xff1a; 在安装的时候&#xff0c;请勾选Add Ruby executables to your PATH这个选项&#xff0c;添加环境变量&#xff1a; 安装Ruby成…
阅读更多...
【css】nth-child选择器实现表格的斑马纹效果

【css】nth-child选择器实现表格的斑马纹效果

nth-child() 选择器可以实现为所有偶数&#xff08;或奇数&#xff09;的表格行添加css样式&#xff0c;even&#xff1a;偶数&#xff0c;odd&#xff1a;奇数。 代码&#xff1a; <style> table {border-collapse: collapse;width: 100%; }th, td {text-align: cente…
阅读更多...
安全测试国家标准解读——并发程序安全

安全测试国家标准解读——并发程序安全

本系列文章主要围绕《GB/T 38674—2020 信息安全技术 应用软件安全编程指南》进行讲解&#xff0c;该标准是2020年4月28日&#xff0c;由国家市场监督管理总局、国家标准化管理委员会发布&#xff0c;2020年11月01日开始实施。我们对该标准中一些常见的漏洞进行了梳理&#xff…
阅读更多...
D. Professor Higashikata(并查集)

D. Professor Higashikata(并查集)

题目链接&#xff1a;https://codeforces.com/contest/1847/problem/D 思路:对于同一个位置&#xff0c;后出现的是无意义的&#xff0c;所以只需要找到每个位置第一次出现的次序&#xff0c;暴力写是的&#xff0c;可以转化为区间覆盖模型&#xff0c;这是并查集的一个经典应…
阅读更多...
CI/CD持续集成持续发布(jenkins)

CI/CD持续集成持续发布(jenkins)

1.背景 在实际开发中&#xff0c;我们经常要一边开发一边测试&#xff0c;当然这里说的测试并不是程序员对自己代码的单元测试&#xff0c;而是同组程序员将代码提交后&#xff0c;由测试人员测试&#xff1b; 或者前后端分离后&#xff0c;经常会修改接口&#xff0c;然后重新…
阅读更多...
三、JVM-如何判断对象已死问题

三、JVM-如何判断对象已死问题

内存模型以及如何判定对象已死问题 体验与验证 2.4.5.1 使用visualvm visualgc插件下载链接 &#xff1a;https://visualvm.github.io/pluginscenters.html 选择对应JDK版本链接—>Tools—>Visual GC 若上述链接找不到合适的&#xff0c;大家也可以自己在网上下载对应…
阅读更多...
【Spring Cloud 六】Hystrix熔断

【Spring Cloud 六】Hystrix熔断

这里写目录标题 系列文章目录背景一、Hystrix是什么服务雪崩服务容错的相关概念熔断器降级超时控制限流 二、会什么要有Hystrix三、如何使用Hystrix进行熔断处理整体项目代码服务提供者pom文件yml配置文件启动类controller 服务消费者pom文件yml配置文件启动类feignhystrixcont…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023