ubuntu20.04+ROS noetic在线运行单USB双目ORB_SLAM

双目摄像头主要有以下几种，各有优缺点。

  • 1.单USB插口，左右图像单独输出
  • 2.双USB插口，左右图像单独输出（可能存在同步性问题）
  • 3.双USB插口，左右图像合成输出
  • 4.单USB插口，左右图像合成输出

官方版本的ORB SLAM2编译运行参考之前记录的博客

虽然在ubuntu22.04上编译和运行的，但我后来在ubuntu20.04上编译和运行，报错也都差不多，主要是OpenCV的版本问题，由于需要使用ROS在线运行，不建议使用OpenCV3，直接先安装ROS noetic，其自带OpenCV4.2.0版本，可不用自己再编译安装。

一、相机话题拆分

我的双目相机是单USB合成图像，然而ORM SLAM2双目ROS在订阅相机话题时，订阅的是左右图像两个节点，因此需要对USB相机话题进行拆封。

参考：
1. ROS调用USB双目摄像头模组
2. ROS&OpenCV下单目和双目摄像头的标定与使用

1. ROS调用自己的双目USB相机

安装usb_cam包

sudo apt install ros-noetic-usb-cam*

查看摄像头占用usb串口号(插上USB查看一次，拔掉USB再查看一次，可确定串口号)

ls /dev/video*

启动launch文件

cd /opt/ros/noetic/share/usb_cam/launch/
sudo gedit usb_cam-test.launch

在这里插入图片描述

修改如上红框几个地方，主要有usb串口号、摄像头分辨率，以及摄像头的像素格式。默认分辨率是640x480，默认像素格式是yuyv，如果不修改的的话可能显示是花的，根据自己的相机修改即可。另外，同一个串口在关机重启后可能会发生变化，如果不显示，查询以后更改即可。

打开双目摄像头

roslaunch usb_cam usb_cam-test.launch

在这里插入图片描述
查看topic

rostopic list

在这里插入图片描述
相机只有一个/usb_cam/image_raw的话题

2. 分割双目相机图像，拆分rostopic

主要思路就是首先启动usb相机，然后新建camera_split节点，该节点订阅usb_cam/image_raw，然后分割双目相机图像，发布左图像和右图像两个节点。
在这里插入图片描述
创建工作空间并初始化(个人习惯放在Documents文件夹下)

mkdir -p catkin_ws/src 
cd catkin_ws 
catkin_make

进入src创建ROS包并添加依赖

cd src
catkin_create_pkg camera_split cv_bridge image_transport roscpp sensor_msgs std_msgs camera_info_manager

修改camera_split包的CMakeLists.txt文件,修改include_directories

find_package(OpenCV 4.2.0 REQUIRED)
#修改include_directories:
include_directories (${catkin_INCLUDE_DIRS}${OpenCV_INCLUDE_DIRS}
)
#添加可执行文件
add_executable(camera_split_node src/camera_split.cpp )
#指定链接库
target_link_libraries(camera_split_node${catkin_LIBRARIES}${OpenCV_LIBRARIES}
)

创建源代码文件camera_split.cpp

#include <ros/ros.h>
#include <iostream>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <camera_info_manager/camera_info_manager.h>#include <opencv2/opencv.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/highgui/highgui.hpp>using namespace std;class CameraSplitter
{
public:CameraSplitter():nh_("~"),it_(nh_){image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, &CameraSplitter::imageCallback, this);image_pub_left_ = it_.advertiseCamera("/left_cam/image_raw", 1);image_pub_right_ = it_.advertiseCamera("/right_cam/image_raw", 1);cinfo_ =boost::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nh_));//读取参数服务器参数，得到左右相机参数文件的位置string left_cal_file = nh_.param<std::string>("left_cam_file", "");string right_cal_file = nh_.param<std::string>("right_cam_file", "");if(!left_cal_file.empty()){if(cinfo_->validateURL(left_cal_file)) {cout<<"Load left camera info file: "<<left_cal_file<<endl;cinfo_->loadCameraInfo(left_cal_file);ci_left_ = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));}else {cout<<"Can't load left camera info file: "<<left_cal_file<<endl;ros::shutdown();}}else {cout<<"Did not specify left camera info file." <<endl;ci_left_=sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());}if(!right_cal_file.empty()){if(cinfo_->validateURL(right_cal_file)) {cout<<"Load right camera info file: "<<right_cal_file<<endl;cinfo_->loadCameraInfo(right_cal_file);ci_right_ = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));}else {cout<<"Can't load right camera info file: "<<left_cal_file<<endl;ros::shutdown();}}else {cout<<"Did not specify right camera info file." <<endl;ci_right_=sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());}}void imageCallback(const sensor_msgs::ImageConstPtr& msg){cv_bridge::CvImageConstPtr cv_ptr;namespace enc= sensor_msgs::image_encodings;cv_ptr= cv_bridge::toCvShare(msg, enc::BGR8);//截取ROI(Region Of Interest)，即左右图像，会将原图像数据拷贝出来。leftImgROI_=cv_ptr->image(cv::Rect(0,0,cv_ptr->image.cols/2, cv_ptr->image.rows));rightImgROI_=cv_ptr->image(cv::Rect(cv_ptr->image.cols/2,0, cv_ptr->image.cols/2, cv_ptr->image.rows ));//创建两个CvImage, 用于存放原始图像的左右部分。CvImage创建时是对Mat进行引用的,不会进行数据拷贝leftImgPtr_=cv_bridge::CvImagePtr(new cv_bridge::CvImage(cv_ptr->header, cv_ptr->encoding,leftImgROI_) );rightImgPtr_=cv_bridge::CvImagePtr(new cv_bridge::CvImage(cv_ptr->header, cv_ptr->encoding,rightImgROI_) );//发布到/left_cam/image_raw和/right_cam/image_rawci_left_->header = cv_ptr->header; 	//很重要，不然会提示不同步导致无法去畸变ci_right_->header = cv_ptr->header;sensor_msgs::ImagePtr leftPtr =leftImgPtr_->toImageMsg();sensor_msgs::ImagePtr rightPtr =rightImgPtr_->toImageMsg();leftPtr->header=msg->header; 		//很重要，不然输出的图象没有时间戳rightPtr->header=msg->header;image_pub_left_.publish(leftPtr,ci_left_);image_pub_right_.publish(rightPtr,ci_right_);}private:ros::NodeHandle nh_;image_transport::ImageTransport it_;image_transport::Subscriber image_sub_;image_transport::CameraPublisher image_pub_left_;image_transport::CameraPublisher image_pub_right_;boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;sensor_msgs::CameraInfoPtr ci_left_;sensor_msgs::CameraInfoPtr ci_right_;cv::Mat leftImgROI_;cv::Mat rightImgROI_;cv_bridge::CvImagePtr leftImgPtr_=NULL;cv_bridge::CvImagePtr rightImgPtr_=NULL;
};int main(int argc,char** argv)
{ros::init(argc,argv, "camera_split");CameraSplitter cameraSplitter;ros::spin();return 0;
}

创建launch文件

<launch><node pkg="camera_split" type="camera_split_node" name="camera_split_node" output="screen" /><node pkg="image_view" type="image_view" name="image_view_left" respawn="false" output="screen"><remap from="image" to="/left_cam/image_raw"/><param name="autosize" value="true" /></node><node pkg="image_view" type="image_view" name="image_view_right" respawn="false" output="screen"><remap from="image" to="/right_cam/image_raw"/><param name="autosize" value="true" /></node>
</launch>

运行（运行之前先启动usb_cam）

cd catkin_ws
catkin_make
source ./devel/setup.bash
roslaunch camera_split camera_split_no_calibration.launch

在这里插入图片描述

二、创建双目相机参数文件

1. 棋盘格图像获取

拆分左右相机图像，按空格键捕获

  • main.cpp
#include<iostream>
#include<string>
#include<sstream>
#include<opencv2/core.hpp>
#include<opencv2/highgui.hpp>
#include<opencv2/videoio.hpp>
#include<opencv2/opencv.hpp>
#include<stdio.h>using namespace std;
using namespace cv;//双目摄像头支持2560x720, 1280x480，640x240
#define FRAME_WIDTH    2560
#define FRAME_HEIGHT   960const char* keys ={"{help h usage ? | | print this message}""{@video | | Video file, if not defined try to use webcamera}"};int main(int argc, char** argv)
{CommandLineParser parser(argc, argv, keys);parser.about("Video Capture");if (parser.has("help")){parser.printMessage();return 0;}String videoFile = parser.get<String>(0);if (!parser.check()){parser.printErrors();return 0;}VideoCapture cap;if (videoFile != ""){cap.open(videoFile);}else{cap.open(0);  //0-笔记本自带摄像头，1-外接usb双目摄像头cap.set(CV_CAP_PROP_FRAME_WIDTH, FRAME_WIDTH);  //设置捕获视频的宽度cap.set(CV_CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT);  //设置捕获视频的高度}if (!cap.isOpened()){cout << "摄像头打开失败!" << endl;return -1;}Mat frame, frame_L, frame_R;cap >> frame;         //从相机捕获一帧Mat grayImage;double fScale = 1.;Size dsize = Size(frame.cols*fScale, frame.rows*fScale);Mat imagedst = Mat(dsize, CV_32S);resize(frame, imagedst, dsize);char key;char image_left[200];char image_right[200];int cap_count = 0;int count = 0;int count1 = 0;int count2 = 0;namedWindow("图片1", 1);namedWindow("图片2", 1);while(1){key = waitKey(50);cap >> frame;count++;resize(frame, imagedst, dsize);frame_L = imagedst(Rect(0, 0, FRAME_WIDTH/2, FRAME_HEIGHT));namedWindow("Video_L", 1);imshow("Video_L", frame_L);frame_R = imagedst(Rect(FRAME_WIDTH/2, 0, FRAME_WIDTH/2, FRAME_HEIGHT));namedWindow("Video_R", 1);imshow("Video_R", frame_R);if (key == 27)break;if (key == 32)            //使用空格键拍照//if (0 == (count % 100))   //每5秒定时拍照{snprintf(image_left, sizeof(image_left), "/home/juling/Documents/CLionProjects/binocular_calibration/images3/left/left%02d.jpg", ++count1);imwrite(image_left, frame_L);imshow("图片1", frame_L);snprintf(image_right, sizeof(image_right), "/home/juling/Documents/CLionProjects/binocular_calibration/images3/right/right%02d.jpg", ++count2);imwrite(image_right, frame_R);imshow("图片2", frame_R);}}cap.release();return 0;
}
  • CmakeLists.txt
cmake_minimum_required(VERSION 3.21)
project(binocular_calibration)set(CMAKE_CXX_STANDARD 11)find_package( OpenCV 3.4.12 REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
aux_source_directory(. DIR_SRCS)
#add_executable(demo ${DIR_SRCS})
add_executable(binocular_calibration main.cpp)
target_link_libraries( binocular_calibration ${OpenCV_LIBS} )

2. 双目标定

OpenCV标定

  • 代码结构
    在这里插入图片描述

  • stereo_calibration.py

# -*- coding: utf-8 -*-import os.path
import numpy as np
import cv2
import globdef draw_parallel_lines(limg, rimg):HEIGHT = limg.shape[0]WIDTH = limg.shape[1]img = np.zeros((HEIGHT, WIDTH * 2 + 20, 3))img[:, :WIDTH, :] = limgimg[:, -WIDTH:, :] = rimgfor i in range(int(HEIGHT / 32)):img[i * 32, :, :] = 255return img# monocular camera calibrationcriteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((5 * 5, 3), np.float32)
objp[:, :2] = np.mgrid[0:5, 0:5].T.reshape(-1, 2)
objp = objp * 100  # 棋盘格方格100mmobjpoints = []
imgpoints1 = []
imgpoints2 = []
root_path ='./images2'
subfix = 'images2'
image_id = 12# 20230828 Julyer
# 左相机imgpoints1与右相机imgpoints2的维度不一样导致报错
left_imgs = glob.glob(root_path + '/left/*.jpg')
right_imgs = glob.glob(root_path + '/right/*.jpg')
for name in left_imgs:img_id = name.split('left')[-1]left_img = cv2.imread(name)right_img = cv2.imread(root_path + '/right/right' + img_id)gray1 = cv2.cvtColor(left_img, cv2.COLOR_BGR2GRAY)gray2 = cv2.cvtColor(right_img, cv2.COLOR_BGR2GRAY)ret1, corners1 = cv2.findChessboardCorners(gray1, (5, 5), cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS)ret2, corners2 = cv2.findChessboardCorners(gray2, (5, 5), cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS)if ret1 and ret2:objpoints.append(objp)corners11 = cv2.cornerSubPix(gray1, corners1, (11, 11), (-1, -1), criteria)corners22 = cv2.cornerSubPix(gray2, corners2, (11, 11), (-1, -1), criteria)imgpoints1.append(corners11)imgpoints2.append(corners22)# img1 = cv2.drawChessboardCorners(left_img, (5, 5), corners11, ret1)# img2 = cv2.drawChessboardCorners(right_img, (5, 5), corners22, ret2)# cv2.imshow('left corners', img1)# cv2.imshow('right corners', img2)# cv2.waitKey(1)elif not ret1:print('left' + img_id + ' couldn\'t be found')elif not ret2:print('right' + img_id + ' couldn\'t be found')
ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(objpoints, imgpoints1, gray1.shape[::-1], None, None)
ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(objpoints, imgpoints2, gray2.shape[::-1], None, None)
print('left ret: ', ret_l)
print('right ret: ', ret_r)# binocular camera calibration
ret, mtx_l, dist_l, mtx_r, dist_r, R, T, E, F = cv2.stereoCalibrate(objpoints, imgpoints1, imgpoints2, mtx_l, dist_l,mtx_r, dist_r, gray1.shape[::-1])np.savez("./parameters for calibration_" + subfix + ".npz", ret=ret, mtx_l=mtx_l, mtx_r=mtx_r, dist_l=dist_l, dist_r=dist_r, R=R, T=T, E=E, F=F)
np.savez("./points_" + subfix + ".npz", objpoints=objpoints, imgpoints1=imgpoints1, imgpoints2=imgpoints2)print('\nintrinsic matrix of left camera=', mtx_l)
print('\nintrinsic matrix of right camera=', mtx_r)
print('\ndistortion coefficients of left camera=', dist_l)
print('\ndistortion coefficients of right camera=', dist_r)
print('\nTransformation from left camera to right:')
print('\nR=', R)
print('\nT=', T)
print('\nReprojection Error=', ret)# stereo rectification
R1, R2, P1, P2, Q, ROI1, ROI2 = cv2.stereoRectify(mtx_l, dist_l, mtx_r, dist_r, gray1.shape[::-1], R, T, flags=0, alpha=-1)# undistort rectifying mapping
map1_l, map2_l = cv2.initUndistortRectifyMap(mtx_l, dist_l, R1, P1, gray1.shape[::-1], cv2.CV_16SC2)  # cv2.CV_32FC1
map1_r, map2_r = cv2.initUndistortRectifyMap(mtx_r, dist_r, R2, P2, gray2.shape[::-1], cv2.CV_16SC2)
print('\nmap1_r size', np.shape(map1_r))
print('\nmap2_r size', np.shape(map2_r))# undistort the original image, take img#12 as an example
left_id = cv2.imread(root_path + '/left/left' + str(image_id) + '.jpg')
right_id = cv2.imread(root_path + '/right/right' + str(image_id) + '.jpg')dst_l = cv2.remap(left_id, map1_l, map2_l, cv2.INTER_LINEAR)  # cv2.INTER_CUBIC
dst_r = cv2.remap(right_id, map1_r, map2_r, cv2.INTER_LINEAR)
cv2.imshow('map dst_r', dst_r)
cv2.waitKey(0)
print('\ndst_r size', np.shape(dst_r))
img_merge = draw_parallel_lines(dst_l, dst_r)# cv2.imwrite('./rectify_results/left03(rectified).jpg', dst_l)
# cv2.imwrite('./rectify_results/right03(rectified).jpg', dst_r)
cv2.imwrite('rectify_results/rectify' + str(image_id) + '_' + subfix + '.jpg', img_merge)
print('\nrectification has been done successfully.')np.savez("./rectify_results/parameters for rectification_" + subfix +".npz", R1=R1, R2=R2, P1=P1, P2=P2, Q=Q, ROI1=ROI1, ROI2=ROI2)print('\nR1=', R1)
print('\nR2=', R2)
print('\nP1=', P1)
print('\nP2=', P2)
print('\nQ=', Q)
print('\nROI1=', ROI1)
print('\nROI2=', ROI2)

标定结果：

/usr/bin/python3.8 /home/juling/Documents/PycharmProjects/Stereo-master/rovmaker/stereo_calibration.py
left ret:  0.3898234269642049
right ret:  0.4078028378647591intrinsic matrix of left camera= [[840.80247861   0.         667.37621909][  0.         840.1220566  519.95457746][  0.           0.           1.        ]]intrinsic matrix of right camera= [[838.1562009    0.         677.06068936][  0.         836.94290586 500.83733639][  0.           0.           1.        ]]distortion coefficients of left camera= [[-0.00459317  0.03249505  0.00071983  0.00213802  0.02668156]]distortion coefficients of right camera= [[ 0.00872802 -0.01583376 -0.00164319  0.00104224  0.08360213]]Transformation from left camera to right:R= [[ 9.99981316e-01  4.00224781e-04 -6.09985120e-03][-3.85052048e-04  9.99996830e-01  2.48836542e-03][ 6.10082777e-03 -2.48597017e-03  9.99978300e-01]]T= [[-57.64570079][ -0.7422294 ][  0.41023682]]Reprojection Error= 27.596230140236862rectification has been done successfully.R1= [[ 0.99982475  0.01329215 -0.01318275][-0.01327549  0.99991097  0.00135007][ 0.01319952 -0.00117483  0.99991219]]R2= [[ 0.9998918   0.01287432 -0.00711575][-0.01288329  0.99991627 -0.0012167 ][ 0.00709949  0.00130824  0.99997394]]P1= [[838.53248123   0.         684.23641968   0.        ][  0.         838.53248123 506.49901199   0.        ][  0.           0.           1.           0.        ]]P2= [[ 8.38532481e+02  0.00000000e+00  6.81501434e+02 -4.83430231e+04][ 0.00000000e+00  8.38532481e+02  5.06499012e+02  0.00000000e+00][ 0.00000000e+00  0.00000000e+00  1.00000000e+00  0.00000000e+00]]Q= [[ 1.00000000e+00  0.00000000e+00  0.00000000e+00 -6.84236420e+02][ 0.00000000e+00  1.00000000e+00  0.00000000e+00 -5.06499012e+02][ 0.00000000e+00  0.00000000e+00  0.00000000e+00  8.38532481e+02][ 0.00000000e+00  0.00000000e+00  1.73454705e-02 -4.74396078e-02]]ROI1= (27, 13, 1221, 909)ROI2= (31, 38, 1213, 903)Process finished with exit code 0

3. 创建yaml参数文件

参考：https://blog.csdn.net/weixin_37918890/article/details/95626004

%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV) 
# Pr矩阵中的值（参考：https://blog.csdn.net/weixin_37918890/article/details/95626004）
Camera.fx: 8.38532481e+02
Camera.fy: 8.38532481e+02
Camera.cx: 6.81501434e+02
Camera.cy: 5.06499012e+02Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0Camera.width: 1280
Camera.height: 960# Camera frames per second 
Camera.fps: 20.0# stereo baseline times fx
# Pr中的值，单位转为m，取绝对值
Camera.bf: 48.3430231# 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: 18#--------------------------------------------------------------------------------------------
# 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: 960
LEFT.width: 1280
LEFT.D: !!opencv-matrixrows: 1cols: 5dt: ddata:[-0.00459317, 0.03249505, 0.00071983, 0.00213802, 0.02668156]
LEFT.K: !!opencv-matrixrows: 3cols: 3dt: ddata: [840.80247861, 0., 667.37621909, 0.0, 840.1220566, 519.95457746, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrixrows: 3cols: 3dt: ddata: [ 0.99982475,  0.01329215, -0.01318275, -0.01327549, 0.99991097, 0.00135007,  0.01319952, -0.00117483,  0.99991219]
LEFT.P:  !!opencv-matrixrows: 3cols: 4dt: ddata: [838.53248123, 0. , 684.23641968, 0. , 0. , 838.53248123, 506.49901199, 0. , 0. , 0. , 1. , 0.]RIGHT.height: 960
RIGHT.width: 1280
RIGHT.D: !!opencv-matrixrows: 1cols: 5dt: ddata:[0.00872802, -0.01583376, -0.00164319, 0.00104224, 0.08360213]
RIGHT.K: !!opencv-matrixrows: 3cols: 3dt: ddata: [838.1562009, 0., 677.06068936, 0.0, 836.94290586, 500.83733639, 0.0, 0.0, 1]
RIGHT.R:  !!opencv-matrixrows: 3cols: 3dt: ddata: [0.9998918, 0.01287432, -0.00711575, -0.01288329, 0.99991627, -0.0012167, 0.00709949, 0.00130824, 0.99997394]
# -4.83430231e+04转为m单位，即-4.83430231e+01
RIGHT.P:  !!opencv-matrixrows: 3cols: 4dt: ddata: [8.38532481e+02, 0.00000000e+00, 6.81501434e+02, -4.83430231e+01, 0, 8.38532481e+02, 5.06499012e+02, 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

三、ROS在线运行ORB SLAM2建立稀疏地图

1. 修改订阅的相机话题为拆分后的话题

复制ros_stereo.cc为ros_stereo_rovmaker.cc，修改如下部分

    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, "/left_cam/image_raw", 1);message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "right_cam/image_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));

2. 重新编译

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/juling/Documents/projects/ORB_SLAM2_binocular
chmod +x build_ros.sh
./build_ros.sh

3. 运行

rosrun ORB_SLAM2 StereoRovmaker Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/rovmaker.yaml true

在这里插入图片描述

四、OpenCV在线运行ORB SLAM2建立稀疏地图

参考：十里桃园的博客
由于是单usb合成图像输出，这里修改了一下代码，输出左右帧。复制Example/Stereo/stereo_euroc.cc，修改为如下代码。

  • stereo_euroc_slty.cc
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/#include<iostream>
#include<algorithm>
#include<fstream>
#include<iomanip>
#include<chrono>#include<opencv2/core/core.hpp>
#include<opencv2/highgui.hpp>
#include<opencv2/videoio.hpp>
#include<opencv2/opencv.hpp>#include<System.h>
using namespace std::chrono;
using namespace std;
using namespace cv;#define FRAME_WIDTH    2560
#define FRAME_HEIGHT   960int main(int argc, char **argv)
{// Retrieve paths to imagesvector<string> vstrImageLeft;vector<string> vstrImageRight;vector<double> vTimeStamp;//LoadImages(string(argv[3]), string(argv[4]), string(argv[5]), vstrImageLeft, vstrImageRight, vTimeStamp);//if(vstrImageLeft.empty() || vstrImageRight.empty())// {//  cerr << "ERROR: No images in provided path." << endl;// return 1;//}// if(vstrImageLeft.size()!=vstrImageRight.size())// {//   cerr << "ERROR: Different number of left and right images." << endl;//     return 1;// }// Read rectification parameterscv::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::Mat M1l,M2l,M1r,M2r;cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,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,M1r,M2r);// const int nImages = vstrImageLeft.size();// 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);// Vector for tracking time statisticsvector<float> vTimesTrack;cout << endl << "-------" << endl;cout << "Start processing camera ..." << endl;cv::Mat imLeft, imRight, imLeftRect, imRightRect;
//***********************************************************************8cv::VideoCapture cap(0, cv::CAP_V4L2);if (!cap.isOpened()){cout << "摄像头打开失败!" << endl;return -1;}else{cap.open(0, cv::CAP_V4L2);  //0-笔记本自带摄像头，1-外接usb双目摄像头cap.set(cv::CAP_PROP_FRAME_WIDTH, FRAME_WIDTH);  //设置捕获视频的宽度cap.set(cv::CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT);  //设置捕获视频的高度cap.set(cv::CAP_PROP_FPS, 30);}cv::Mat frame;cap >> frame;         //从相机捕获一帧cv::Mat grayImage;double fScale = 1.;cv::Size dsize = cv::Size(frame.cols*fScale, frame.rows*fScale);cv::Mat imagedst = cv::Mat(dsize, CV_32S);
//***********************************************************************8long int nImages = 0;int ni=0;
// Main loopwhile(ni>-1){cap >> frame;cv::resize(frame, imagedst, dsize);imLeft = imagedst(cv::Rect(0, 0, FRAME_WIDTH/2, FRAME_HEIGHT));imRight = imagedst(cv::Rect(FRAME_WIDTH/2, 0, FRAME_WIDTH/2, FRAME_HEIGHT));
//***********************************************************************8if(imLeft.empty()){cerr << endl << "Check Left Camera!! "<< endl;return 1;}if(imRight.empty()){cerr << endl << "Check Right Camera!! "<< endl;return 1;}cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);time_point<system_clock> now = system_clock::now();double tframe = now.time_since_epoch().count();vTimeStamp.push_back(tframe);#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#elsestd::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif// Pass the images to the SLAM systemSLAM.TrackStereo(imLeftRect,imRightRect,tframe);#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#elsestd::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endifdouble ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();vTimesTrack.push_back(ttrack);// Wait to load the next frame
/*        double T=0;if(ni<nImages-1)T = vTimeStamp[ni+1]-tframe;else if(ni>0)T = tframe-vTimeStamp[ni-1];if(ttrack<T)usleep((T-ttrack)*1e6);
*/nImages++;std::cout << "shilitaoyuan_frames: "<<nImages<< std::endl; }// Stop all threadsSLAM.Shutdown();// Tracking time statisticssort(vTimesTrack.begin(),vTimesTrack.end());float totaltime = 0;for(int ni=0; ni<nImages; ni++){totaltime+=vTimesTrack[ni];}cout << "-------" << endl << endl;cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;cout << "mean tracking time: " << totaltime/nImages << endl;// Save camera trajectorySLAM.SaveTrajectoryTUM("CameraTrajectory.txt");return 0;
}
  • /ORB_SLAM2_binocular/CmakeLists.txt
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)add_executable(stereo_kitti
Examples/Stereo/stereo_kitti.cc)
target_link_libraries(stereo_kitti ${PROJECT_NAME})add_executable(stereo_euroc
Examples/Stereo/stereo_euroc.cc)
target_link_libraries(stereo_euroc ${PROJECT_NAME})
# 增加下面几行
add_executable(stereo_euroc_slty
Examples/Stereo/stereo_euroc_slty.cc)
target_link_libraries(stereo_euroc_slty ${PROJECT_NAME})

重新编译

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/juling/Documents/projects/ORB_SLAM2_binocular
chmod +x build.sh
./build.sh

运行

./Examples/Stereo/stereo_euroc_slty Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/rovmaker.yaml

yaml文件中的特征点数量ORBextractor.nFeatures从1200改成了2500，初始化的时候要慢一些，相机移动速度要平稳。
办公室稀疏建图结果：
在这里插入图片描述

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

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

相关文章

Java实现获取微信小程序scheme码报错

Java实现获取微信小程序scheme码报错

如标题所见&#xff0c;使用Java获取小程序scheme时除了出现文档中的常见错误&#xff0c;我将我调试的时候遇到的错误和解决方式分享出来方便大家少花一部分时间解决该问题。&#xff08;往下划有结论节省时间&#xff09;。 获取scheme码之前需要先获取access_token&#xff…
阅读更多...
使用Postman如何在接口测试前将请求的参数进行自定义处理

使用Postman如何在接口测试前将请求的参数进行自定义处理

1、前言 当我们使用 Postman 进行接口测试时&#xff0c;对于简单的不需要处理的接口&#xff0c;直接请求即可&#xff0c;但是对于需要处理的接口&#xff0c;如需要转码、替换值等&#xff0c;则就麻烦一些&#xff0c;一般我们都是先手动把修改好的值拷贝到请求里再进行请…
阅读更多...
Jaeger的经典BUG原创

Jaeger的经典BUG原创

前端&#xff0c;笔者在使用Jaeger进行Trace监控的时候&#xff0c;当数据量增大到一定数量级时&#xff0c;出现了一次CPU暴增导致节点服务器挂了的经典案例&#xff0c;这里对案例进行一个简单的抽象&#xff0c;供大家参考&#xff1a; 首先通过pprof对耗时的函数进行定位&…
阅读更多...
spacy安装旧版本en_core_web_sm的解决方案

spacy安装旧版本en_core_web_sm的解决方案

大家好，我是爱编程的喵喵。双985硕士毕业，现担任全栈工程师一职，热衷于将数据思维应用到工作与生活中。从事机器学习以及相关的前后端开发工作。曾在阿里云、科大讯飞、CCF等比赛获得多次Top名次。现为CSDN博客专家、人工智能领域优质创作者。喜欢通过博客创作的方式对所学的…
阅读更多...
MQTT，如何在SpringBoot中使用MQTT实现消息的订阅和发布

MQTT，如何在SpringBoot中使用MQTT实现消息的订阅和发布

一、MQTT介绍 1.1 什么是MQTT&#xff1f; MQTT&#xff08;Message Queuing Telemetry Transport&#xff0c;消息队列遥测传输协议&#xff09;&#xff0c;是一种基于发布/订阅&#xff08;publish/subscribe&#xff09;模式的“轻量级”通讯协议&#xff0c;该协议构建于…
阅读更多...
排序算法学习记录-快速排序

排序算法学习记录-快速排序

快速排序 快速排序关键在于确定一个中间值&#xff0c;使得小于这个中间值的数在左边&#xff0c;大于这个中间值的数在右边。那么中间值该如何确定呢&#xff1f;有以下几种做法 首元素&#xff0c;也就是arr[l]尾元素&#xff0c;也就是arr[r]中间元素&#xff0c;也就是ar…
阅读更多...
JavaScript中的事件委托（event delegation）

JavaScript中的事件委托（event delegation）

聚沙成塔每天进步一点点 ⭐ 专栏简介⭐ JavaScript事件委托⭐ 事件冒泡&#xff08;Event Bubbling&#xff09;⭐ 事件委托的优点⭐ 如何使用事件委托⭐ 写在最后 ⭐ 专栏简介 前端入门之旅&#xff1a;探索Web开发的奇妙世界 记得点击上方或者右侧链接订阅本专栏哦 几何带你启…
阅读更多...
CocosCreator3.8研究笔记（二）windows环境 VS Code 编辑器的配置

CocosCreator3.8研究笔记（二）windows环境 VS Code 编辑器的配置

一、设置文件显示和搜索过滤步骤 为了提高搜索效率以及文件列表中隐藏不需要显示的文件&#xff0c; VS Code 需要设置排除目录用于过滤。 比如 cocoscreator 中&#xff0c;编辑器运行时会自动生成一些目录&#xff1a;build、temp、library&#xff0c; 所以应该在搜索中排除…
阅读更多...
电商项目part10 高并发缓存实战

电商项目part10 高并发缓存实战

缓存的数据一致性 只要使用到缓存&#xff0c;无论是本地内存做缓存还是使用 redis 做缓存&#xff0c;那么就会存在数据同步的问题。 先读缓存数据&#xff0c;缓存数据有&#xff0c;则立即返回结果&#xff1b;如果没有数据&#xff0c;则从数据库读数据&#xff0c;并且把…
阅读更多...
怎么把pdf压缩的小一点？

怎么把pdf压缩的小一点？

怎么把pdf压缩的小一点&#xff1f;在我们日常的学习和工作中&#xff0c;PDF文件是一个非常常见和有用的文件格式&#xff0c;并且受到很多小伙伴的喜欢。有时候&#xff0c;一些PDF文件可能会很大&#xff0c;造成pdf文件较大的原因其实很明确&#xff0c;主要是因为pdf文件中…
阅读更多...
【LeetCode算法系列题解】第46~50题

【LeetCode算法系列题解】第46~50题

CONTENTS LeetCode 46. 全排列&#xff08;中等&#xff09;LeetCode 47. 全排列 II&#xff08;中等&#xff09;LeetCode 48. 旋转图像&#xff08;中等&#xff09;LeetCode 49. 字母异位词分组&#xff08;中等&#xff09;LeetCode 50. Pow(x, n)&#xff08;中等&#xf…
阅读更多...
华为云云服务器评测 | 从零开始：云耀云服务器L实例的全面使用解析指南

华为云云服务器评测 | 从零开始：云耀云服务器L实例的全面使用解析指南

文章目录 一、前言二、云耀云服务器L实例要点介绍2.1 什么是云耀云服务器L实例2.2 云耀云服务器L实例的产品定位2.3 云耀云服务器L实例优势2.4 云耀云服务器L实例支持的镜像与应用场景2.5 云耀云服务器L实例与弹性云服务器&#xff08;ECS&#xff09;区别 三、购买与配置云耀云…
阅读更多...
【100天精通Python】Day51：Python 数据分析_数据分析入门基础与Anaconda 环境搭建

【100天精通Python】Day51：Python 数据分析_数据分析入门基础与Anaconda 环境搭建

目录 1 科学计算和数据分析概述 2. 数据收集和准备 2.1 数据收集 2.1.1 文件导入&#xff1a; 2.1.2 数据库连接&#xff1a; 2.1.3 API请求&#xff1a; 2.1.4 网络爬虫&#xff1a; 2.2 数据清洗 2.2.1 处理缺失值&#xff1a; 2.2.2 去除重复值&#xff1a; 2.2…
阅读更多...
dlopen “libnvcuvid.so“ failed!

dlopen “libnvcuvid.so“ failed!

在使用NVIDIA DALI库进行视频数据处理时&#xff0c;出现了以上打开libnvcuvid.so动态库错误的问题&#xff0c;如下图所示&#xff1a; libnvcuvid.so是使用CUDA进行硬编解码需要的一个库&#xff0c;使用NVIDIA DALI进行视频处理时会依赖它。 本人是在Docker容器中运行的程序…
阅读更多...
langchain介绍之-Prompt

langchain介绍之-Prompt

LangChain 是一个基于语言模型开发应用程序的框架。它使得应用程序具备以下特点&#xff1a;1.数据感知&#xff1a;将语言模型与其他数据源连接起来。2.代理性&#xff1a;允许语言模型与其环境进行交互 LangChain 的主要价值在于&#xff1a;组件&#xff1a;用于处理语言模型…
阅读更多...
[华为云云服务器评测] Unbutnu添加SSH Key、编译启动Springboot项目

[华为云云服务器评测] Unbutnu添加SSH Key、编译启动Springboot项目

系列文章目录 第一章 [linux实战] 华为云耀云服务器L实例 Java、node环境配置 第二章 [linux实战] Unbutnu添加SSH Key、启动Springboot项目 文章目录 系列文章目录前言一、任务拆解二、配置git,添加SSH Key2.1、登录远程主机2.2、配置git用户名和邮箱2.3、生成SSH key2.4、查…
阅读更多...
【DevOps视频笔记】6 - 7. Jenkins 介绍 和 安装

【DevOps视频笔记】6 - 7. Jenkins 介绍 和 安装

一、Integrate 工具 二、Jenkins 介绍 1. Jenkins 最主要的工作 2. CI / CD 可以理解为&#xff1a; 2.1 CI 过程 2.2 CD 过程 三、Jenkins 安装 1. 安装准备工作 2. 安装 Jenkins Stage 1&#xff1a;拉取 jenkins 镜像 Stage 2&#xff1a;编写docker-compose.yml St…
阅读更多...
小白开始学习C++

小白开始学习C++

第一节&#xff1a;控制台输出hello word&#xff01; #include<iostream> //引入库文件 int main() { //控制台输出 hello word! 之后回车 std::cout << "hello word!\n"; #include<iostream> //引入库文件int main() {//控制台输出…
阅读更多...
docker 笔记6：高级篇 DockerFile解析

docker 笔记6：高级篇 DockerFile解析

目录 1.是什么&#xff1f; 2.构建三步骤 3.DockerFile构建过程解析 3.1 Dockerfile内容基础知识 3.2Docker执行Dockerfile的大致流程 总结 4.DockerFile常用保留字指令 5.案例&#xff1a;自定义镜像 5.1 要求&#xff1a; Centos7镜像具备vimifconfigjdk8 5.2编写 5…
阅读更多...
Android 1.2.1 使用Eclipse + ADT + SDK开发Android APP

Android 1.2.1 使用Eclipse + ADT + SDK开发Android APP

1.2.1 使用Eclipse ADT SDK开发Android APP 1.前言 这里我们有两条路可以选&#xff0c;直接使用封装好的用于开发Android的ADT Bundle&#xff0c;或者自己进行配置 因为谷歌已经放弃了ADT的更新&#xff0c;官网上也取消的下载链接&#xff0c;这里提供谷歌放弃更新前最新…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023