配置:双系统ubuntu20.04, iphone13前置摄像头(关闭畸变矫正)
1.手机摄像头无线连接电脑
DroidCam是一个桥梁工具,当电脑没有USB摄像头时,它可以将手机的摄像头视频流通过USB/WiFi传输到电脑,并在Linux系统中创建一个虚拟的V4L2设备(如/dev/video0)。参考ubuntu使用droidcam连接手机摄像头连接手机摄像头,这里博主使用的是iphone手机,也是可以连接的,注意断开校园网(会有AP 隔离)。
安装好之后执行droidcam发现有输出Video: /dev/video0说明连接成功,已经捕获到了手机摄像头数据,或者也可以使用lsusb | grep -i apple进行验证:
可以打开ubuntu自带的茄子软件,点击中间的翻转即可直接连接到手机相机,不过使用下来不太稳定,一直会崩,因此博主这里使用ffplay /dev/video0进行查看:
2.编写ROS图像发布节点
下面通过OpenCV读取DroidCam创建的/dev/video0设备,将OpenCV的BGR图像格式转换成ROS标准的sensor_msgs/Image消息格式,并将图像数据发布到/camera/image_raw话题,供其他Ros节点订阅使用:
点击查看代码
# 创建功能包
cd ~/catkin_ws/src
catkin_create_pkg droidcam_pkg rospy cv_bridge sensor_msgs
# 创建 scripts 目录和 Python 节点
cd droidcam_pkg
mkdir scripts
cd scripts
# 创建并编辑 Python 文件
gedit droidcam_publisher.py
点击查看代码
#!/usr/bin/env python3
import cv2
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridgedef main():rospy.init_node('droidcam_publisher', anonymous=True)pub = rospy.Publisher('/camera/image_raw', Image, queue_size=10)bridge = CvBridge()# 打开 DroidCam 设备(根据 v4l2-ctl 的结果,这里是 /dev/video0)cap = cv2.VideoCapture('/dev/video0')if not cap.isOpened():rospy.logerr("无法打开 /dev/video0")returnrate = rospy.Rate(30) # 30 fpswhile not rospy.is_shutdown():ret, frame = cap.read()if ret:img_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")pub.publish(img_msg)rate.sleep()cap.release()if __name__ == '__main__':main()
点击查看代码
# 赋予可执行权限
chmod +x droidcam_publisher.py
# 使用 catkin build 编译
cd ~/catkin_ws
catkin build droidcam_pkg
source ~/catkin_ws/devel/setup.bash
下面运行节点:
点击查看代码
# 启动DroidCam并连接手机
droidcam
# 启动 ROS master
roscore
# 运行节点
rosrun droidcam_pkg droidcam_publisher.py
# 查看图像
rosrun image_view image_view image:=/camera/image_raw
即可在ubuntu中看到对应的图像数据:
启动ORB_SLAM2
配置ORB_SLAM2的过程请看之前的教程,启动它需要调整输入参数(~/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml),包括了相机内参,数据格式,特征点数量(如果所处环境纹理稀疏,请降低特征点数量,不然不能顺利初始化,或者跟踪丢失问题)等,下面来确定这些参数:
首先进行iphone13手机前置摄像头标定,标定代码可以参考视觉SLAM十四讲2,选择的标定板为:

标定结果如下:

但是这里踩了坑,视频模式下拍摄的图片大小是3672x2066,因此相机内参都是基于这个分辨率计算得来的,因此当我将这些数据替换到Asus.yaml文件中时,启动SLAM一直无法顺利初始化,一开始我以为是分辨率太大耗时的问题,后来通过执行rostopic echo /camera/image_raw --noarr发现获取到手机摄像头视频流的每一帧数据如下:

即图像分辨率对应的竟然是640x480,说明DroidCam会自动压缩图像分辨率,同时编码是bgr8,而不是默认的RGB格式,最终将对应内参都进行了缩放,结果如下:

最后在上一节基础上再执行下面代码即可
点击查看代码
rosrun ORB_SLAM2 Mono /home/chime/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/chime/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml

tips:还有一些优秀的做法,包括摄像头图像接入程序;选择背景简单干净的场景进行标定
