使用MATLAB工具箱标定双目相机并在Python中获取三维坐标记录

Step

双目相机采集图像

50对获取关注距离处的棋盘格图片

细节：必须保持整个棋盘格出现在两个相机视野中，也不可有遮挡，尽量让标定板不要太歪，尽量让标定板每次出现在视野中的不同位置。

最好是png格式、棋盘表面和相机成像平面夹角小于45度

尽量采集各种不同的棋盘图像。镜头的畸变从图像中心径向增加，并且有时在图像各帧上表现不均匀，为了获取图像的畸变信息，棋盘应当处在图像的各种不同边缘处

import cv2
import os

# 创建保存图像的文件夹
left_folder = "F:\\camera\\images\\left"
right_folder = "F:\\camera\\images\\right"
os.makedirs(left_folder, exist_ok=True)
os.makedirs(right_folder, exist_ok=True)

# 打开左侧摄像头
left_camera = cv2.VideoCapture(1)  # 根据实际情况修改索引

# 打开右侧摄像头
right_camera = cv2.VideoCapture(2)  # 根据实际情况修改索引

# 检查摄像头是否成功打开
if not left_camera.isOpened() or not right_camera.isOpened():
    print("无法打开摄像头。请检查连接或选择正确的摄像头索引。")
    exit()

# 初始化图像编号
image_count = 1

# 循环读取并显示摄像头的帧
while True:
    # 读取左侧摄像头的帧
    ret1, left_frame = left_camera.read()

    # 读取右侧摄像头的帧
    ret2, right_frame = right_camera.read()

    # 检查帧是否成功读取
    if not ret1 or not ret2:
        print("无法读取摄像头帧。")
        break

    # 在窗口中显示左侧摄像头的帧
    cv2.imshow("Left Camera Feed", left_frame)

    # 在窗口中显示右侧摄像头的帧
    cv2.imshow("Right Camera Feed", right_frame)

    # 等待用户按键
    key = cv2.waitKey(1)

    # 如果用户按下 's' 键，保存图像
    if key == ord('s'):
        left_filename = os.path.join(left_folder, f"left_{image_count}.jpg")
        right_filename = os.path.join(right_folder, f"right_{image_count}.jpg")

        # 保存左侧摄像头的图像
        cv2.imwrite(left_filename, left_frame)
        print(f"Saved {left_filename}")

        # 保存右侧摄像头的图像
        cv2.imwrite(right_filename, right_frame)
        print(f"Saved {right_filename}")

        # 增加图像编号
        image_count += 1

    # 按下 'q' 键退出循环
    elif key == ord('q'):
        break

# 释放摄像头资源和关闭窗口
left_camera.release()
right_camera.release()
cv2.destroyAllWindows()

Matlab标定

左右图像对应放入文件夹中

matlab – APP- stereo Camera Calibrator-Add images 将两个文件夹的位置录入如下

加载图像，进行标定

加载完成后，浏览每组图片，检查标定板上的坐标轴朝向是否一致，通常情况下没有错误，但偶尔会有！若有，则删除对应的该组图片。

设定将 Radial Distortion: 3 coeefficients、Tangential Distortion、Skew全部勾选，点击Calibrate进行标定。

查看误差删除部分图像

在“Reprojection Errors”中查看误差，虚线为平均误差，将误差很大的图片，在左侧缩略图中去掉该组图片，然后Calibrate重新标定

在“Camera-centric”中查看相机与图片的空间位置是否与你实际拍摄时候的位置差不多，因为当误差太大的时候，这里的左相机、右相机、图片三者之间的位置关系真的很离谱！

提取参数导出文件

Export Camera Parameters提取到工作区

双击“IntrinsicMatrix” 查看内参矩阵，注意参数文件中的所有矩阵都是转置后的矩阵，在使用的时候要转置过来！！！

后期查看参数可直接打开该文件，无需再次标定！

标定结果应用到OpenCV

通过Matlab标定后得到的旋转矩阵、平移矩阵和内参矩阵，都需要转置以后才可以给OpenCV用，另外OpenCV畸变向量中前5个畸变系数的依次是：[k1, k2, p1, p2, k3]

数据手动输入到python代码中

试图在python中导入mat文件遇到matlabopaque，要手动输入数值（这个真的很不智能）

上网搜索最后的结果如下：

双目图像立体矫正映射

使用OpenCV库中的stereoRectify函数进行双目图像矫正

stereoRectify 用于计算校正矩阵和新的内参矩阵， initUndistortRectifyMap 用于计算映射表。remap 函数将原始图像应用这些映射表，从而得到校正后的图像。

立体矫正代码如下：

import cv2
import numpy as np
from scipy.io import loadmat

# 读取摄像机参数
# 除了dist_coeffs参数 需要手动调整一下；其余参数直接从matlab中复制到 chatgpt中（将[;;]调整为nparray的格式） 再复制过来 

camera_matrix_matlab = np.array([[4.619621018035629e+02, 0, 0],
                                  [0.018476340660536, 4.621684179137602e+02, 0],
                                  [2.836848709805677e+02, 2.324822555597804e+02, 1]])
camera_matrix_left = camera_matrix_matlab.T

dist_coeffs_left = np.array([0.185513328447228,-0.322479094634718,0.001000193512043,0.003188955563855,0.216022407530830])   
                            # [k1_left, k2_left, p1_left, p2_left, k3_left]

camera_matrix_matlab = np.array([[4.634851481918351e+02, 0, 0],
                        [0.746347967852480, 4.632896762744298e+02, 0],
                        [2.785712060644789e+02, 2.419945122070812e+02, 1]])
camera_matrix_right = camera_matrix_matlab.T

dist_coeffs_right = np.array([0.181290329537866,-0.282819177030661,0.001664635004817,0.002254013510170,0.160055872669854])

rotation_of_camera2_matlab = np.array([[0.999998966172461, 6.294797405647547e-06, -0.001437920159483],
                                       [-1.006345059697613e-05, 0.999996565349587, -0.002620913534745],
                                       [0.001437898722610, 0.002620925295611, 0.999995531588946]])
R = rotation_of_camera2_matlab.T

translation_of_camera2 = [-25.126404451655620, -0.252195810295758, -0.148873903046680] 
T = np.array([[translation_of_camera2[0]],
              [translation_of_camera2[1]],
              [translation_of_camera2[2]]])

# 图像大小
image_size = (640, 480)

# 计算立体校正映射
R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(camera_matrix_left, dist_coeffs_left,
                                           camera_matrix_right, dist_coeffs_right,
                                           image_size, R, T)

# 计算映射表
map_left_x, map_left_y = cv2.initUndistortRectifyMap(camera_matrix_left, dist_coeffs_left, R1, P1, image_size, cv2.CV_32FC1)
map_right_x, map_right_y = cv2.initUndistortRectifyMap(camera_matrix_right, dist_coeffs_right, R2, P2, image_size, cv2.CV_32FC1)

# 原始双目图像
I1 = cv2.imread("F:\\camera\\images\\left\\left_2.jpg")
I2 = cv2.imread("F:\\camera\\images\\right\\right_2.jpg")

# 应用立体校正映射
rectified_left = cv2.remap(I1, map_left_x, map_left_y, cv2.INTER_LINEAR)  # 或使用cv2.INTER_CUBIC
rectified_right = cv2.remap(I2, map_right_x, map_right_y, cv2.INTER_LINEAR)

# 保存矫正后的图像
cv2.imwrite("F:\\camera\\undistorted-py\\left_2.jpg", rectified_left)
cv2.imwrite("F:\\camera\\undistorted-py\\right_2.jpg", rectified_right)

其中，针对remap函数中的插值参数选择，有cv2.INTER_LINEAR及 cv2.INTER_CUBIC。

通常来说，cv2.INTER_LINEAR 是一种较快的插值方法，而 cv2.INTER_CUBIC 是一种更慢但更精细的插值方法。如果在缩放图像或者进行其他需要高质量插值的任务时，可能会选择后者。

我的效果：查看双线性和双三次插值得到的矫正后图像的SSIM，双三次的图像质量并没有更好，但是目前还是选择CUBIC。

极线约束：立体图像校正将图像投影到共同的图像平面上，使得对应的点具有相同的行坐标

在水平方向均匀的画线，以一组为例，代码如下：

import cv2
import numpy as np
import matplotlib.pyplot as plt

# 矫正前
I1 = cv2.imread("F:\\camera\\images\\left\\left_2.jpg")
I2 = cv2.imread("F:\\camera\\images\\right\\right_2.jpg")

# 矫正后-matlab
J1 = cv2.imread("F:\\camera\\undistorted\\left_2.jpg")
J2 = cv2.imread("F:\\camera\\undistorted\\right_2.jpg")

# 堆叠图像
stacked_image1 = np.hstack((I1, I2))
stacked_image2 = np.hstack((J1, J2))

height, width = stacked_image1.shape[:2]   # matlab的 长宽都变了 其实是需要针对matlab后的高度进行修改的  但是暂时还是按照原来的高度进行画线的 www

# 在水平方向绘制红色线
num_lines = 8
line_color = (0, 0, 255) 

# 计算线的间距
line_spacing = height // (num_lines + 1)

# 在堆叠后的图像上绘制红色线
for i in range(1, num_lines + 1):
    y_position = i * line_spacing
    cv2.line(stacked_image1, (0, y_position), (width, y_position), line_color, 2)
    cv2.line(stacked_image2, (0, y_position), (width, y_position), line_color, 2)

plt.axis('off')
plt.imshow(cv2.cvtColor(stacked_image1, cv2.COLOR_BGR2RGB))
plt.title('Original Images')
plt.show()

plt.axis('off')
plt.imshow(cv2.cvtColor(stacked_image2, cv2.COLOR_BGR2RGB))
plt.title('Rectified Images Matlab')
plt.show()

查看效果如下图所示：（相同的像素在同一条水平线上）

图像大小： original image 640×480 Matlab得到的图像 588×426
python得到的图像 640×480 但是存在四周黑色

得到世界坐标系下的坐标——三角测量

双目图像得到世界坐标系下坐标 2种方法：

1.双目图像立体匹配（图像信息）——得到视差信息——三维坐标

2.双目图像中关键点匹配（图像坐标信息）——三角测量/三角化——三维坐标

使用cv2.triangulatePoints()函数来进行三角测量。这个函数需要两个摄像机的投影矩阵（由相机内外参矩阵计算得到）和匹配的关键点像素坐标作为输入，并返回三维坐标，具体代码如下：

# 从立体矫正后的图像找匹配的关键点坐标
# 已知的匹配的关键点坐标  （这里先取的是棋盘格上的一个正方形的左上右上）
points1 = np.array([[298, 134], [321, 134]], dtype=np.float32)
points2 = np.array([[261, 134], [285, 134]], dtype=np.float32)

# P1 P2 投影矩阵在上面有得到

# 世界坐标系下
points_world=[]
# 三角测量
for i in range(len(points1)):
    points_3d = cv2.triangulatePoints(P1, P2, points1[i], points2[i])
    # 将齐次坐标转换为非齐次坐标
    points_3d /= points_3d[3]
    print(points_3d)
    points_world.append(points_3d)

# 计算欧氏距离
distance = np.linalg.norm(points_world[1] - points_world[0])
print(distance)   

# 单位mm（原本标定棋盘格单格边长 是1.8cm）结果相差不大

数据记录：

标定板与摄像头距离：0.3-0.5m（暂定）

棋盘格单格边长：1.8cm

当前棋盘格图像采集使用的是jpg格式的

后面再一次标定的时候Tips：

1.多个双目摄像头要注意参数文件和摄像头编号对应（暂时算解决了）

使用两个双目摄像头采集同样的照片进行矫正，发现使用之前标定的参数都能符合极线约束，说明这两个双目的标定参数是差不多的（偷懒只标定一个，共用吧）

2. 确定双目摄像头的左右（已解决）如下

## 调试摄像头的索引  索引会一直变化 使用的时候需要注意 需要先确定下摄像头标号

# 查看在设备管理器中的摄像头名称
from pygrabber.dshow_graph import FilterGraph
graph = FilterGraph()
print(graph.get_input_devices())   # RGB Camera 左侧摄像头 R1 右侧摄像头  WebCam电脑自带摄像头

使用之前先配对同一个双目的左右（因为打印出来的信息不是完全完善，但是能区分左右以及电脑自带的摄像头）

3. 如果后期出现问题需要考虑是否要把k3去掉（看的论文里没有使用k3 from 索）

暂时使用k3没有啥问题

参考

https://blog.csdn.net/weixin_43662044/article/details/134552839

matlab查看图像矫正后的可视化效果代码如下所示

clear all
clc

I1 = imread("F:\camera\images\left\left_2.jpg");%读取左右图片
I2 = imread("F:\camera\images\right\right_2.jpg");
figure
imshowpair(I1, I2, 'montage');
title('Original Images');

%加载stereoParameters对象。
load('stereoParams1.mat');%加载你保存的相机标定的mat

[J1, J2] = rectifyStereoImages(I1, I2, stereoParams1);
% 保存校正后的图像
imwrite(J1, 'F:\camera\undistorted\left_2.jpg');
imwrite(J2, 'F:\camera\undistorted\right_2.jpg');

figure
imshowpair(J1, J2, 'montage');
title('Undistorted Images');

figure; imshow(cat(3, J1(:,:,1), J2(:,:,2:3)), 'InitialMagnification', 50);%图像显示50%
% 校正后需要看一下校正的结果，把第一个图像的红通道和第二个图像的蓝通道和绿通道结合构成图像
% 可以看出浅浅的叠影，红色的就是我们的图1，其他色的就是我们的图2