双目三维重建-双目摄像头实现双目测距（Python）
目录
双目立体视觉深度相机实现双目测距功能，主要分为4个步骤：相机标定+双目校正+双目匹配+计算深度信息：
(1）相机标定：需要对双目相机进行标定，得到两个相机的内外参数、单应矩阵。
(2) 双目校正：根据标定结果对原始图像进行校正，校正后的两张图像位于同一平面且互相平行。
(3）双目匹配：对校正后的两张图像进行像素点匹配。
(4）计算深度图：根据匹配结果计算每个像素的深度，从而获得深度图。
本篇将着重介绍双目测距的深度图计算原理和实现，关于相机标定+双目校正+双目匹配等内容，鄙人已经整理了一个完整的项目内容，详情请查看博客《双目三维重建系统(双目标定+立体校正+双目测距+点云显示)Python》
1.双目测距原理
原理很简单，利用了相似三角形计算距离，所以双目测距的主要任务在于前期摄像头的定标、双目图像点的特征匹配上。
2.双目测距实现
实现双目测距最关键的步骤是获得视差图，视差图计算请查看博客《双目三维重建系统(双目标定+立体校正+双目测距+点云显示)Python》
得到了视差图之后，就可以计算像素深度了,在opencv中使用StereoRectify()函数可以得到一个重投影矩阵Q，它是一个4*4的视差图到深度图的映射矩阵(disparity-to-depth mapping matrix )，使用Q矩阵和cv2.reprojectImageTo3D即可实现将像素坐标转换为三维坐标，该函数会返回一个3通道的矩阵，分别存储X、Y、Z坐标（左摄像机坐标系下）。
def reprojectImageTo3D(disparity, Q, _3dImage=None, handleMissingValues=None, ddepth=None):
"""
:param disparity: 输入视差图
:param Q: 输入4*4的视差图到深度图的映射矩阵，即重投影矩阵 通过stereoRectify得到
(disparity-to-depth mapping matrix)
:param _3dImage: 映射后存储三维坐标的图像
contains 3D coordinates of the point (x,y) computed from the disparity map
:param handleMissingValues: 计算得到的非正常值是否给值，如果为true则给值10000
:param ddepth: 输出类型 -1 即默认为CV_32FC3 还可以是 CV_16S, CV_32S, CV_32F
:return:
"""运算如下：
![[X,Y,Z,W] ^T=Q*[x,y,disparity(x,y),1]^ T](https://aitechtogether.com/wp-content/uploads/2023/03/gif-624.gif)
重投影矩阵Q中
和
为左相机主点在图像中的坐标，f为焦距，
为两台相机投影中心间的平移（负值），即基线baseline,相当于平移向量T[0], 
是右相机主点在图像中的坐标。
其中Z即是深度距离depth：
其中 f 为焦距长度（像素焦距），b为基线长度，d为视差，
与
为两个相机主点的列坐标。
这里有个地方需要注意，如果获得视差图像是CV_16S类型的，这样的视差图的每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16。在进行映射后应该乘以16，以获得毫米级真实位置。
def get_depth(self, disparity, Q, scale=1.0, method=False):
"""
reprojectImageTo3D(disparity, Q),输入的Q,单位必须是毫米(mm)
:param disparity: 视差图
:param Q: 重投影矩阵Q=[[1, 0, 0, -cx]
[0, 1, 0, -cy]
[0, 0, 0, f]
[1, 0, -1/Tx, (cx-cx`)/Tx]]
其中f为焦距，Tx相当于平移向量T的第一个参数
:param scale: 单位变换尺度,默认scale=1.0,单位为毫米
:return depth:ndarray(np.uint16),depth返回深度图, 即距离
"""
# 将图片扩展至3d空间中，其z方向的值则为当前的距离
if method:
points_3d = cv2.reprojectImageTo3D(disparity, Q) # 单位是毫米(mm)
x, y, depth = cv2.split(points_3d)
else:
# baseline = abs(camera_config["T"][0])
baseline = 1 / Q[3, 2] # 基线也可以由T[0]计算
fx = abs(Q[2, 3])
depth = (fx * baseline) / disparity
depth = depth * scale
# depth = np.asarray(depth, dtype=np.uint16)
depth = np.asarray(depth, dtype=np.float32)
return depth- 运行
demo.py后，鼠标点击图像任意区域，终端会打印对应距离 - 鼠标点击手部区域会打印距离摄像头的距离约633mm,即0.63米，还是比较准的
(x,y)=(203,273),depth=633.881653mm
(x,y)=(197,329),depth=640.386047mm
(x,y)=(222,292),depth=631.549072mm
(x,y)=(237,270),depth=630.389221mm
(x,y)=(208,246),depth=652.560669mm
双目测距的精度 说明：
根据上式可以看出，某点像素的深度精度取决于该点处估计的视差d的精度。假设视差d的误差恒定，当测量距离越远，得到的深度精度则越差，因此使用双目相机不适宜测量太远的目标。
如果想要对与较远的目标能够得到较为可靠的深度，一方面需要提高相机的基线距离，但是基线距离越大，左右视图的重叠区域就会变小，内容差异变大，从而提高立体匹配的难度，另一方面可以选择更大焦距的相机，然而焦距越大，相机的视域则越小，导致离相机较近的物体的距离难以估计。
理论上，深度方向的测量误差与测量距离的平方成正比，而X/Y方向的误差与距离成正比；而距离很近时，由于存在死角，会导致难以匹配的问题；想象一下，如果你眼前放置一块物体，那你左眼只能看到物体左侧表面，右眼同理只能看到物体右侧表面，这时由于配准失败，导致视差计算失败；这个问题在基线越长，问题就越严重
3.3D点云显示
恢复三维坐标后，就可以使用python-pcl和Open3D库显示点云图
PCL Python版比较难安装，如果安装不了，那可以采用Open3D勉强凑合使用吧
如下图所示，你可以用鼠标旋转坐标轴，放大点云
| 2D-RGB | Open3D点云显示 | PCL点云显示 |
 |  |  |
def show_3dcloud_for_open3d(self, frameL, frameR, points_3d):
"""
使用open3d显示点云
:param frameL:
:param frameR:
:param points_3d:
:return:
"""
if self.use_open3d:
x, y, depth = cv2.split(points_3d) # depth = points_3d[:, :, 2]
self.open3d_viewer.show(color_image=frameL, depth_image=depth)
def show_3dcloud_for_pcl(self, frameL, frameR, points_3d):
"""
使用PCL显示点云
:param frameL:
:param frameR:
:param points_3d:
:return:
"""
if self.use_pcl:
self.pcl_viewer.add_3dpoints(points_3d, frameL)
self.pcl_viewer.show()4.双目摄像头实现双目测距（Python）
【完整的项目代码】双目三维重建系统(双目标定+立体校正+双目测距+点云显示)Python源码
5.双目摄像头实现双目测距（OpenCV C++）
目前已经实现了OpenCV C++版本的双目测距，与Python版本效果几乎一致，
详细请查看鄙人另一篇博客《双目摄像头实现双目测距（C++）》OpenCV C++双目三维重建：双目摄像头实现双目测距
6.双目摄像头实现双目测距（Android版本）
如果你需要Android版本的双目测距， 请查看鄙人另一篇博客《Android OpenCV实现双目三维重建：双目摄像头实现双目测距》
文章出处登录后可见！
