OpenCV中projectPoints()与undistortPoints()使用异常问题排查

我尝试验证是否正确使用OpenCV的projectPoints()和undistortPoints()函数，但得到了难以理解的结果。

已定义如下相机内参：

# calibration matrix
K = np.array([
    [500.0, 0.0, 300.0], 
    [0.0, 500.0, 250.0],
    [0.0, 0.0, 1.0]
    ])

# distortion coefficients (k1, k2, p1, p2, k3)
distCoeffs = np.array([1.5, -0.95, -0.005, 0.0025,  1.16])

以及相机坐标系下的平面点云：

# Coordinates of points in plane, representing a pointcloud in camera reference (c)
# plane
H, W = 1, 2
X, Y = np.meshgrid(np.arange(-W, W, 0.2), np.arange(-H, H, 0.2))
X, Y = X.reshape(1, -1), Y.reshape(1, -1)
# add depth. Pointcloud of n points represented as a (3, n) array:
Z = 5
P_c = np.concatenate((X, Y, Z * np.ones_like(X)), axis=0)

我预期通过以下流程还原原始点云：

1. 投影点并加入畸变，得到图像平面的畸变坐标：

# project points, including with lens distortion 
U_dist, _ = cv2.projectPoints(P_c, np.zeros((3,)), np.zeros((3,)), K, distCoeffs)
# projections as (2, n) array.
U_dist = U_dist[:, 0].T

2. 对图像坐标去畸变，得到相机坐标系下的归一化坐标：

# get normalized coordinates, in camera reference, as a (2, n) array
xn_u = cv2.undistortPoints(U_dist.T, K, distCoeffs, None, None)[:, 0].T

3. 将归一化坐标乘以平面深度得到原始点云：

# add depth.
P_c2 = Z * np.concatenate((xn_u, np.ones_like(X)))

完整代码如下：

import numpy as np
import cv2


# calibration matrix
K = np.array([
    [500.0, 0.0, 300.0], 
    [0.0, 500.0, 250.0],
    [0.0, 0.0, 1.0]
    ])

# distortion coefficients (k1, k2, p1, p2, k3)
distCoeffs = np.array([1.5, -0.95, -0.005, 0.0025,  1.16])

# Coordinates of points in plane, representing a pointcloud in camera reference (c)
# plane
H, W = 1, 2
X, Y = np.meshgrid(np.arange(-W, W, 0.2), np.arange(-H, H, 0.2))
X, Y = X.reshape(1, -1), Y.reshape(1, -1)
# add depth. Pointcloud of n points represented as a (3, n) array:
Z = 5
P_c = np.concatenate((X, Y, Z * np.ones_like(X)), axis=0)

# ---------------------------------------------
# PROJECTION WITH DISTORTION
# project points, including with lens distortion 
U_dist, _ = cv2.projectPoints(P_c, np.zeros((3,)), np.zeros((3,)), K, distCoeffs)
# projections as (2, n) array.
U_dist = U_dist[:, 0].T

#-----------------------------
# UNPROJECTION accounting for distortion
# get unnormalized coordinates, in camera reference, as a (2, n) array
xn_u = cv2.undistortPoints(U_dist.T, K, distCoeffs, None, None)[:, 0].T
# add depth.
P_c2 = Z * np.concatenate((xn_u, np.ones_like(X)))

# check equality (raises error)
assert np.allclose(P_c, P_c2), f'max difference: {np.abs(P_c - P_c2).max()}'

但实际结果与原始点云差异显著，断言失败。我怀疑是对函数的使用存在误解，恳请帮助排查问题所在。

编辑补充：经进一步测试，我认为问题出在undistortPoints()而非projectPoints()。后者是确定性的，而前者需要求解非线性优化问题。实验表明，畸变程度越高，undistortPoints()的结果误差越大，低畸变时则能正确还原。

核心原因 #

undistortPoints()内部使用迭代法求解畸变的逆过程，默认的迭代次数和精度阈值在高畸变场景下不足以收敛到足够精确的结果，导致去畸变后的归一化坐标误差较大，最终还原的点云与原始点云偏差明显。

解决方法 #

1. 手动实现高精度迭代去畸变 #

由于畸变的逆过程是非线性的，我们可以基于OpenCV的畸变模型，用牛顿迭代法手动求解，确保在高畸变下收敛到精确解。示例代码如下：

def undistort_points_custom(distorted_points, K, dist_coeffs, max_iter=100, eps=1e-8):
    # 转换为归一化畸变坐标 (u', v')
    fx, fy, cx, cy = K[0,0], K[1,1], K[0,2], K[1,2]
    u_dist, v_dist = distorted_points[0], distorted_points[1]
    x_dist = (u_dist - cx) / fx
    y_dist = (v_dist - cy) / fy
    
    # 初始化迭代值为畸变坐标
    x_undist, y_undist = x_dist.copy(), y_dist.copy()
    
    k1, k2, p1, p2, k3 = dist_coeffs
    
    for _ in range(max_iter):
        x2 = x_undist ** 2
        y2 = y_undist ** 2
        r2 = x2 + y2
        r4 = r2 ** 2
        r6 = r2 ** 3
        
        # 计算畸变后的坐标
        x_dist_pred = x_undist * (1 + k1*r2 + k2*r4 + k3*r6) + 2*p1*x_undist*y_undist + p2*(r2 + 2*x2)
        y_dist_pred = y_undist * (1 + k1*r2 + k2*r4 + k3*r6) + p1*(r2 + 2*y2) + 2*p2*x_undist*y_undist
        
        # 计算残差
        dx = x_dist_pred - x_dist
        dy = y_dist_pred - y_dist
        
        # 计算雅可比矩阵
        dx_dx = 1 + k1*(3*x2 + y2) + k2*(5*x4 + 6*x2*y2 + y4) + k3*(7*x6 + 12*x4*y2 + 6*x2*y4 + y6) + 2*p1*y_undist + 6*p2*x_undist
        dy_dx = 2*p1*x_undist + 2*p2*y_undist + 2*k1*x_undist*y_undist + 4*k2*x_undist*y_undist*(x2 + y2) + 6*k3*x_undist*y_undist*(x2 + y2)**2
        
        dx_dy = 2*p1*x_undist + 2*p2*y_undist + 2*k1*x_undist*y_undist + 4*k2*x_undist*y_undist*(x2 + y2) + 6*k3*x_undist*y_undist*(x2 + y2)**2
        dy_dy = 1 + k1*(x2 + 3*y2) + k2*(x4 + 6*x2*y2 + 5*y4) + k3*(x6 + 6*x4*y2 + 12*x2*y4 + 7*y6) + 6*p1*y_undist + 2*p2*x_undist
        
        # 计算逆矩阵并更新
        det = dx_dx * dy_dy - dx_dy * dy_dx
        delta_x = (dy_dy * dx - dx_dy * dy) / det
        delta_y = (dx_dx * dy - dx_dy * dx) / det
        
        x_undist -= delta_x
        y_undist -= delta_y
        
        # 检查收敛
        if np.max(np.abs(delta_x)) < eps and np.max(np.abs(delta_y)) < eps:
            break
    
    return np.array([x_undist, y_undist])

# 使用自定义函数替代cv2.undistortPoints
xn_u = undistort_points_custom(U_dist, K, distCoeffs)
P_c2 = Z * np.concatenate((xn_u, np.ones_like(X)))

# 再次验证
assert np.allclose(P_c, P_c2, atol=1e-6), f'max difference: {np.abs(P_c - P_c2).max()}'

2. 验证projectPoints()的正确性 #

可以先确认投影过程无问题：将原始点云转换为归一化坐标（X/Z, Y/Z），手动应用畸变公式后用内参投影，对比结果与U_dist是否一致，排除投影环节的错误。

总结 #

高畸变场景下，OpenCV内置的undistortPoints()默认迭代精度不足，导致逆过程误差大。通过手动实现高精度迭代求解，可以显著提升去畸变精度，从而准确还原原始点云。

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