如何修复OpenCV生成点云出现inf坐标系的问题？

问题根源分析 #

你遇到的inf坐标和CloudCompare警告，主要由三个核心问题导致：

1. 视差图格式未正确转换：StereoBM输出的视差是CV_16S格式（16位有符号整数），值为实际视差的16倍，直接使用会导致视差数值错误，进而计算出无效的3D坐标。
2. Q矩阵参数错误：手动构造的Q矩阵最后一行参数错误，缺少基线（左右相机间距）的倒数项，导致z坐标计算时出现除以0的情况，生成inf值。
3. 无效点过滤不彻底：仅过滤大于视差最小值的点，但最小值可能是无效的负数，且未排除计算后产生的inf/nan点。

修正后的完整代码 #

import numpy as np
import cv2 as cv

ply_header = '''ply
format ascii 1.0
element vertex %(vert_num)d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
'''

def write_ply(fn, verts, colors):
    verts = verts.reshape(-1, 3)
    colors = colors.reshape(-1, 3)
    # 过滤包含inf或nan的无效点
    valid_mask = ~np.isinf(verts).any(axis=1) & ~np.isnan(verts).any(axis=1)
    verts = verts[valid_mask]
    colors = colors[valid_mask]
    verts = np.hstack([verts, colors])
    with open(fn, 'wb') as f:
        f.write((ply_header % dict(vert_num=len(verts))).encode('utf-8'))
        np.savetxt(f, verts, fmt='%f %f %f %d %d %d ')

pcd_dir = "/some/path/points.ply"
# 读取彩色图用于颜色提取，灰度图用于视差计算
imL_color = cv.imread("/path/to/left/frame0000.jpg")
imR_color = cv.imread("/path/to/right/frame0000.jpg")
imL = cv.cvtColor(imL_color, cv.COLOR_BGR2GRAY)
imR = cv.cvtColor(imR_color, cv.COLOR_BGR2GRAY)

# 初始化StereoBM，调整参数提升视差质量（numDisparities必须是16的倍数）
stereo = cv.StereoBM_create(numDisparities=80, blockSize=15)
# 直方图均衡预处理，提升匹配稳定性
imL = cv.equalizeHist(imL)
imR = cv.equalizeHist(imR)
disparity = stereo.compute(imL, imR)

# 转换视差为浮点型，除以16得到真实视差（StereoBM输出为16倍视差的整数）
disparity = disparity.astype(np.float32) / 16.0

h, w = imL.shape[:2]
# 若有立体校正后的Q矩阵，替换为实际校正输出的Q（优先使用）
# Q = 你的校正Q矩阵
# 无校正参数时，手动构造Q矩阵（b为基线长度，单位米，需根据实际相机调整）
f = 0.8 * w  # 焦距估算值
b = 0.1      # 基线长度示例，需替换为实际相机间距
Q = np.float32([[1, 0, 0, -0.5 * w],
                [0, -1, 0, 0.5 * h],
                [0, 0, 0, -f],
                [0, 0, 1/b, 0]])  # 修正最后一行，添加基线倒数项

points = cv.reprojectImageTo3D(disparity, Q)

# 提取颜色并过滤视差无效区域
colors = cv.cvtColor(imL_color, cv.COLOR_BGR2RGB)
mask = disparity > 0  # 过滤视差<=0的无效匹配点
out_points = points[mask]
out_colors = colors[mask]

write_ply(pcd_dir, out_points, out_colors)

关键修改说明 #

1. 视差图格式转换

   disparity = disparity.astype(np.float32) / 16.0
   

   StereoBM输出的视差是放大16倍的整数，必须转换为浮点型并除以16，才能得到真实的视差值，否则3D坐标计算完全错误。

2. Q矩阵参数修正
   原Q矩阵最后一行[0,0,1,0]缺少基线倒数项，导致z坐标计算时出现z = -f*b/d中b为无穷大的情况，修正为[0,0,1/b,0]后，才能得到有效的z值（b为左右相机的实际间距，单位米）。

3. 无效点双重过滤

   • 先通过disparity > 0过滤未匹配的视差点
   • 在写入PLY前，额外过滤包含inf/nan的点，确保所有输出点都是有效的3D坐标

4. 视差参数优化

   • numDisparities设为80（16的倍数），覆盖更大的视差范围
   • blockSize设为15（奇数），提升匹配的稳定性
   • 添加直方图均衡预处理，提升图像对比度，减少匹配错误

额外建议 #

• 如果有相机标定和立体校正的参数，务必使用cv.stereoRectify输出的Q矩阵，手动构造的Q矩阵仅能作为临时替代，精度远低于校正后的参数。
• 可以尝试使用cv.StereoSGBM替代StereoBM，SGBM算法能生成更精准的视差图，减少无效匹配点。
• 若CloudCompare仍提示“Global shift/scale”，是因为点云坐标范围过大，可在软件中选择自动应用shift/scale即可正常显示。

内容的提问来源于stack exchange，提问作者Corbjn