下面是准确的代码：

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

# === 1. 加载标定参数 ===
params = loadmat(r"D:\2024LLY\Camera_calibration\calib_params.mat")

# 相机参数
m1, d1 = params['m1'], params['dist1']
m2, d2 = params['m2'], params['dist2']
R1, P1 = params['R1'], params['P1']
R2, P2 = params['R2'], params['P2']
map11, map12 = params['map11'], params['map12']
map21, map22 = params['map21'], params['map22']
Q = params['Q']

# === 2. 读取左右图像 ===
left_img = cv2.imread(r"D:\2024LLY\original data\2025.10.29\binocular\chessboard\left\Image_1.bmp")
right_img = cv2.imread(r"D:\2024LLY\original data\2025.10.29\binocular\chessboard\left\Image_1.bmp")

if left_img is None or right_img is None:
    raise FileNotFoundError("请检查左右图像路径是否正确")

# === 3. 使用预计算的映射进行立体校正 ===
left_rectified = cv2.remap(left_img, map11, map12, cv2.INTER_LINEAR)
right_rectified = cv2.remap(right_img, map21, map22, cv2.INTER_LINEAR)

cv2.imwrite(r"D:\2024LLY\Camera_calibration\rectified_left.png", left_rectified)
cv2.imwrite(r"D:\2024LLY\Camera_calibration\rectified_right.png", right_rectified)
print("✅ 已保存校正后的图像")


# === 3.1 生成带水平极线的对比图 ===
def draw_epilines(img, line_interval=40):
    img_with_lines = img.copy()
    h, w = img.shape[:2]
    for y in range(0, h, line_interval):
        cv2.line(img_with_lines, (0, y), (w, y), (0, 255, 0), 1)
    return img_with_lines

left_with_lines = draw_epilines(left_rectified)
right_with_lines = draw_epilines(right_rectified)

# 合并两个图像做对比（左右拼接）
rectified_compare = np.hstack((left_with_lines, right_with_lines))

# 保存
cv2.imwrite(r"D:\2024LLY\Camera_calibration\rectified_with_lines.png",
            rectified_compare)

print("📌 已保存带极线的校正对比图 rectified_with_lines.png")


# === 4. 计算视差图 ===
# 使用 StereoSGBM 算法
window_size = 5
min_disp = 0
num_disp = 320  # 必须是16的倍数
stereo = cv2.StereoSGBM_create(
    minDisparity=min_disp,
    numDisparities=num_disp,
    blockSize=window_size,
    P1=8 * 3 * window_size ** 2,
    P2=32 * 3 * window_size ** 2,
    disp12MaxDiff=10,
    uniquenessRatio=5,
    speckleWindowSize=50,
    speckleRange=2
)
disparity = stereo.compute(cv2.cvtColor(left_rectified, cv2.COLOR_BGR2GRAY),
                           cv2.cvtColor(right_rectified, cv2.COLOR_BGR2GRAY)).astype(np.float32) / 16.0

# 归一化显示
disp_vis = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX)
disp_vis = np.uint8(disp_vis)
cv2.imwrite(r"D:\2024LLY\Camera_calibration\disparity.png", disp_vis)
print("✅ 已保存视差图")

# === 5. 计算欧氏距离（硬编码两点）===
# 示例：选择两个像素点 (x1, y1) 和 (x2, y2)
pt1 = (354,300)
pt2 =  (384,299)

# 画点
cv2.circle(left_rectified, pt1, 5, (0,255,0), -1)
cv2.circle(right_rectified, pt1, 5, (0,0,255), -1)

# 保存
cv2.imwrite(r"D:\2024LLY\Camera_calibration\left_with_pt1.png", left_rectified)
cv2.imwrite(r"D:\2024LLY\Camera_calibration\right_with_pt1.png", right_rectified)


disp1 = disparity[pt1[1], pt1[0]]
disp2 = disparity[pt2[1], pt2[0]]

if disp1 <= 0 or disp2 <= 0:
    print(f"⚠️ 某个点的视差无效: disparity1={disp1}, disparity2={disp2}")
else:
    # 使用 Q 矩阵反投影到三维空间
    points_3D = cv2.reprojectImageTo3D(disparity, Q)
    p1_3d = points_3D[pt1[1], pt1[0]]
    p2_3d = points_3D[pt2[1], pt2[0]]

    dist = np.linalg.norm(p1_3d - p2_3d)
    print(f"🎯 点1 {pt1} disparity={disp1}")
    print(f"🎯 点2 {pt2} disparity={disp2}")
    print(f"📏 欧氏距离 = {dist:.3f} mm (或根据相机单位)")

print("✅ 完成所有步骤。")
# === 6. 可视化两个像素点及其距离 ===

# 复制原图
vis_img = left_rectified.copy()

# 画两个点
cv2.circle(vis_img, pt1, 6, (0, 0, 255), -1)   # 红色点
cv2.circle(vis_img, pt2, 6, (255, 0, 0), -1)   # 蓝色点

# 画连接线
cv2.line(vis_img, pt1, pt2, (0, 255, 0), 2)

# 写上三维距离信息
text = f"Distance: {dist:.2f} mm"
cv2.putText(vis_img, text, (30, 30), cv2.FONT_HERSHEY_SIMPLEX,
            1.0, (0, 255, 255), 2)

# 写上两个点的视差信息
cv2.putText(vis_img, f"pt1 disp={disp1:.2f}", (pt1[0] + 10, pt1[1] - 10),
            cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.putText(vis_img, f"pt2 disp={disp2:.2f}", (pt2[0] + 10, pt2[1] - 10),
            cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)

# 保存可视化结果
save_path = r"D:\2024LLY\Camera_calibration\distance_visualization.png"
cv2.imwrite(save_path, vis_img)

print(f"📌 已保存距离可视化图像：{save_path}")

下面是不正确的代码：

import cv2
import numpy as np
import scipy.io
import os

# 设置输出目录
output_folder = r"D:\2024LLY\Camera_calibration\results"
os.makedirs(output_folder, exist_ok=True)

# 加载 .mat 文件
mat_data = scipy.io.loadmat(r"D:\2024LLY\Camera_calibration\calib_params.mat")

# 获取映射表和重投影矩阵 Q
left_map1 = mat_data['map11']
left_map2 = mat_data['map12']
right_map1 = mat_data['map21']
right_map2 = mat_data['map22']
Q = mat_data['Q']

# 读取左右图像（彩色读入，保证3通道）
left_img = cv2.imread(r"D:\2024LLY\original data\2025.10.29\binocular\chessboard\left\Image_1.bmp")
right_img = cv2.imread(r"D:\2024LLY\original data\2025.10.29\binocular\chessboard\right\Image_1.bmp")

if left_img is None or right_img is None:
    raise FileNotFoundError("图像加载失败，请检查路径是否正确")

# 图像校正
left_rectified = cv2.remap(left_img, left_map1, left_map2, cv2.INTER_LINEAR)
right_rectified = cv2.remap(right_img, right_map1, right_map2, cv2.INTER_LINEAR)

cv2.imwrite(os.path.join(output_folder, "left_rectified.jpg"), left_rectified)
cv2.imwrite(os.path.join(output_folder, "right_rectified.jpg"), right_rectified)

# 计算视差图
stereo = cv2.StereoSGBM_create(
    minDisparity=0,
    numDisparities=16*18,
    blockSize=5,
    P1=8*3*5**2,
    P2=32*3*5**2,
    mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY
)
# 转灰度再计算视差
disparity = stereo.compute(cv2.cvtColor(left_rectified, cv2.COLOR_BGR2GRAY),
                           cv2.cvtColor(right_rectified, cv2.COLOR_BGR2GRAY)).astype(np.float32)/16.0

# 保存视差图
disp_color = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8)
cv2.imwrite(os.path.join(output_folder, "disparity.jpg"), disp_color)

# 使用 reprojectImageTo3D 生成整个场景的3D坐标
points_3D = cv2.reprojectImageTo3D(disparity, Q)

# ======== 多点对定义 ========
point_pairs = [
    ((354, 300), (384, 299)),
    ((264, 83), (291, 138)),
]

# ======== 可视化 ========
# 确认是彩色图，再用作绘制
if len(left_rectified.shape) == 2:  # 灰度图
    vis = cv2.cvtColor(left_rectified, cv2.COLOR_GRAY2BGR)
else:
    vis = left_rectified.copy()

for i, (p1, p2) in enumerate(point_pairs, start=1):
    p1_3d = points_3D[p1[1], p1[0]]  # 注意索引顺序 [y, x]
    p2_3d = points_3D[p2[1], p2[0]]

    if np.isfinite(p1_3d).all() and np.isfinite(p2_3d).all():
        distance_mm = np.linalg.norm(p1_3d - p2_3d)
        distance_cm = distance_mm / 10
        print(f"\n--- 点对{i} ---")
        print(f"点1 {p1} 3D坐标: {p1_3d}")
        print(f"点2 {p2} 3D坐标: {p2_3d}")
        print(f"欧氏距离: {distance_mm:.2f} mm ({distance_cm:.2f} cm)")

        # 可视化
        cv2.circle(vis, p1, 5, (0, 0, 255), -1)
        cv2.circle(vis, p2, 5, (0, 255, 0), -1)
        cv2.line(vis, p1, p2, (255, 0, 0), 2)
        cv2.putText(vis, f"D{i}:{distance_cm:.1f}cm", (p1[0]+10, p1[1]-10),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
    else:
        print(f"点对{i}视差无效，无法计算3D坐标")

# 保存可视化结果
cv2.imwrite(os.path.join(output_folder, "measurement_multi_reproject_fixed.bmp"), vis)
cv2.imshow("Result", vis)
cv2.waitKey(0)
cv2.destroyAllWindows()

不正确的原因：

1. 第一段读入的是彩色图，第二段读入的是灰度图；
2. 我将第二段的灰度图改为彩色图后，仍然不准确，原因是没有修改对应的p1p2的惩罚项，在 StereoSGBM 中，P1/P2 是根据图像通道数来设置的，公式是：

   

• 灰度图只有 1 个通道，所以 SGBM 的平滑惩罚项 P1/P2 要乘 1。

• 彩色图有 3 个通道（BGR），所以 P1/P2 要乘 3，否则匹配惩罚不够，会导致视差计算不稳定或者出错。

结果