import cv2
import numpy as np
import os
import glob
from scipy.optimize import least_squares
import sys
# 本代码为博客【张正友标定法解析与C++代码实现】的Python对应实现
# 程序中公式的原理，可查阅此博客
# https://blog.csdn.net/memmolo/article/details/131741018
def readImages(pattern, images):
"""读取指定路径下的PNG图片"""
# 查找路径下所有png图片，非递归
list_files = glob.glob(pattern)
if not list_files:
print("未在路径下找到PNG格式图片!")
return False
else:
print(f"找到图片数量：{len(list_files)}")
for file_path in list_files:
print(file_path)
# 读取图片至列表
img = cv2.imread(file_path, cv2.IMREAD_UNCHANGED)
images.append(img)
return True
def findCorners(images, w, h, scale, board_type, imgPntsVec, objPntsVec):
"""查找棋盘格角点"""
imgPoints = []
objPoints = []
# 生成物体坐标系中的角点坐标列表
for i in range(h):
for j in range(w):
objPoints.append((j * scale, i * scale, 0))
count = 0
for img in images:
if board_type == "chessboard":
# 调用OpenCV查找棋盘格角点，findChessboardCornersSB是亚像素级角点检测
ret, corners = cv2.findChessboardCornersSB(img, (w, h), flags=0)
elif board_type == "circleboard":
ret, corners = cv2.findCirclesGrid(img, (w, h))
else:
print("unknow calib board")
exit()
if ret:
imgPntsVec.append(corners)
objPntsVec.append(objPoints)
count += 1
# 显示棋盘格检测结果
cv2.namedWindow("Corners")
if len(img.shape) == 2: # 灰度图（只有高、宽，无通道）
img_copy = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) # 灰度转BGR彩色
else: # 已是彩色图（有通道维度）
img_copy = img.copy() # 直接复制彩色图
cv2.drawChessboardCorners(img_copy, (w, h), corners, ret)
cv2.imshow("Corners", img_copy)
cv2.waitKey(0)
cv2.destroyWindow("Corners")
if count < 3:
print("检测出的有效棋盘格标定板数量小于3！")
return False
else:
print(f"检测出有效棋盘格标定板数量为：{count}")
return True
def findHomography(imgPoints, objPoints):
"""求解单应矩阵（无归一化）"""
number = len(imgPoints)
A = np.zeros((number * 2, 9), dtype=np.float64)
# 构造A矩阵，参考博客公式(1)
for i in range(number):
u = imgPoints[i][0][0]
v = imgPoints[i][0][1]
x = objPoints[i][0]
y = objPoints[i][1]
A[2*i, 0] = x
A[2*i, 1] = y
A[2*i, 2] = 1
A[2*i, 3] = 0
A[2*i, 4] = 0
A[2*i, 5] = 0
A[2*i, 6] = -u * x
A[2*i, 7] = -u * y
A[2*i, 8] = -u
A[2*i+1, 0] = 0
A[2*i+1, 1] = 0
A[2*i+1, 2] = 0
A[2*i+1, 3] = x
A[2*i+1, 4] = y
A[2*i+1, 5] = 1
A[2*i+1, 6] = -v * x
A[2*i+1, 7] = -v * y
A[2*i+1, 8] = -v
# 求解Ax=0 (SVD分解)
u, s, vt = np.linalg.svd(A)
homo = vt[-1].reshape(-1, 1)
# 单应矩阵处理，保持最后一个元素为1
homo = homo / homo[8, 0]
homo = homo.reshape(3, 3)
return homo
def normalizePoints(points_2d=None, points_3d=None):
"""
对点进行归一化，使求解更稳定
支持2D点或3D点归一化，返回归一化后的点和归一化矩阵N
"""
N = np.eye(3, dtype=np.float64)
if points_2d is not None:
points = np.array([(p[0][0], p[0][1]) for p in points_2d])
is_2d = True
elif points_3d is not None:
points = np.array(points_3d)[:, :2] # 只取x,y
is_2d = False
else:
return None, N
num = len(points)
# 计算均值
xmean = np.mean(points[:, 0])
ymean = np.mean(points[:, 1])
# 计算标准差
xstd = np.sqrt(np.sum((points[:, 0] - xmean)**2) / num)
ystd = np.sqrt(np.sum((points[:, 1] - ymean)**2) / num)
# 点归一化
normalized = points.copy()
normalized[:, 0] = (normalized[:, 0] - xmean) / xstd
normalized[:, 1] = (normalized[:, 1] - ymean) / ystd
# 构建归一化矩阵
N[0, 0] = 1 / xstd
N[0, 2] = -xmean / xstd
N[1, 1] = 1 / ystd
N[1, 2] = -ymean / ystd
# 转换回原格式
if is_2d:
normalized_points = [[(x, y)] for x, y in normalized]
else:
normalized_points = [(x, y, 0) for x, y in normalized]
return normalized_points, N
def findHomographyWithNormalization(imgPoints, objPoints):
"""求解单应矩阵（带归一化）"""
number = len(imgPoints)
A = np.zeros((number * 2, 9), dtype=np.float64)
# 物点归一化
objNPnts, No = normalizePoints(points_3d=objPoints)
# 图像点归一化
imgNPnts, Ni = normalizePoints(points_2d=imgPoints)
# 构造A矩阵
for i in range(number):
u = imgNPnts[i][0][0]
v = imgNPnts[i][0][1]
x = objNPnts[i][0]
y = objNPnts[i][1]
A[2*i, 0] = x
A[2*i, 1] = y
A[2*i, 2] = 1
A[2*i, 3] = 0
A[2*i, 4] = 0
A[2*i, 5] = 0
A[2*i, 6] = -u * x
A[2*i, 7] = -u * y
A[2*i, 8] = -u
A[2*i+1, 0] = 0
A[2*i+1, 1] = 0
A[2*i+1, 2] = 0
A[2*i+1, 3] = x
A[2*i+1, 4] = y
A[2*i+1, 5] = 1
A[2*i+1, 6] = -v * x
A[2*i+1, 7] = -v * y
A[2*i+1, 8] = -v
# 求解Ax=0
u, s, vt = np.linalg.svd(A)
homo = vt[-1].reshape(3, 3)
# 参考博客公式(3)
homo = np.linalg.inv(Ni) @ homo @ No
# 单应矩阵处理，保持最后一个元素为1
homo = homo / homo[2, 2]
return homo
def findAllHomography(imgPntsVec, objPntsVec, homoVec):
"""求解所有图像的单应矩阵"""
for imgPnts, objPnts in zip(imgPntsVec, objPntsVec):
# 使用带归一化的单应矩阵求解
homo = findHomographyWithNormalization(imgPnts, objPnts)
homoVec.append(homo)
def estimateIntrinsics(homoVec, K):
"""从单应矩阵估计内参"""
num_homo = len(homoVec)
A = np.zeros((num_homo * 2, 5), dtype=np.float64)
# 构造Ax=0的A矩阵，参考博客公式(4)
for i in range(num_homo):
h11 = homoVec[i][0, 0]
h21 = homoVec[i][1, 0]
h31 = homoVec[i][2, 0]
h12 = homoVec[i][0, 1]
h22 = homoVec[i][1, 1]
h32 = homoVec[i][2, 1]
A[2*i, 0] = h11 * h12
A[2*i, 1] = h21 * h22
A[2*i, 2] = h31 * h12 + h11 * h32
A[2*i, 3] = h31 * h22 + h21 * h32
A[2*i, 4] = h31 * h32
A[2*i+1, 0] = h11*h11 - h12*h12
A[2*i+1, 1] = h21*h21 - h22*h22
A[2*i+1, 2] = 2 * (h11*h31 - h12*h32)
A[2*i+1, 3] = 2 * (h21*h31 - h22*h32)
A[2*i+1, 4] = h31*h31 - h32*h32
# 求解B矩阵 (Ax=0)
u, s, vt = np.linalg.svd(A)
B = vt[-1]
print(f"B: {B}")
# 从B矩阵恢复内参，假定skewness为零，参考博客公式(5)
B11 = B[0]
B22 = B[1]
B13 = B[2]
B23 = B[3]
B33 = B[4]
cx = -B13 / B11
cy = -B23 / B22
lmd = B33 - (B13*B13/B11 + B23*B23/B22)
fx = np.sqrt(lmd / B11)
fy = np.sqrt(lmd / B22)
# print(f"内参：fx: {fx} fy: {fy} cx: {cx} cy: {cy} lmd: {lmd}")
# 给内参矩阵赋值
K[0, 0] = fx
K[1, 1] = fy
K[0, 2] = cx
K[1, 2] = cy
def estimateExtrinsics(homoVec, K, rvec, tvec):
"""从单应矩阵和内参估计外参"""
invK = np.linalg.inv(K)
for homo in homoVec:
# 求解外参，rrt=[r1, r2, t]，参考博客公式(6)
rrt = invK @ homo
lmd = (np.linalg.norm(rrt[:, 0]) + np.linalg.norm(rrt[:, 1])) / 2
rrt = rrt / lmd
r1 = rrt[:, 0].reshape(-1, 1)
r2 = rrt[:, 1].reshape(-1, 1)
t = rrt[:, 2].reshape(-1, 1)
r3 = np.cross(r1.flatten(), r2.flatten()).reshape(-1, 1)
R = np.hstack([r1, r2, r3])
# SVD分解，找到符合约束的旋转矩阵，参考博客公式(7)
u, s, vt = np.linalg.svd(R)
R = u @ vt
# 旋转矩阵转旋转向量
axisangle, _ = cv2.Rodrigues(R)
rvec.append(axisangle)
tvec.append(t)
def reprojection_error(params, imgPntsVec, objPntsVec):
"""重投影误差计算（用于数值优化）"""
# 解析参数
fx, fy, cx, cy = params[:4]
k1, k2, k3, p1, p2 = params[4:9]
poses = params[9:].reshape(-1, 6) # 每个pose: [rx, ry, rz, tx, ty, tz]
residuals = []
for i, (imgPnts, objPnts) in enumerate(zip(imgPntsVec, objPntsVec)):
# 获取当前位姿
rx, ry, rz, tx, ty, tz = poses[i]
rvec = np.array([rx, ry, rz], dtype=np.float64)
tvec = np.array([tx, ty, tz], dtype=np.float64)
# 旋转矩阵
R, _ = cv2.Rodrigues(rvec)
for j, (img_pnt, obj_pnt) in enumerate(zip(imgPnts, objPnts)):
# 3D点转换到相机坐标系
p3d = np.array(obj_pnt, dtype=np.float64).reshape(3, 1)
p_cam = R @ p3d + tvec.reshape(3, 1)
# 归一化平面
x = p_cam[0, 0] / p_cam[2, 0]
y = p_cam[1, 0] / p_cam[2, 0]
# 畸变矫正
r2 = x*x + y*y
r4 = r2*r2
r6 = r4*r2
xd = x*(1 + k1*r2 + k2*r4 + k3*r6) + 2*p1*x*y + p2*(r2 + 2*x*x)
yd = y*(1 + k1*r2 + k2*r4 + k3*r6) + 2*p2*x*y + p1*(r2 + 2*y*y)
# 像素坐标
u = fx * xd + cx
v = fy * yd + cy
# 计算残差
residuals.append(u - img_pnt[0][0])
residuals.append(v - img_pnt[0][1])
return np.array(residuals, dtype=np.float64)
def ceresCalibrate(imgPntsVec, objPntsVec, K, D, rvec, tvec):
"""模拟Ceres优化标定（使用scipy.optimize.least_squares）"""
# print("Ceres优化（Python替代实现）")
# 初始化参数
# cam: [fx, fy, cx, cy, k1, k2, k3, p1, p2]
cam = np.array([
K[0, 0], K[1, 1], K[0, 2], K[1, 2],
D[0, 0], D[0, 1], D[0, 4], D[0, 2], D[0, 3]
], dtype=np.float64)
# pose: [rx, ry, rz, tx, ty, tz] * num_images
pose = []
for r, t in zip(rvec, tvec):
pose.extend([r[0,0], r[1,0], r[2,0], t[0,0], t[1,0], t[2,0]])
pose = np.array(pose, dtype=np.float64)
# 合并参数
x0 = np.concatenate([cam, pose])
# 优化
result = least_squares(
reprojection_error,
x0,
args=(imgPntsVec, objPntsVec),
method='lm',
ftol=1e-10,
gtol=1e-10,
xtol=1e-8,
max_nfev=50,
verbose=2
)
print("\n优化摘要:")
print(f"成功: {result.success}")
print(f"迭代次数: {result.nfev}")
print(f"最终成本: {result.cost}")
# 提取优化结果
opt_params = result.x
# 更新内参
K[0, 0] = opt_params[0] # fx
K[1, 1] = opt_params[1] # fy
K[0, 2] = opt_params[2] # cx
K[1, 2] = opt_params[3] # cy
# 更新畸变参数
D[0, 0] = opt_params[4] # k1
D[0, 1] = opt_params[5] # k2
D[0, 2] = opt_params[7] # p1
D[0, 3] = opt_params[8] # p2
D[0, 4] = opt_params[6] # k3
# 更新外参
poses = opt_params[9:].reshape(-1, 6)
for i in range(len(rvec)):
rvec[i][0,0] = poses[i][0]
rvec[i][1,0] = poses[i][1]
rvec[i][2,0] = poses[i][2]
tvec[i][0,0] = poses[i][3]
tvec[i][1,0] = poses[i][4]
tvec[i][2,0] = poses[i][5]
def displayResults(K, D, rvec, tvec):
"""显示标定结果"""
print("内参矩阵: ")
print(K)
print("畸变参数: ", D.flatten())
for i, (r, t) in enumerate(zip(rvec, tvec)):
print(f"图像索引: {i}，旋转向量: {r.T}，平移向量: {t.T}")
def project3DPoint(p3, r, t, K, D):
"""将3D点投影到2D像素坐标"""
p = np.array([p3[0], p3[1], p3[2]], dtype=np.float64).reshape(3, 1)
R, _ = cv2.Rodrigues(r)
p = R @ p + t
x = p[0, 0] / p[2, 0]
y = p[1, 0] / p[2, 0]
r2 = x*x + y*y
r4 = r2*r2
r6 = r4*r2
k1 = D[0, 0]
k2 = D[0, 1]
p1 = D[0, 2]
p2 = D[0, 3]
k3 = D[0, 4]
xd = x*(1 + k1*r2 + k2*r4 + k3*r6) + 2*p1*x*y + p2*(r2 + 2*x*x)
yd = y*(1 + k1*r2 + k2*r4 + k3*r6) + 2*p2*x*y + p1*(r2 + 2*y*y)
fx = K[0, 0]
fy = K[1, 1]
cx = K[0, 2]
cy = K[1, 2]
u = fx * xd + cx
v = fy * yd + cy
return (u, v)
def calculateReprojectionError(imgPntsVec, objPntsVec, K, D, rvec, tvec):
"""计算重投影误差"""
error = 0.0
count = 0
# 增加边界检查
if len(imgPntsVec) != len(rvec) or len(imgPntsVec) != len(tvec):
print(f"警告：数据长度不匹配！imgPntsVec: {len(imgPntsVec)}, rvec: {len(rvec)}, tvec: {len(tvec)}")
# 取最小长度避免越界
valid_len = min(len(imgPntsVec), len(rvec), len(tvec))
imgPntsVec = imgPntsVec[:valid_len]
objPntsVec = objPntsVec[:valid_len]
rvec = rvec[:valid_len]
tvec = tvec[:valid_len]
for i in range(len(imgPntsVec)):
for j in range(len(imgPntsVec[i])):
prjpnt = project3DPoint(objPntsVec[i][j], rvec[i], tvec[i], K, D)
imgpnt = imgPntsVec[i][j][0]
# 计算欧氏距离
dist = np.sqrt((imgpnt[0] - prjpnt[0])**2 + (imgpnt[1] - prjpnt[1])**2)
error += dist
count += 1
print(f"有效标定点数量: {count}")
if count > 0:
avg_error = error / count
print(f"平均重投影误差: {avg_error:.6f} 像素")
else:
print("无有效标定点，无法计算重投影误差")
avg_error = 0.0
return avg_error
def main():
# 图片文件夹路径
path = r"F:\DEV\calib_test\halcon"
# 查找的图片格式
pattern = "/*.png"
# 存储图片的列表
imageData = []
# 查找并读取图片
if not readImages(path + pattern, imageData):
return
# 棋盘格长、宽、尺度（内角点数量，比如11x8表示每行11个角点，每列8个）
columns = 7
rows = 7
scale = 12.5 # millimeter
board_type = "circleboard"
objectPointsVector = []
imagePointsVector = []
# 查找角点
if not findCorners(imageData, columns, rows, scale, board_type, imagePointsVector, objectPointsVector):
return
# 计算单应
homographyVector = []
findAllHomography(imagePointsVector, objectPointsVector, homographyVector)
# 估计内参
K = np.eye(3, dtype=np.float64)
estimateIntrinsics(homographyVector, K)
# 估计外参
rotationVector = []
translationVector = []
estimateExtrinsics(homographyVector, K, rotationVector, translationVector)
# 畸变初值设为0
D = np.zeros((1, 5), dtype=np.float64)
# Ceres优化标定（Python替代实现）
ceresCalibrate(imagePointsVector, objectPointsVector, K, D, rotationVector, translationVector)
print("\n===== 自定义标定优化结果 =====")
displayResults(K, D, rotationVector, translationVector)
calculateReprojectionError(imagePointsVector, objectPointsVector, K, D, rotationVector, translationVector)
# OpenCV标定（修复核心错误：使用所有有效图片的角点）
print("\n===== OpenCV标定结果 =====")
# 转换数据格式以适配OpenCV
obj_points = [np.array(pnts, dtype=np.float32) for pnts in objectPointsVector]
img_points = [np.array(pnts, dtype=np.float32) for pnts in imagePointsVector]
# 检查图像尺寸（使用第一张有效图片的尺寸）
img_size = imageData[0].shape[:2][::-1] if imageData else (640, 480)
# OpenCV标定
ret, K_opencv, D_opencv, rvecs_opencv, tvecs_opencv = cv2.calibrateCamera(
obj_points, img_points, img_size, None, None # 不传入初始内参，让OpenCV自动求解
)
# 更新结果
K[:] = K_opencv
D[:] = D_opencv
rotationVector[:] = rvecs_opencv
translationVector[:] = tvecs_opencv
displayResults(K, D, rotationVector, translationVector)
calculateReprojectionError(imagePointsVector, objectPointsVector, K, D, rotationVector, translationVector)
if __name__ == "__main__":
main()
浙公网安备 33010602011771号