【摄影测量】从零实现张正友标定法：手写代码解析相机内参/外参与畸变校正

1. 从棋盘格到数学模型张正友标定法基础第一次接触相机标定时我被那些复杂的数学符号吓到了。直到自己动手实现了一遍张正友标定法才发现它的精妙之处其实非常直观。想象你手里拿着一个国际象棋棋盘用手机从不同角度拍摄它——这就是张正友标定法需要的全部材料。棋盘格的每个角点都是已知的世界坐标点而它们在照片中的位置可以通过OpenCV的findChessboardCorners()轻松获取。这两个信息之间藏着相机的全部秘密。我常用的棋盘格是9x6的网格内角点8x5打印在A4纸上就能用。关键是要保证棋盘格平整实测发现贴在玻璃板上效果最好。单应性矩阵(Homography)是这个过程中的关键桥梁。它建立了棋盘格平面Z0的世界坐标系与图像平面之间的映射关系。计算单应性矩阵时我习惯用DLT直接线性变换算法。下面是核心代码片段def compute_homography(world_points, image_points): A [] for (x, y), (u, v) in zip(world_points, image_points): A.append([x, y, 1, 0, 0, 0, -u*x, -u*y, -u]) A.append([0, 0, 0, x, y, 1, -v*x, -v*y, -v]) A np.array(A) _, _, V np.linalg.svd(A) H V[-1].reshape(3, 3) return H / H[2,2] # 归一化这个函数输入世界坐标和对应的图像坐标输出3x3的单应性矩阵。注意最后要进行归一化处理让H[2,2]1。在实际项目中我通常会加入RANSAC来剔除异常点提高鲁棒性。2. 内参约束与闭合解计算拿到单应性矩阵后真正的魔术开始了。张正友的聪明之处在于发现了内参矩阵的约束条件。相机内参矩阵K通常长这样[fx 0 cx] [0 fy cy] [0 0 1]其中fx,fy是焦距cx,cy是主点坐标。通过定义B(K⁻ᵀ)K⁻¹我们可以建立两个关键约束h₁ᵀBh₂ 0h₁ᵀBh₁ h₂ᵀBh₂每个单应性矩阵可以提供这两个方程而B有6个未知数因为对称。所以至少需要3张不同角度的棋盘格图片才能求解。我建议用10-15张图片这样结果更稳定。具体实现时我建了一个方程组求解器def solve_intrinsics(homographies): V [] for H in homographies: h1, h2 H[:,0], H[:,1] V.append([h1[0]*h2[0], h1[0]*h2[1]h1[1]*h2[0], h1[1]*h2[1], h1[2]*h2[0]h1[0]*h2[2], h1[2]*h2[1]h1[1]*h2[2], h1[2]*h2[2]]) V.append([(h1[0]**2-h2[0]**2), 2*(h1[0]*h1[1]-h2[0]*h2[1]), (h1[1]**2-h2[1]**2), 2*(h1[0]*h1[2]-h2[0]*h2[2]), 2*(h1[1]*h1[2]-h2[1]*h2[2]), (h1[2]**2-h2[2]**2)]) _, _, V np.linalg.svd(np.array(V)) b V[-1] B np.array([[b[0], b[1], b[3]], [b[1], b[2], b[4]], [b[3], b[4], b[5]]]) return B解出B后通过Cholesky分解就能得到内参矩阵K。这里有个坑要确保B是正定矩阵否则分解会失败。我在实际项目中遇到过这个问题最后是通过增加标定图片数量解决的。3. 外参提取与初始畸变估计有了内参矩阵K外参就很容易计算了。对于每个单应性矩阵H我们可以求出对应的旋转矩阵R和平移向量tdef compute_extrinsics(K, H): K_inv np.linalg.inv(K) h1, h2, h3 H[:,0], H[:,1], H[:,2] lambda_ 1 / np.linalg.norm(K_inv h1) r1 lambda_ * K_inv h1 r2 lambda_ * K_inv h2 r3 np.cross(r1, r2) t lambda_ * K_inv h3 R np.column_stack((r1, r2, r3)) # 通过SVD保证R是旋转矩阵 U, _, Vt np.linalg.svd(R) R U Vt return R, t这里有个关键点理论上r1和r2应该是正交的但由于噪声存在直接计算出的R可能不满足旋转矩阵的性质行列式1。我通过SVD分解进行了修正确保得到的R是真正的旋转矩阵。畸变参数初始估计也很重要。我通常先假设只有径向畸变(k1,k2)用线性最小二乘求解def estimate_distortion(pts_2d, pts_3d, K, dist_coeffs): # 将3D点投影到图像平面 rvecs [cv2.Rodrigues(R)[0] for R in rotations] tvecs translations obj_points [pts_3d] * len(rvecs) # 只优化k1,k2 flags cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_TANGENT_DIST ret, _, _, _, dist cv2.calibrateCamera( obj_points, pts_2d, image_size, K, None, flagsflags, criteria(cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)) return dist这个初始估计会作为后续非线性优化的起点。在实际项目中我发现如果初始畸变估计偏差太大后续优化可能陷入局部最优。4. 非线性优化与实战技巧前面的步骤得到的参数还比较粗糙需要用Levenberg-Marquardt算法进行非线性优化。优化目标是最小化重投影误差def reprojection_error(params, world_points, image_points, num_views): # 解包参数 fx, fy, cx, cy, k1, k2 params[:6] K np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) dist np.array([k1, k2, 0, 0]) # 计算重投影误差 error 0 for i in range(num_views): rvec params[6i*6 : 9i*6] tvec params[9i*6 : 12i*6] proj_points, _ cv2.projectPoints(world_points, rvec, tvec, K, dist) error np.sum((image_points[i] - proj_points.squeeze())**2) return error这里我使用了scipy的least_squares优化器。实测发现将旋转向量表示为罗德里格斯向量而不是旋转矩阵可以减少参数数量提高优化效率。几个实战中总结的技巧标定板要尽量充满画面但所有角点必须清晰可见拍摄角度要多样化包含俯视、仰视、倾斜等多种姿势光照要均匀避免反光和阴影影响角点检测至少需要10-15张高质量图片20张以上效果更好重投影误差最好控制在0.1像素以下我曾经在一个工业检测项目中由于现场光线问题导致标定不稳定。后来改用红外棋盘格和红外相机问题迎刃而解。这提醒我们实际应用中要根据环境特点灵活调整方案。5. 结果验证与可视化标定完成后验证环节必不可少。我通常会做三件事检查重投影误差将标定板角点重新投影到图像上计算与检测角点的距离可视化畸变校正效果用cv2.undistort观察校正前后的图像变化测试外参稳定性移动标定板观察外参变化是否符合预期下面是一个简单的可视化代码def visualize_calibration(img, K, dist): h, w img.shape[:2] new_K, roi cv2.getOptimalNewCameraMatrix(K, dist, (w,h), 1, (w,h)) undistorted cv2.undistort(img, K, dist, None, new_K) plt.figure(figsize(12,6)) plt.subplot(121) plt.imshow(img) plt.title(原始图像) plt.subplot(122) plt.imshow(undistorted) plt.title(校正后图像) plt.show()在无人机视觉导航项目中我发现虽然标定时的重投影误差很小但实际使用时仍有明显偏差。后来发现是因为镜头对焦距离变化导致内参改变。于是改为在不同对焦距离下分别标定使用时根据对焦距离插值得到内参效果显著改善。标定结果的精度评估也很重要。除了重投影误差我还会计算参数的不确定性。通过雅可比矩阵的奇异值分解可以估计各个参数的置信区间。这在需要高精度的测量应用中尤为重要。