双目立体视觉

立体双目视觉
三维重建步骤：相机标定，图像矫正，立体匹配，深度计算。
1.相机标定与图像矫正

import numpy as np
import cv2
import glob

# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

images = glob.glob('*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (7,6),None)

    # If found, add object points, image points (after refining them)
    if ret == True:
        objpoints.append(objp)

        cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners)

        # Draw and display the corners
        cv2.drawChessboardCorners(img, (7,6), corners2,ret)
        cv2.imshow('img',img)
        cv2.waitKey(500)

cv2.destroyAllWindows()

然后返回相机外参内参和畸变系数

ret, k, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

k是相机内参，dist为畸变系数，r，t为旋转平移矩阵。

然后进行畸变矫正

img = cv2.imread('left12.jpg')
h,  w = img.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)

# crop the image
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('calibresult.png',dst)

2.立体匹配
2.1 对极几何

C0和C1为光心，P0和P1为极点，PC0C1平面为极平面，极平面与两个成像面的交线为极线。
左图中P0对应的点一定在右图的极线上，将搜索极点的二维搜索转化为一维搜索。
极线的关系：

E为本征矩阵，只与两相机模型有关。

3. 视差与深度计算

视差为$D=Xr-Xt$。

摄像头不平行时如何计算？进行立体矫正。

opencv立体矫正步骤：

矫正实现方式：https://blog.csdn.net/qq_22424571/article/details/80836342

2.2 立体匹配
匹配步骤：
1、匹配代价计算（Matching Cost Computation:CC）
2、代价聚合（Cost Aggregation:CA）
3、视差计算（Disparity Computation ）
4、视差精化（Disparity Refinement ）：对上一步得到的粗估计的视差图进行精确计算，策略有很多，例如plane fitting，BP，动态规划等。

全局匹配：
建立全局能量函数，通过最小化全局能量函数得到最优视差值。全局匹配算法得到的结果比较准确，但是其运行时间比较长，不适合实时运行。主要的算法有图割（graph cuts）、信念传播（belief propagation）、动态规划（Dynamic Programming ）等算法。

局部匹配：
基本原理是给定在一幅图像上的某一点，选取该像素点邻域内的一个子窗口，在另一幅图像中的一个区域内，根据某种相似性判断依据，寻找与子窗口图像最为相似的子图，而其匹配的子图中对应的像素点就为该像素的匹配点。通常方法有SAD、SSD、NCC等等。

匹配代价计算：