ROS 生成深度图
这部分代码已上传到 Gitee ：这里
文章目录
简介
双目摄像头测距与 OpenCV 简介
人眼能够区分远和近的物体，靠的就是两只眼睛的配合，它们之间有着一个固定的距离，大脑能够根据这两只眼睛生成的两幅有一定差异的图像拼装出立体的图像，而且由于人眼能够转动，在相同的位置还可以调节焦距和两只眼睛的转动角度来看清远的物体和近的物体。
而在计算机，可以使用两只摄像头来模拟人眼，然后调用 OpenCV 库的一些函数即可生成一幅深度图，两只摄像头之间的距离是一定的，摄像头有着各自的焦距、光学中心等自身不可变的物理属性，我们可以通过这些属性来计算出双目测距的一些基础信息，这样才能通过 OpenCV 来正确计算出深度图。
双目测距原理
双目测距的原理在 OpenCV 的官方文档中也有介绍，在翻到官方文档的双目测距章节时，能够看到以下两幅图1：
图1 双摄像头模型俯视图
图2 双摄像头模型立体视图
书上写出了利用双目摄像头的一些数学公式，这里就不再阐述了（因为我也看不懂）。其实双目摄像头说简单点就是利用相似三角形，在知道了两个摄像头的一些固定属性之后，可以根据这些属性和当前获取到的图像计算出某一点距离摄像头的距离。
准备
双目测距准备
上面说到，在使用 OpenCV 根据两幅图像计算深度图前，需要获取到摄像机的一些固定的物理属性，这些属性包括两只摄像头各自的焦距、光学中心、两只摄像头之间的距离（三维坐标）等。
但是这些数据很难使用测量仪器测量出来，这里需要先固定好两只摄像头，调整他们的镜头，使得两只摄像头之间的距离不会因为其他原因被改变和使得他们能够清楚地拍出清晰的照片，然后在这个基础上使用双目摄像头进行标定。
标定过程可以简单地说一下：
- 首先就是先打印一个棋盘出来。棋盘的文件可以到 OpenCV 官方教程处下载，或者也可以直接到网上找一些棋盘图片，也可以自己画一个棋盘出来，然后要把这个棋盘的图片沾到一个比较坚硬一点的板子上，方便在拍照的时候拿着和移动
- 然后自己写一个简单的程序，同时获取两个相机的图像并保存。这里获取的图像就是上面制作好的棋盘的图片，通过移动棋盘或者摄像头，拍出不同角度和距离的棋盘照片，大概拍20张左右即可。
- 最后就是使用标定程序或软件进行摄像机标定了。这里可以使用 OpenCV 官方教程的程序进行标定，也可以使用 Matlab 的标定工具包2进行标定，标定完成后就可以得到相机的固定属性数据了。
值得一提的是，双目摄像头的标定需要比较多的耐心，特别是使用 Matlab 的标定工具包时，需要一定的操作才能标定好，所以最好在标定之后就不要动相机了，把两个相机固定好，要不然还得再标定一次。
ROS 的摄像头仿真
因为现实情况中，好一点的摄像头比较贵，而且需要固定等操作，在标定的时候也会因为一些不必要的物理因素而影响到标定结果，所以使用真实的摄像头去标定的话会很麻烦。
这里就是利用了 ROS 仿真的便利性，可以随意调节摄像头的像素、摄像头之间的距离和夹角和一些内部属性等，而且一些不能设置的属性如焦距、光学中心等都可以直接通过 ros 获取到，可以说直接跳过了标定的步骤，简直就是双目测距的福音呀。
ROS 获取仿真摄像头的固定属性数据
可以在写完两个摄像头的模型文件后，直接通过获取发布的相机话题的信息，即可获取到相机的基本属性：
rostopic echo 你发布的相机的话题名称
这样就可以得到类似下面的数据了：
header:
seq: 19
stamp:
secs: 156
nsecs: 170000000
frame_id: "left_camera_link"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [381.36246688113556, 0.0, 320.5, 0.0, 381.36246688113556, 240.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [381.36246688113556, 0.0, 320.5, -26.69537268167949, 0.0, 381.36246688113556, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
是不是很轻松！！！
生成深度图
由于对于 OpenCV 并不是很熟悉，这里就使用大佬的代码3进行修改。
设置相机属性参数
这里根据大佬的代码和标定数据，可以对照到自己的摄像头数据对代码进行修改。
这里根据上面的摄像头数据进行修改：
#!/usr/bin/env python
# coding: utf-8
# filename: camera_configs.py
import cv2
import numpy as np
left_camera_matrix = np.array([[381.36246688113556, 0.0, 320.5],
[0.0, 381.36246688113556, 240.5],
[0.0, 0.0, 1.0]])
left_distortion = np.array([[0., 0., 0., 0., 0.]])
right_camera_matrix = left_camera_matrix
right_distortion = left_distortion
om = np.array([0., 0., 0.]) # 旋转关系向量
R = cv2.Rodrigues(om)[0] # 使用Rodrigues变换将om变换为R
T = np.array([-50., 0., -0.]) # 平移关系向量
size = (640, 480) # 图像尺寸 (width, height)
# 进行立体更正
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
right_camera_matrix, right_distortion, size, R,
T)
# 计算更正map
left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
生成深度图
其实在获取到两个摄像头的数据之后生成深度图的代码大佬已经给我们准备好了，但这里需要连接到 ros 发布的仿真摄像机的话题，所以这里需要分开三个部分说一下，因为这里看似很小的改动却用了我很长的时间。
使用 python 获取话题的图像数据
使用 Python 获取话题的图像数据可以使用下面的代码来获取：
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
bridge = CvBridge()
def image_callback(message):
frame = bridge.imgmsg_to_cv2(message, "bgr8")
cv2.namedWindow("Frame", cv2.WINDOW_NORMAL)
cv2.imshow("Frame", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
def camera_listener():
rospy.Subscriber("你的相机的话题名字", Image, imge_callback)
rospy.spin()
rospy.init_node('随便起一个名字就行', anonymous=True)
camera_listener()
看着很简单，实际存在一个特别大的问题，那就是因为 rospy.spin() 这一句是阻塞型的语句，所以只能在同一个进程中获取到一个相机的图像，另一个相机就不能获取了，所以这也就不能达成生成深度图的条件了。
其实这个问题使用 C++ 进行编程可以很容易解决，因为 ros 的 C++ 函数提供了 spinOnce() 函数，这会在获取了一张图片的数据之后直接跳转到下一条指令继续执行，但由于没有现成的 C++ 的深度图生成代码，所以这并不能行得通。
通过查找资料4，终于找到了解决办法：使用 rospy.wait_for_message() 函数。这个函数跟 C++ 中提供的函数类似，能够在获取一次数据之后停下来，执行下一条语句，而不是一直在这获取相同话题的数据。但是，这个函数也有一个不好的地方，那就是获取到的数据的类型并不是 sensor_msgs.msg.Image ，而是 rospy.Message 类型。这就很头疼了。
把话题数据转换为可以显示的图像数据
在编写这个文档的时候，我突然想到可不可以使用
bridge.imgmsg_to_cv2()函数来转换数据，但因为时间问题，这里就先不测试这个想法了。
这里需要把获取到的数据转换为 OpenCV 可以直接显示的图像数据，才能输入到深度图生成函数中生成深度图。
在官网上找文档，没有找到可以直接转换的方法，在查看代码的说明文档时也找不到具体的转换函数，这可就麻烦了呀。
然后我发现可以直接把这个数据转换为 str 类型，于是输出它的前面200个字符看看：
header:
seq: 0
stamp:
secs: 2
nsecs: 960000000
frame_id: "left_camera_link"
height: 480
width: 640
encoding: "rgb8"
is_bigendian: 0
step: 1920
data: [178, 178, 178, 176, 176, 176, 175,
很明显，这就是 yaml 数据嘛，然后找 python 的 yaml 教程，想要通过 yaml 模块来获取到 data 字段的数据，说干就干。
但是不知道是因为我的电脑内存太小还是什么原因，在使用 yaml 模块对这个字符串生成 yaml 对象时，直接卡住，连鼠标都动不了（一开始我使用的图像的大小是1280*720的，data字段的数据很多），连续试了几次都是这个情况，这里浪费的时间起码有半个小时以上。
最后，我想到，既然我截取前面200个字符这么容易，那么为什么不直接截取这部分的内容呢，毕竟都是在 data 字段之后，而且这个字段是最后一个字段。这个方法真的很快，比使用 yaml 模块快了无数倍。
获取到这个字符串后，可以通过 ", " 字符串来分割成为一个列表，然后使用 map 函数转换为 int 类型的数据，再通过 numpy 来转换为合适的结构即可：
#!/usr/bin/env python
# coding: utf-8
import rospy
import cv2
import numpy
topicName = "/camera/left_camera/left_image_raw"
if __name__ == "__main__":
rospy.init_node("getImageTest", anonymous=True)
frame = rospy.wait_for_message(topicName, Image, timeout=None)
print(type(frame))
# 获取 frame 的字符串类型的数据
frameStr = str(frame)
# 找到 "data: [" 字符串的位置，后面就是字符串列表数据
keyStr = "data: ["
# 截断字符串，取 keyStr 后面、最后的 "]" 之前的字符串
dataStr = frameStr[frameStr.find(keyStr) + len(keyStr): -2]
# 根据 ", " 分割字符串，直接获得一个列表
data = dataStr.split(", ")
# 对列表中的每一个元素转化为 int 类型，python2 转化后还是一个列表，
# python3 则需要使用 list 强制转换一下
data = map(int, data)
# 把列表转换为 numpy 数组
dataNpArr = numpy.array(data)
width = 1280 # 图像的宽度
height = 720 # 图像的高度
channel = 3 # 图像的通道数
# shape is (height, width, channel)
# 转换为正确的图片的格式
dataNpArr = dataNpArr.reshape((height, width, channel))
# 转换为正确的数据类型
dataNpArr = dataNpArr.astype(numpy.uint8)
# 显示图像看一下
cv2.namedWindow("frame", cv2.WINDOW_NORMAL)
cv2.imshow("frame", dataNpArr)
if cv2.waitKey(0) & 0xFF == ord('q'):
cv2.destroyAllWindows()
根据图像数据生成深度图
获取到具体的图像之后就很简单了，修改一下大佬的代码然后直接用就好啦：
#!/usr/bin/env python
# coding: utf-8
import numpy as np
import cv2
import camera_configs
import rospy
from sensor_msgs.msg import Image
from MessageTransToImage import *
imgWidth = camera_configs.size[0]
imgHeight = camera_configs.size[1]
cv2.namedWindow("depth")
# 添加点击事件，打印当前点的距离
def callbackFunc(e, x, y, f, p):
if e == cv2.EVENT_LBUTTONDOWN:
print(threeD[y][x])
cv2.setMouseCallback("depth", callbackFunc, None)
rospy.init_node("camera_listener")
leftCameraTopicName = "/camera/left_camera/left_image_raw"
rightCameraTopicName = "/camera/right_camera/right_image_raw"
def getFrame(topicName, timeout=None):
'''
获取一帧数据
获取指定话题的一帧数据
'''
return rospy.wait_for_message(topicName, Image, timeout=timeout)
if "__main__" != __name__:
exit()
while True:
# 获取一帧图像
imgL = getFrame(leftCameraTopicName, 1)
imgR = getFrame(rightCameraTopicName, 1)
# 不能获取图像则跳过
if imgL is None or imgR is None:
print("Image is None\nCheck your topic name?")
continue
# 把 Message 转换成 ndarray 数据
imgL = MessageTransToImage(imgL, imgWidth, imgHeight)
imgR = MessageTransToImage(imgR, imgWidth, imgHeight)
# 根据更正map对图片进行重构
img1_rectified = cv2.remap(imgL, camera_configs.left_map1, camera_configs.left_map2, cv2.INTER_LINEAR)
img2_rectified = cv2.remap(imgR, camera_configs.right_map1, camera_configs.right_map2, cv2.INTER_LINEAR)
# 将图片置为灰度图，为StereoBM作准备
imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY)
num = 2
blockSize = 23
# 根据Block Maching方法生成差异图（opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试）
stereo = cv2.StereoBM_create(numDisparities=16*num, blockSize=blockSize)
disparity = stereo.compute(imgL, imgR)
disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
# 将图片扩展至3d空间中，其z方向的值则为当前的距离
threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32)/16., camera_configs.Q)
cv2.imshow("depth", disp)
key = cv2.waitKey(1)
if key == ord("q"):
break
elif key == ord("s"):
cv2.imwrite("BM_left.jpg", imgL)
cv2.imwrite("BM_right.jpg", imgR)
cv2.imwrite("BM_depth.jpg", disp)
cv2.destroyAllWindows()
本来想着可以衣食无忧了，结果真正打开仿真和深度图生成函数，才发现事情没有这么简单，可能是我的电脑太古董的原因，深度图根本就不能流畅地生成，移动摄像头的位置之后，深度图生成函数要反应个十万八千里才能反应过来，最终导致的问题就是不能实时生成深度图，可能的原因有：1. 我的电脑太老了，跑不动；2. 图像数据转换太慢了； 3. ROS的发布的话题传递的数据是阻塞型的，也就是需要把前面的数据都获取完才能获取到后面的数据。目前还没有解决这个问题。
1215
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
