双目相机求解三维坐标详述

小鹿( ﹡ˆoˆ﹡ )

已于 2024-07-05 00:02:40 修改

阅读量57

点赞数 1

分类专栏： Python 文章标签：计算机视觉 opencv 图像处理

于 2022-05-06 21:35:23 首次发布

本文链接：https://blog.csdn.net/qq_44771627/article/details/124616704

版权

第七节、双目视觉之空间坐标计算 - 大奥特曼打小怪兽 - 博客园 (cnblogs.com)https://www.cnblogs.com/zyly/p/9373991.html#_label1_4

1、reprojectImageTo3D函数求三维坐标

该函数将视差图，通过投影矩阵Q，得到一副映射图，图像大小与视差图相同，且每个像素具有三个通道，分别存储了该像素位置在相机坐标系下的三维点坐标在x, y, z三个轴上的值，即每个像素的在相机坐标系下的三维坐标。

void cv::reprojectImageTo3D( 
	InputArray disparity, 	//视差图像
	OutputArray _3dImage,	//映射后存储三维坐标的图像
	InputArray Q,			//重投影矩阵 通过stereoRectify得到
	bool handleMissingValues = false, //计算得到的非正常值是否给值，如果为true则给值10000
	int ddepth = -1			//输出类型 -1 即默认为CV_32FC3 还可以是 CV_16S, CV_32S, CV_32F
)

1.1 函数原理

相机坐标系下的三维坐标求解原理：

深度Z=f*B/d(f是焦距，B是基线也就是Tx，d是视差)

X=x*Z/f

Y=y*Z/f(x,y是相机坐标)

$\large {\color{Magenta} {\color{Green} }}matrix = [[img_x], [img_y], [disp[img_y, img_x]], [1]]$

matrix中的imgx,imgy是校正后像素坐标，通过计算Q*matrix，然后进行归一化，满足上式。

1.2 投影矩阵Q的求解

此时的投影矩阵Q是通过stereoRectify函数求得，

# cameraMatrix1-输入矩阵，matlab双目标定后得到的左相机内参
# distCoeffs1-输入矩阵，matlab双目标定后得到的左相机畸变矩阵
# cameraMatrix2-输入矩阵，matlab双目标定后得到的右相机内参
# distCoeffs2-输入矩阵，matlab双目标定后得到的右相机畸变矩阵

# R,T-输入矩阵， 以左相机坐标系为参考系，右相机相对于左相机的旋转和平移
# R1-输出矩阵，第一个摄像机的校正变换矩阵（旋转变换）
# R2 - 输出矩阵，第二个摄像机的校正变换矩阵（旋转矩阵）

R1,R2是左右相机坐标系校正到同一极线下的旋转矩阵
# P1 - 输出矩阵，第一个摄像机在新坐标系下的投影矩阵
# P2 - 输出矩阵，第二个摄像机在新坐标系下的投影矩阵

P1，P2两个矫正畸变后左右相机坐标系的相机投影矩阵
# Q - 4 * 4的深度差异映射矩阵
# alpha: -1或者不设置则使用自动剪切，0的时候没有黑边，1的时候保留所有原图像素，会有黑边。
CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1,
                                 InputArray cameraMatrix2, InputArray distCoeffs2,
                                 Size imageSize, InputArray R, InputArray T,
                                 OutputArray R1, OutputArray R2,
                                 OutputArray P1, OutputArray P2,
                                 OutputArray Q, int flags = CALIB_ZERO_DISPARITY,
                                 double alpha = -1, Size newImageSize = Size(),
                                 CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 );
注意：R,T,R1,R2是不同的。

由下面世界坐标系到像素坐标系的求解可以看出，投影矩阵P=K*[R,T],K是相机内参，R,T是相机外参。

        所以对于未校正的原图，R,T是以左相机坐标系为参考系，右相机相对于左相机的旋转和平移，左右相机的投影矩阵分别是：

[1 0 0 0]
PL=cameraMatrix1*[0 1 0 0]
[0 0 1 0]

PR=cameraMatrix1*[R,T]

对于校正后的图像，R1,R2是左右相机坐标系校正到同一极线下的旋转矩阵，左右相机的投影矩阵分别是：

（P1，P2是通过stereoRectify函数求得矫正畸变后左右相机坐标系的相机投影矩阵）

PL=P1

                                                       PR=P2

1.3 三维坐标求解

思考1：

因为reprojectImageTo3D函数是通过视差图和Q求解三维坐标，而视差图是通过校正后的左右视图得到的，所以想要通过人机交互系统完成对三维坐标求取，就需要得到的是校正后的左右视图的像素坐标，而不是原图的像素坐标。

# coding:utf-8

import cv2
import time
from PIL import Image
import numpy as np
from cv2 import FileStorage
from scipy.linalg import lstsq
# //左相机内参矩阵
from matplotlib import pyplot as plt

global Left_rectified, Right_rectified
global imgL
global imgR

global cameraMatrix1
# //左相机畸变系数矩阵
global distCoeffs1
# //右相机内参矩阵
global cameraMatrix2
# //右相机畸变系数矩阵
global distCoeffs2
# Mat R, T, E, F; //R 旋转矢量 T平移矢量 E本征矩阵 F基础矩阵 Q重投影矩阵
global R
global T
global E
global F
global Q
global PL
global PR
# 图像宽度
global width
# 图像高度
global height
# 标定板的宽度
global board_width
# 标定板的高度
global board_height


def readFile(filename):
    global cameraMatrix1
    global cameraMatrix2
    global distCoeffs1
    global distCoeffs2
    global R
    global T
    global E
    global F
    global width
    global height
    global board_width
    global board_height
    fs = FileStorage(filename, cv2.FileStorage_READ)
    width = fs.getNode("width").real()
    height = fs.getNode("height").real()
    board_width = fs.getNode("board_width").real()
    board_height = fs.getNode("board_height").real()
    cameraMatrix1 = fs.getNode("cameraMatrix1").mat()
    distCoeffs1 = fs.getNode("distCoeffs1").mat()
    cameraMatrix2 = fs.getNode("cameraMatrix2").mat()
    distCoeffs2 = fs.getNode("distCoeffs2").mat()
    R = fs.getNode("R").mat()
    T = fs.getNode("T").mat()
    E = fs.getNode("E").mat()
    F = fs.getNode("F").mat()
    fs.release()


def rectify(imgL, imgR):
    """
    校正图像(显示图像时窗口名不能是中文名)
    :param imgL: 左视图(灰度图像)
    :param imgR: 右视图(灰度图像)
    :return: 返回原始图像和校正后的图像
    """
    # 双目相机的标定
    # 设置标志位为cv2.CALIB_FIX_INTRINSIC，这样就会固定输入的cameraMatrix和distCoeffs不变，只求解𝑅,𝑇,𝐸,𝐹
    flags = 0
    flags |= cv2.CALIB_ZERO_DISPARITY
    # 利用stereoRectify()计算立体校正的映射矩阵
    rectify_scale = -1  # 设置为0的话，对图片进行剪裁，设置为1则保留所有原图像像素
    global Q
    global PL
    global PR
    # R,T: 是由相机1变换到相机2的变换矩阵，按照视觉这边的习惯，一般世界坐标系下的点变换到相机坐标系下的点
    # R1-输出矩阵，第一个摄像机的校正变换矩阵（旋转变换）
    # R2 - 输出矩阵，第二个摄像机的校正变换矩阵（旋转矩阵）
    # P1 - 输出矩阵，第一个摄像机在新坐标系下的投影矩阵
    # P2 - 输出矩阵，第二个摄像机在新坐标系下的投影矩阵 两个矫正畸变后的相机投影矩阵，相对的坐标系还是原来的相机所在坐标系
    # Q - 4 * 4的深度差异映射矩阵
    # rectify_scale: -1或者不设置则使用自动剪切，0的时候没有黑边，1的时候保留所有原图像素，会有黑边
    RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
                                                      imgL.shape[::-1], R, T,
                                                      rectify_scale, (0, 0))
    # 利用initUndistortRectifyMap函数计算畸变矫正和立体校正的映射变换，实现极线对齐。
    Left_Stereo_Map = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, RL, PL,
                                                  imgL.shape[::-1], cv2.CV_16SC2)
    Right_Stereo_Map = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, RR, PR,
                                                   imgL.shape[::-1], cv2.CV_16SC2)
    Left_rectified = cv2.remap(imgL, Left_Stereo_Map[0], Left_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT,
                               0)  # 使用remap函数完成映射

    Right_rectified = cv2.remap(imgR, Right_Stereo_Map[0], Right_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT,
                                0)
    imgRectify = np.hstack([Left_rectified, Right_rectified])
    imgOriginal = np.hstack([imgL, imgR])
    # 在已经极线对齐的图片上均匀画线
    for i in range(1, int(imgL.shape[0] / 20)):
        cv2.line(imgRectify, (0, 20 * i), (2 * imgL.shape[1], 20 * i), (0, 0, 0), 1, cv2.LINE_8)
        cv2.line(imgOriginal, (0, 20 * i), (2 * imgL.shape[1], 20 * i), (0, 0, 0), 1, cv2.LINE_8)
    # cv2.namedWindow("Rectify", cv2.WINDOW_FREERATIO)
    # cv2.namedWindow("Original", cv2.WINDOW_FREERATIO)
    # cv2.imshow("Rectify", imgRectify)
    # cv2.imshow("Original", imgOriginal)
    return Left_rectified, Right_rectified


# 视差计算
def disparity_SGBM(left_image, right_image, down_scale=False):
    """
    通过SGBM方法得到视差图
    :param left_image: 极线校正左图像
    :param right_image: 极线校正右图像
    :param down_scale:
    :return: 返回视差图
    """
    # SGBM匹配参数设置
    if left_image.ndim == 2:
        img_channels = 1
    else:
        img_channels = 3
    blockSize = 3
    param = {'minDisparity': 0,
             'numDisparities': 240,  # 视差搜索范围
             'blockSize': blockSize,  # blockSize，块匹配的大小，应该为奇数，在3~11的范围
             'P1': 8 * img_channels * 9 ** 2,  # 值越大越平滑
             'P2': 32 * img_channels * 9 ** 2,
             'disp12MaxDiff': 1,  # 左右一致性检测允许的最大误差值
             'preFilterCap': 32,  # 映射滤波器
             'uniquenessRatio': 1,  # 唯一性检测
             'speckleWindowSize': 100,  # 视差图连通区域像素点个数的大小，<speckleWindowSize则为噪声点
             'speckleRange': 32,  # >speckleRange：则为不连通
             'mode': cv2.STEREO_SGBM_MODE_SGBM
             }

    # 构建SGBM对象
    sgbm = cv2.StereoSGBM_create(**param)

    # 计算视差图
    size = (left_image.shape[1], left_image.shape[0])
    if down_scale == False:
        disparity_left = sgbm.compute(left_image, right_image)
        disparity_left = cv2.normalize(disparity_left, disparity_left, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
        disparity_left = cv2.medianBlur(disparity_left, 3)
        disparity_left = np.divide(disparity_left.astype(np.float32), 16.)  # 除以16得到真实视差图

        disparity_right = sgbm.compute(right_image, left_image)
        disparity_right = np.divide(disparity_right.astype(np.float32), 16.)  # 除以16得到真实视差图
    else:
        left_image_down = cv2.pyrDown(left_image)
        right_image_down = cv2.pyrDown(right_image)
        factor = size[0] / left_image_down.shape[1]
        disparity_left_half = sgbm.compute(left_image_down, right_image_down)
        disparity_right_half = sgbm.compute(right_image_down, left_image_down)
        disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
        disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
        disparity_left *= factor
        disparity_right *= factor

    return disparity_left, disparity_right


# 计算三维坐标，并删除错误点
def threeD(disp, img_x, img_y, Q):
    ""&

最低0.47元/天解锁文章

小鹿( ﹡ˆoˆ﹡ )

关注

1
点赞
踩
1

收藏

觉得还不错? 一键收藏
打赏
0
评论
双目相机求解三维坐标详述

1、reprojectImageTo3D函数求三维坐标该函数将视差图，通过投影矩阵Q，得到一副映射图，图像大小与视差图相同，且每个像素具有三个通道，分别存储了该像素位置在相机坐标系下的三维点坐标在x, y, z三个轴上的值，即每个像素的在相机坐标系下的三维坐标。void cv::reprojectImageTo3D( InputArray disparity, //视差图像 OutputArray _3dImage, //映射后存储三维坐标的图像 InputArray Q, //
复制链接

扫一扫