做百度还是阿里网站好,php做视频网站源码,快递公司网站源码,公司网站开发项目文章目录 0 引言1 D435i相机配置2 新增发布双目位姿功能2.1 新增d435i_stereo.cc代码2.2 修改CMakeLists.txt2.3 新增配置文件D435i.yaml 3 编译运行和结果3.1 编译运行3.2 结果3.3 可能出现的问题 0 引言
ORB-SLAM2学习笔记1已成功编译安装ROS版本ORB-SLAM2到本地#xff0c… 文章目录 0 引言1 D435i相机配置2 新增发布双目位姿功能2.1 新增d435i_stereo.cc代码2.2 修改CMakeLists.txt2.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.04ROS-melodicROS版ORB-SLAM2D435i相机和驱动
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 nameinfra_width default848/arg nameinfra_height default480/arg nameenable_infra defaulttrue/arg nameenable_infra1 defaulttrue/arg nameenable_infra2 defaulttrue/
...2 新增发布双目位姿功能
2.1 新增d435i_stereo.cc代码
在ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/目录下新增d435i_stereo.cc 代码文件如下代码片段来增加
#includeiostream
#includealgorithm
#includefstream
#includechrono#includetf/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#includeopencv2/core/core.hpp#include../../../include/System.husing 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_l0 || rows_r0 || cols_l0 || cols_r0){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::Subscribersensor_msgs::Image left_sub(nh, /camera/left/image_raw, 1);//message_filters::Subscribersensor_msgs::Image right_sub(nh, camera/right/image_raw, 1);message_filters::Subscribersensor_msgs::Image left_sub(nh, /camera/infra1/image_rect_raw, 1);message_filters::Subscribersensor_msgs::Image right_sub(nh, /camera/infra2/image_rect_raw, 1);typedef message_filters::sync_policies::ApproximateTimesensor_msgs::Image, sensor_msgs::Image sync_pol;message_filters::Synchronizersync_pol sync(sync_pol(10), left_sub,right_sub);sync.registerCallback(boost::bind(ImageGrabber::GrabStereo,igb,_1,_2));pose_pub nh.advertisegeometry_msgs::PoseStamped(ORB_SLAM/pose, 5);stereo_path_pub nh.advertisenav_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 informationvectorfloat q ORB_SLAM2::Converter::toQuaternion(Rwc);tf::Transform new_transform;new_transform.setOrigin(tf::Vector3(twc.atfloat(0, 0), twc.atfloat(0, 1), twc.atfloat(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_idpath;stereo_path.header.stampros::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 false3.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::Exceptionwhat(): /build/opencv-L2vuMj/opencv-3.2.0dfsg/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 须知少时凌云志曾许人间第一流。 ⭐️
