一、双目立体匹配算法

在opencv中用的比较多的双目立体匹配算法有两种：BM和SGBM。SGBM是BM立体匹配算法的优化版，属于半全局匹配，相对于BM花的时间要更多，但效果优于BM。本文使用的是SGBM半全局匹配方式。
步骤：
1.打开相机，获取到左目和右目的图像；
2.矫正畸变；
3.图像灰度化；
4.立体匹配，输出结果。

代码步骤

导入所需的第三方库

import cv2
import numpy as np
# 畸变矫正脚本
import camera_config

矫正畸变

left_remap = cv2.remap(imgLeft, camera_config.left_map1, camera_config.left_map2, cv2.INTER_LINEAR)
right_remap = cv2.remap(imgRight, camera_config.right_map1, camera_config.right_map2, cv2.INTER_LINEAR)

灰度化

imgL_gray = cv2.cvtColor(left_remap, cv2.COLOR_BGR2GRAY)
imgR_gray = cv2.cvtColor(right_remap, cv2.COLOR_BGR2GRAY)

立体匹配

### 设置参数
#块大小必须为奇数(3-11)
blockSize = 5

img_channels = 2
num_disp = 16 * 8

param = {
    'preFilterCap': 63,     #映射滤波器大小，默认15
    "minDisparity" : 0,    #最小视差
    "numDisparities" : num_disp,    #视差的搜索范围，16的整数倍
    "blockSize" : blockSize,
    "uniquenessRatio" : 10,     #唯一检测性参数，匹配区分度不够，则误匹配(5-15)
    "speckleWindowSize" : 0,      #视差连通区域像素点个数的大小（噪声点）(50-200)或用0禁用斑点过滤
    "speckleRange" : 1,             #认为不连通(1-2)
    "disp12MaxDiff" : 2,        #左右一致性检测中最大容许误差值
    "P1" : 8 * img_channels * blockSize** 2,   #值越大，视差越平滑，相邻像素视差+/-1的惩罚系数
    "P2" : 32 * img_channels * blockSize** 2,  #同上，相邻像素视差变化值>1的惩罚系数
    # 'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
    }
## 开始计算深度图
left_matcher = cv2.StereoSGBM_create(**param)
left_disp = left_matcher.compute(imgL_gray, imgR_gray)
# 得到深度图
disp = cv2.normalize(dispL, dispL, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)

效果

二、wls滤波

从上面的结果来看，深度图存在很多“黑色区域”，“黑色区域”是没有正确匹配的地方，也就是说这部分是没有深度信息的，这种情况就是“不够稠密”。
在opencv的扩展包opencv-contrib里提供了一种WLS视差滤波的办法，可以使得重建更加稠密。
改进部分为立体匹配部分，在经过立体匹配后再做一步处理：

## 接上面的参数
left_matcher = cv2.StereoSGBM_create(**param)

right_matcher = cv2.ximgproc.createRightMatcher(left_matcher)

left_disp = left_matcher.compute(imgL_gray, imgR_gray)
right_disp = right_matcher.compute(imgR_gray, imgL_gray)
wls_filter = cv2.ximgproc.createDisparityWLSFilter(left_matcher)
# sigmaColor典型范围值为0.8-2.0
wls_filter.setLambda(8000.)
wls_filter.setSigmaColor(1.3)
wls_filter.setLRCthresh(24)
wls_filter.setDepthDiscontinuityRadius(3)

filtered_disp = wls_filter.filter(left_disp, imgL_gray, disparity_map_right=right_disp)

disp = cv2.normalize(filtered_disp, filtered_disp, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)

可以看到滤波之后的深度图变得相当稠密了。
但这种办法有利有弊：好处就是重建变得稠密，弊端就是这种办法会抹除掉一些细节深度，使得一块地方由原来的不同深度变成同一高度。
举个例子，上图中的杯子在实际观察中他应该是圆柱形，但滤波之后整个杯身将变成同一高度，即原来的高度差被抹除了。因此该方法请根据具体情况决定是否使用。

三、open3d点云重建

open3d提供了RGBD重建的办法，具体实现如下：
首先要准备一个相机内参文件（这样做比较省事），命名为camera_intrinsic.json，
根据左目相机内参矩阵：

fx	0	cx
0	fy	cy
0	0	1

以列为顺序写入：

{
    "width": 960,
    "height": 960,
    "intrinsic_matrix": [
        fx,
        0,
        0,
        0,
        fy,
        0,
        cx,
        cy,
        1
    ]
}

保存，作为相机内参文件。再在主程序中继续接入以下代码：

# 获取内参
intrinsic = o3d.io.read_pinhole_camera_intrinsic("camera_intrinsic.json")
# 转换图像
color_image = o3d.geometry.Image(left_remap)
depth_image = o3d.geometry.Image(disp)
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(color_image, depth_image, depth_trunc=4.0, convert_rgb_to_intensity=False)
# 根据RGBD图像重建
temp = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
pcd.points = temp.points
pcd.colors = temp.colors
# 显示效果
o3d.visualization.draw_geometries_with_editing([pcd], window_name="3D", width=1280, height=720)

四、OpenCV的点云重建

除此之外也可以用opencv自带的办法重建，这个重建速度要比open3d快很多。
处理时需要剪掉一些不合理的点云数据。

# 将h×w×3数组转换为N×3的数组
def hw3ToN3(points):
    height, width = points.shape[0:2]

    points_1 = points[:, :, 0].reshape(height * width, 1)
    points_2 = points[:, :, 1].reshape(height * width, 1)
    points_3 = points[:, :, 2].reshape(height * width, 1)

    points_ = np.hstack((points_1, points_2, points_3))

    return points_

def DepthColor2Cloud(points_3d, colors):
    rows, cols = points_3d.shape[0:2]
    size = rows * cols

    points_ = hw3ToN3(points_3d).astype(np.int16)
    colors_ = hw3ToN3(colors).astype(np.int64)

    # 颜色信息
    blue = colors_[:, 0].reshape(size, 1)
    green = colors_[:, 1].reshape(size, 1)
    red = colors_[:, 2].reshape(size, 1)
    # rgb = np.left_shift(blue, 0) + np.left_shift(green, 8) + np.left_shift(red, 16)
    rgb = blue + green + red

    # 将坐标+颜色叠加为点云数组
    pointcloud = np.hstack((points_, red/255., green/255., blue/255.)).astype(np.float64)

    # 删掉一些不合适的点
    X = pointcloud[:, 0]
    Y = pointcloud[:, 1]
    Z = pointcloud[:, 2]

    remove_idx1 = np.where(Z <= 0)
    remove_idx2 = np.where(Z > 1000)
    remove_idx3 = np.where(X > 1000)
    remove_idx4 = np.where(X < -1000)
    remove_idx5 = np.where(Y > 1000)
    remove_idx6 = np.where(Y < -1000)
    remove_idx = np.hstack((remove_idx1[0], remove_idx2[0], remove_idx3[0], remove_idx4[0], remove_idx5[0], remove_idx6[0]))

    pointcloud_1 = np.delete(pointcloud, remove_idx, 0)


    return pointcloud_1

上面的函数可以单独用一个脚本文件保存再导入。
主脚本文件加入以下代码：

threeD = cv2.reprojectImageTo3D(disp, camera_config.Q)
pointcloud = DepthColor2Cloud(threeD, left_remap)

# 转换为open3d的点云数据
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pointcloud[:,:3])
pcd.colors = o3d.utility.Vector3dVector(pointcloud[:,3:])
o3d.visualization.draw_geometries_with_editing([pcd], window_name="3D", width=1280, height=720)