253 lines
11 KiB
Python
253 lines
11 KiB
Python
import os.path as osp
|
||
import numpy as np
|
||
import cv2 as cv
|
||
import argparse
|
||
from tqdm import tqdm
|
||
|
||
from calib_tools import read_json, write_json
|
||
from calib_tools import read_img_paths
|
||
from calib_tools import DataPath
|
||
|
||
|
||
# //////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
# detect_chessboard
|
||
# 辅助外参检测，检测外参图片中的棋盘格角点，生成json文件，包含2d点和3d点
|
||
def _findChessboardCorners(img, pattern):
|
||
"basic function"
|
||
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||
retval, corners = cv.findChessboardCorners(img, pattern,
|
||
flags=cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_FAST_CHECK + cv.CALIB_CB_FILTER_QUADS)
|
||
if not retval:
|
||
return False, None
|
||
corners = cv.cornerSubPix(img, corners, (11, 11), (-1, -1), criteria)
|
||
corners = corners.squeeze()
|
||
return True, corners
|
||
|
||
|
||
def _findChessboardCornersAdapt(img, pattern):
|
||
"Adapt mode"
|
||
img = cv.adaptiveThreshold(img, 255, cv.ADAPTIVE_THRESH_GAUSSIAN_C,
|
||
cv.THRESH_BINARY, 21, 2)
|
||
return _findChessboardCorners(img, pattern)
|
||
|
||
|
||
# 检测棋盘格角点并且可视化
|
||
def findChessboardCorners(img_path, pattern, show):
|
||
img = cv.imread(img_path)
|
||
if img is None:
|
||
raise FileNotFoundError(f"Image not found at {img_path}")
|
||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||
|
||
# Find the chess board corners
|
||
for func in [_findChessboardCorners, _findChessboardCornersAdapt]:
|
||
ret, corners = func(gray, pattern)
|
||
if ret: break
|
||
else:
|
||
return None
|
||
# 附加置信度 1.0 并返回
|
||
kpts2d = np.hstack([corners, np.ones((corners.shape[0], 1))])
|
||
|
||
# 标注棋盘格的原点和最后一个点
|
||
if show:
|
||
# Draw and display the corners
|
||
img_with_corners = cv.drawChessboardCorners(img, pattern, corners, ret)
|
||
# 标出棋盘格的原点
|
||
origin = tuple(corners[0].astype(int)) # 原点的像素坐标
|
||
cv.circle(img_with_corners, origin, 10, (0, 0, 255), -1) # 绘制原点
|
||
cv.putText(img_with_corners, "Origin", (origin[0] + 10, origin[1] - 10),
|
||
cv.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||
|
||
# 标出最后一个点
|
||
last_point = tuple(corners[-1].astype(int)) # 角点数组的最后一个点
|
||
cv.circle(img_with_corners, last_point, 10, (0, 255, 0), -1) # 绿色圆点
|
||
cv.putText(img_with_corners, "Last", (last_point[0] + 15, last_point[1] - 15),
|
||
cv.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) # 添加文字 "Last"
|
||
|
||
# 显示图像
|
||
cv.imwrite(osp.join(DataPath.extri_chessboard_vis, osp.basename(img_path)), img_with_corners)
|
||
return kpts2d
|
||
|
||
|
||
# 根据棋盘格生成三维坐标，棋盘格坐标系原点在左上角（同时也是全局坐标原点）
|
||
# 设定标定板z轴朝上，yx表示棋盘在yx平面上
|
||
# easymocap
|
||
# 注意，采用11x8的棋盘格，长边是y轴11，短边是x轴8，可以用opencv试一下
|
||
def getChessboard3d(pattern, gridSize, axis='yx'):
|
||
# 注意：这里为了让标定板z轴朝上，设定了短边是x，长边是y
|
||
template = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1, 2) # 棋盘格的坐标
|
||
object_points = np.zeros((pattern[1] * pattern[0], 3), np.float32) # 3d坐标，默认向上的坐标轴为0
|
||
# 长边是x,短边是z
|
||
if axis == 'xz':
|
||
object_points[:, 0] = template[:, 0]
|
||
object_points[:, 2] = template[:, 1]
|
||
elif axis == 'yx':
|
||
object_points[:, 0] = template[:, 1]
|
||
object_points[:, 1] = template[:, 0]
|
||
else:
|
||
raise NotImplementedError
|
||
object_points = object_points * gridSize
|
||
return object_points
|
||
|
||
|
||
# 检测文件夹下的所有棋盘格图片，生成3d点和2d点，存入json文件
|
||
# 图片应该按照cam0.jpg, cam1.jpg, cam2.jpg, ...的命名方式，要和内参文件夹对应
|
||
def detect_chessboard(extri_img_path, pattern, gridSize, show):
|
||
imgPaths = read_img_paths(extri_img_path)
|
||
if len(imgPaths) == 0:
|
||
print("No images found!")
|
||
return
|
||
|
||
data = {}
|
||
for imgPath in tqdm(imgPaths):
|
||
camname = osp.basename(imgPath).split(".")[0]
|
||
keypoints2d = findChessboardCorners(imgPath, pattern, show)
|
||
if keypoints2d is not None:
|
||
keypoints3d = getChessboard3d(pattern, gridSize)
|
||
data[camname] = {
|
||
"keypoints2d": keypoints2d.tolist(),
|
||
"keypoints3d": keypoints3d.tolist()
|
||
}
|
||
return data
|
||
|
||
|
||
# //////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
def solvePnP(k3d, k2d, K, dist, flag, tryextri=False):
|
||
k2d = np.ascontiguousarray(k2d[:, :2]) # 保留前两列
|
||
# try different initial values:
|
||
if tryextri: # 尝试不同的初始化外参
|
||
def closure(rvec, tvec):
|
||
ret, rvec, tvec = cv.solvePnP(k3d, k2d, K, dist, rvec, tvec, True, flags=flag)
|
||
points2d_repro, xxx = cv.projectPoints(k3d, rvec, tvec, K, dist)
|
||
kpts_repro = points2d_repro.squeeze()
|
||
err = np.linalg.norm(points2d_repro.squeeze() - k2d, axis=1).mean()
|
||
return err, rvec, tvec, kpts_repro
|
||
|
||
# create a series of extrinsic parameters looking at the origin
|
||
height_guess = 2.7 # 相机的初始高度猜测
|
||
radius_guess = 4. # 相机的初始水平距离猜测，圆的半径，需要根据自己的实际情况调整
|
||
infos = []
|
||
for theta in np.linspace(0, 2 * np.pi, 180):
|
||
st = np.sin(theta)
|
||
ct = np.cos(theta)
|
||
center = np.array([radius_guess * ct, radius_guess * st, height_guess]).reshape(3, 1)
|
||
R = np.array([
|
||
[-st, ct, 0],
|
||
[0, 0, -1],
|
||
[-ct, -st, 0]
|
||
])
|
||
tvec = - R @ center
|
||
rvec = cv.Rodrigues(R)[0]
|
||
err, rvec, tvec, kpts_repro = closure(rvec, tvec)
|
||
infos.append({
|
||
'err': err,
|
||
'repro': kpts_repro,
|
||
'rvec': rvec,
|
||
'tvec': tvec
|
||
})
|
||
infos.sort(key=lambda x: x['err'])
|
||
err, rvec, tvec, kpts_repro = infos[0]['err'], infos[0]['rvec'], infos[0]['tvec'], infos[0]['repro']
|
||
else:
|
||
# 直接求解的初值是零向量
|
||
ret, rvec, tvec = cv.solvePnP(k3d, k2d, K, dist, flags=flag)
|
||
points2d_repro, xxx = cv.projectPoints(k3d, rvec, tvec, K, dist)
|
||
kpts_repro = points2d_repro.squeeze()
|
||
err = np.linalg.norm(points2d_repro.squeeze() - k2d, axis=1).mean()
|
||
# print(err)
|
||
return err, rvec, tvec, kpts_repro
|
||
|
||
|
||
# 对单个相机进行外参标定
|
||
def _calibrate_extri(k3d, k2d, K, dist, tryfocal=False, tryextri=False):
|
||
extri = {}
|
||
methods = [cv.SOLVEPNP_ITERATIVE]
|
||
# 检查关键点数据的数量是否匹配
|
||
if k3d.shape[0] != k2d.shape[0]:
|
||
print('k3d {} doesnot match k2d {}'.format(k3d.shape, k2d.shape))
|
||
length = min(k3d.shape[0], k2d.shape[0])
|
||
k3d = k3d[:length]
|
||
k2d = k2d[:length]
|
||
valididx = k2d[:, 2] > 0 # k2d第三列是置信度，检查是否大于0
|
||
if valididx.sum() < 4: # 筛选出有效的2D和3D关键点，数量大于4
|
||
rvec = np.zeros((1, 3)) # 初始话旋转和平移为0并标记为失败
|
||
tvec = np.zeros((3, 1))
|
||
extri['Rvec'] = rvec
|
||
extri['R'] = cv.Rodrigues(rvec)[0]
|
||
extri['T'] = tvec
|
||
print('[ERROR] Failed to initialize the extrinsic parameters')
|
||
return extri
|
||
k3d = k3d[valididx]
|
||
k2d = k2d[valididx]
|
||
# 优化相机焦距
|
||
# 如果启用焦距优化
|
||
if tryfocal:
|
||
infos = []
|
||
for focal in range(500, 5000, 10): # 遍历焦距范围
|
||
# 设置焦距值
|
||
K[0, 0] = focal # 更新 K 的 fx
|
||
K[1, 1] = focal # 更新 K 的 fy
|
||
for method in methods:
|
||
# 调用 solvePnP
|
||
err, rvec, tvec, kpts_repro = solvePnP(k3d, k2d, K, dist, method, tryextri)
|
||
# 保存结果
|
||
infos.append({
|
||
'focal': focal,
|
||
'err': err,
|
||
'rvec': rvec,
|
||
'tvec': tvec,
|
||
'repro': kpts_repro
|
||
})
|
||
# 根据重投影误差选择最佳焦距
|
||
infos.sort(key=lambda x: x['err'])
|
||
best_result = infos[0]
|
||
focal = best_result['focal']
|
||
err, rvec, tvec, kpts_repro = best_result['err'], best_result['rvec'], best_result['tvec'], best_result['repro']
|
||
# 更新内参中的焦距
|
||
K[0, 0] = focal
|
||
K[1, 1] = focal
|
||
print(f'[INFO] Optimal focal length found: {focal}, reprojection error: {err:.3f}')
|
||
else:
|
||
# 如果不优化焦距，直接调用 solvePnP
|
||
err, rvec, tvec, kpts_repro = solvePnP(k3d, k2d, K, dist, cv.SOLVEPNP_ITERATIVE, tryextri)
|
||
|
||
# 保存外参结果
|
||
extri['Rvec'] = rvec.tolist()
|
||
extri['R'] = cv.Rodrigues(rvec)[0].tolist()
|
||
extri['T'] = tvec.tolist()
|
||
center = - cv.Rodrigues(rvec)[0].T @ tvec
|
||
print(f'[INFO] Camera center: {center.squeeze()}, reprojection error: {err:.3f}')
|
||
return extri
|
||
|
||
|
||
def calibrate_extri(pattern, gridSize, show=True, tryfocal=True, tryextri=True):
|
||
extri = {}
|
||
intri_data = read_json(DataPath.intri_json_path)
|
||
kpts_data = detect_chessboard(DataPath.extri_chessboard_data, pattern, gridSize, show) # 棋盘格的模式和大小
|
||
# 获取内参
|
||
camnames = list(intri_data.keys())
|
||
for cam in camnames:
|
||
print(f'[INFO] Processing camera: {cam}')
|
||
K = np.array(intri_data[cam]['K'])
|
||
dist = np.array(intri_data[cam]['dist'])
|
||
k3d = np.array(kpts_data[cam]['keypoints3d'])
|
||
k2d = np.array(kpts_data[cam]['keypoints2d'])
|
||
|
||
extri[cam] = _calibrate_extri(k3d, k2d, K, dist, tryfocal, tryextri)
|
||
|
||
write_json(extri, osp.join(DataPath.extri_json_path, 'extri.json'))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
parser = argparse.ArgumentParser(description="相机外参标定")
|
||
parser.add_argument("--pattern", type=str, default="11,8",
|
||
help="棋盘格角点数 (列数, 行数)，例如 '11,8'")
|
||
parser.add_argument("--gridSize", type=float, default=60.0,
|
||
help="棋盘格方块的实际边长（单位与数据一致，例如 mm 或 m）")
|
||
parser.add_argument("--show", dest="show", action="store_true", default=False, help="启用标定结果的可视化输出")
|
||
parser.add_argument("--tryfocal", dest="tryfocal", action="store_true", default=False, help="尝试优化焦距参数")
|
||
parser.add_argument("--tryextri", dest="tryextri", action="store_true", default=False, help="尝试优化外参")
|
||
args = parser.parse_args()
|
||
|
||
# 将解析结果传递给 calibrate_extri 函数
|
||
pattern = tuple(map(int, args.pattern.split(','))) # 将棋盘格角点数的字符串转换为元组
|
||
calibrate_extri(pattern, args.gridSize, show=args.show, tryfocal=args.tryfocal, tryextri=args.tryextri)
|