好的好的
这是我自写的标定代码：
import argparse
import os
from pathlib import Path
import cv2
import numpy as np
SCRIPT_DIR = os.path.dirname(os.path.realpath(file))
ROOT_DIR = os.path.abspath(os.path.join(SCRIPT_DIR, “..”))
GS130W 模组基线（米），用于核对双目 T 的平移量是否在同一量级。
EXPECTED_BASELINE_M = 0.080
def parse_args():
parser = argparse.ArgumentParser(
description=“Offline stereo calibration from raw distorted chessboard images, "
“outputting a kalibr-style cam0/cam1 yaml (radtan, 4 distortion coeffs).”
)
parser.add_argument(
“–image_dir”,
default=os.path.join(ROOT_DIR, “digua18yuanshi”),
type=str,
help=“Directory containing left_XXXX / right_XXXX chessboard images.”,
)
parser.add_argument(
“–output”,
default=os.path.join(ROOT_DIR, “output”, “calibration”, “digua18_stereo.yaml”),
type=str,
help=“Output kalibr-style yaml path.”,
)
parser.add_argument(”–pattern_cols", default=11, type=int, help=“Inner corners per chessboard row.”)
parser.add_argument(“–pattern_rows”, default=8, type=int, help=“Inner corners per chessboard column.”)
parser.add_argument(“–square_size”, default=0.030, type=float, help=“Chessboard square size in meters.”)
parser.add_argument(
“–model”,
default=“radtan”,
type=str,
choices=[“radtan”, “fisheye”],
help="Lens model. radtan: pinhole + Brown-Conrady (k1,k2,p1,p2,k3). "
“fisheye: equidistant (k1~k4), for straightness validation only.”,
)
parser.add_argument(
“–max_frame_rms”,
default=1.0,
type=float,
help=“Per-frame reprojection RMS threshold (px). Frames above this are dropped.”,
)
parser.add_argument(
“–fix_intrinsics”,
default=1,
type=int,
help=“1: fix intrinsics from mono calibration and only optimize R,T in stereoCalibrate.”,
)
parser.add_argument(
“–compare_fisheye”,
default=0,
type=int,
help=“1: additionally run fisheye mono calibration + straightness check for comparison.”,
)
return parser.parse_args()
图片配对
def extract_index(path):
“”“从 left_0001.png / right_0001.png 里提取编号，用于严格左右配对。”“”
stem = Path(path).stem.lower()
for prefix in (“left_”, “right_”, “left-”, “right-”):
if stem.startswith(prefix):
return stem[len(prefix):]
if stem.startswith(“left”):
return stem[4:]
if stem.startswith(“right”):
return stem[5:]
return None
def collect_stereo_pairs(image_dir):
“”“扫描目录中的左右图，按编号配对。”“”
supported_exts = {“.jpg”, “.jpeg”, “.png”, “.bmp”, “.tif”, “.tiff”}
left_map = {}
right_map = {}
for path in Path(image_dir).iterdir():
if not path.is_file() or path.suffix.lower() not in supported_exts:
continue
idx = extract_index(path.name)
if not idx:
continue
lower_name = path.name.lower()
if lower_name.startswith("left"):
left_map[idx] = str(path)
elif lower_name.startswith("right"):
right_map[idx] = str(path)
common_ids = sorted(set(left_map.keys()) & set(right_map.keys()), key=lambda x: int(x))
if not common_ids:
raise RuntimeError(f"No valid stereo pairs found in {image_dir}")
return [(idx, left_map[idx], right_map[idx]) for idx in common_ids]
角点与目标点
def create_object_points(pattern_size, square_size):
“”"
生成棋盘格 3D 角点模板，Z 恒为 0。
形状按 (1, N, 3) float64 组织 —— cv2.fisheye.calibrate 强制要求这个形状，
普通 cv2.calibrateCamera 用 float32 版即可（见 as_pinhole_points）。
"""
cols, rows = pattern_size
n = cols * rows
objp = np.zeros((1, n, 3), np.float64)
objp[0, :, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
objp *= square_size
return objp
def find_corners(gray, pattern_size):
“”“检测棋盘格角点 + 亚像素优化。”“”
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
found, corners = cv2.findChessboardCorners(gray, pattern_size, flags)
if not found:
return False, None
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
return True, corners
投影 / 重投影误差
def project_points(obj_points, rvec, tvec, camera_matrix, dist_coeffs, model):
“”“把一组 3D 角点按模型投到像素平面，返回 (N, 2)。”“”
if model == “radtan”:
objp = np.asarray(obj_points, dtype=np.float32).reshape(-1, 3)
projected, _ = cv2.projectPoints(objp, rvec, tvec, camera_matrix, dist_coeffs)
else:
objp = np.asarray(obj_points, dtype=np.float64).reshape(1, -1, 3)
projected, _ = cv2.fisheye.projectPoints(objp, rvec, tvec, camera_matrix, dist_coeffs)
return projected.reshape(-1, 2)
def compute_rms(img_points, obj_points, rvecs, tvecs, camera_matrix, dist_coeffs, model):
“”“逐帧计算重投影 RMS，返回 (总体 RMS, 每帧 RMS 列表)。”“”
total_sq = 0.0
total_n = 0
per_frame =
for objp, imgp, rvec, tvec in zip(obj_points, img_points, rvecs, tvecs):
projected = project_points(objp, rvec, tvec, camera_matrix, dist_coeffs, model)
imgp = imgp.reshape(-1, 2)
sq = float(np.sum((imgp - projected) ** 2))
n = imgp.shape[0]
per_frame.append(float(np.sqrt(sq / n)))
total_sq += sq
total_n += n
return float(np.sqrt(total_sq / total_n)), per_frame
单目标定
def calibrate_mono(obj_points, img_points, image_size, model):
“”“单目标定，返回 (rms, K, dist, rvecs, tvecs)。”“”
if model == “radtan”:
obj_list = [np.asarray(o, dtype=np.float32).reshape(-1, 3) for o in obj_points]
img_list = [np.asarray(p, dtype=np.float32).reshape(-1, 2) for p in img_points]
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
obj_list, img_list, image_size, None, None
)
else:
obj_list = [np.asarray(o, dtype=np.float64).reshape(1, -1, 3) for o in obj_points]
img_list = [np.asarray(p, dtype=np.float64).reshape(1, -1, 2) for p in img_points]
ret, K, dist, rvecs, tvecs = cv2.fisheye.calibrate(
obj_list, img_list, image_size, None, None
)
return float(ret), K, dist, rvecs, tvecs
双目标定
def calibrate_stereo(obj_points, img_points_left, img_points_right, image_size, model,
K_left, dist_left, K_right, dist_right, fix_intrinsics):
“”“双目标定，返回 (rms, K_left, dist_left, K_right, dist_right, R, T, E, F)。”“”
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-6)
if model == "radtan":
obj_list = [np.asarray(o, dtype=np.float32).reshape(-1, 3) for o in obj_points]
img_l = [np.asarray(p, dtype=np.float32).reshape(-1, 2) for p in img_points_left]
img_r = [np.asarray(p, dtype=np.float32).reshape(-1, 2) for p in img_points_right]
flags = cv2.CALIB_FIX_INTRINSIC if fix_intrinsics else cv2.CALIB_USE_INTRINSIC_GUESS
out = cv2.stereoCalibrate(
obj_list, img_l, img_r,
K_left, dist_left, K_right, dist_right,
image_size, criteria=criteria, flags=flags,
)
rms, K_l, d_l, K_r, d_r, R, T, E, F = out
else:
obj_list = [np.asarray(o, dtype=np.float64).reshape(1, -1, 3) for o in obj_points]
img_l = [np.asarray(p, dtype=np.float64).reshape(1, -1, 2) for p in img_points_left]
img_r = [np.asarray(p, dtype=np.float64).reshape(1, -1, 2) for p in img_points_right]
flags = cv2.fisheye.CALIB_FIX_INTRINSIC if fix_intrinsics else cv2.fisheye.CALIB_USE_INTRINSIC_GUESS
out = cv2.fisheye.stereoCalibrate(
obj_list, img_l, img_r,
K_left, dist_left, K_right, dist_right,
image_size, flags=flags, criteria=criteria,
)
rms, K_l, d_l, K_r, d_r, R, T = out
E = F = None
return float(rms), K_l, d_l, K_r, d_r, R, T, E, F
去畸变后直线度校验
def undistort_points_to_pixels(points, K, dist, model):
“”“把角点去畸变到理想像素坐标，返回 (N, 2)。”“”
pts = np.asarray(points, dtype=np.float64).reshape(-1, 1, 2)
if model == “radtan”:
u = cv2.undistortPoints(pts, K, dist, P=K)
else:
u = cv2.fisheye.undistortPoints(pts, K, dist)
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]
u = u * np.array([[[fx, fy]]]) + np.array([[[cx, cy]]])
return u.reshape(-1, 2)
def _line_rms(pts):
“”“拟合一条直线，返回点到直线的 RMS 垂直距离（像素）。”“”
if pts.shape[0] < 2:
return 0.0
# 用 PCA：最小特征值对应方向即为法向，点到均值在法向的投影即残差。
mean = pts.mean(axis=0)
centered = pts - mean
cov = centered.T @ centered / pts.shape[0]
eigvals, eigvecs = np.linalg.eigh(cov)
normal = eigvecs[:, 0] # 最小特征值方向
residuals = centered @ normal
return float(np.sqrt(np.mean(residuals ** 2)))
def straightness_check(img_points, K, dist, pattern_size, model):
“”“去畸变后检查棋盘格每行/每列角点是否共线，返回 RMS（像素）。”“”
cols, rows = pattern_size
all_rms =
for pts in img_points:
up = undistort_points_to_pixels(pts, K, dist, model).reshape(rows, cols, 2)
for r in range(rows):
all_rms.append(_line_rms(up[r]))
for c in range(cols):
all_rms.append(_line_rms(up[:, c]))
return float(np.sqrt(np.mean(np.array(all_rms) ** 2))), all_rms
kalibr yaml 输出
def _fmt(x):
“”“格式化单个数值，保证足够精度且整洁。”“”
if isinstance(x, (int, np.integer)):
return str(int(x))
s = f"{float(x):.12g}"
if s in (“-0”, “0”) and s.startswith(“-0”):
s = “0”
return s
def _fmt_vec(values):
return “[” + “, “.join(_fmt(v) for v in np.asarray(values).ravel()) + “]”
def _fmt_mat_4x4(mat):
rows =
for i in range(4):
rows.append(” - [” + ", “.join(_fmt(v) for v in mat[i]) + “]”)
return “\n”.join(rows)
def write_kalibr_yaml(output_path, K_left, dist_left, K_right, dist_right, R, T, image_size, model):
“””
输出 kalibr 风格 cam0/cam1 yaml。
radtan：distortion_coeffs 取 OpenCV 5 个系数里的前 4 个 [k1,k2,p1,p2]，
与 kalibr 的 radtan 模型一致。
fisheye：distortion_model 标为 equidistant，仅供诊断，不能直接当下游 radtan 用。
"""
output_path = os.path.abspath(output_path)
os.makedirs(os.path.dirname(output_path), exist_ok=True)
fx, fy = float(K_left[0, 0]), float(K_left[1, 1])
cx, cy = float(K_left[0, 2]), float(K_left[1, 2])
fx2, fy2 = float(K_right[0, 0]), float(K_right[1, 1])
cx2, cy2 = float(K_right[0, 2]), float(K_right[1, 2])
d_left = np.asarray(dist_left).ravel()
d_right = np.asarray(dist_right).ravel()
if model == "radtan":
# kalibr radtan = [k1, k2, p1, p2]
d_left_out = d_left[:4]
d_right_out = d_right[:4]
distortion_model = "radtan"
else:
d_left_out = d_left[:4]
d_right_out = d_right[:4]
distortion_model = "equidistant"
T_cn_cnm1 = np.eye(4)
T_cn_cnm1[:3, :3] = R
T_cn_cnm1[:3, 3] = np.asarray(T).ravel()
cam0_T = np.eye(4)
lines = []
lines.append("cam0:")
lines.append(" T_cam_imu:")
lines.append(_fmt_mat_4x4(cam0_T))
lines.append(" camera_model: pinhole")
lines.append(f" distortion_coeffs: {_fmt_vec(d_left_out)}")
lines.append(f" distortion_model: {distortion_model}")
lines.append(f" intrinsics: [{_fmt(fx)}, {_fmt(fy)}, {_fmt(cx)}, {_fmt(cy)}]")
lines.append(f" resolution: [{int(image_size[0])}, {int(image_size[1])}]")
lines.append(" rostopic: /cam0/image_raw")
lines.append("cam1:")
lines.append(" T_cn_cnm1:")
lines.append(_fmt_mat_4x4(T_cn_cnm1))
lines.append(" camera_model: pinhole")
lines.append(f" distortion_coeffs: {_fmt_vec(d_right_out)}")
lines.append(f" distortion_model: {distortion_model}")
lines.append(f" intrinsics: [{_fmt(fx2)}, {_fmt(fy2)}, {_fmt(cx2)}, {_fmt(cy2)}]")
lines.append(f" resolution: [{int(image_size[0])}, {int(image_size[1])}]")
lines.append(" rostopic: /cam1/image_raw")
with open(output_path, "w", encoding="utf-8") as f:
f.write("\n".join(lines) + "\n")
return output_path
主流程
def run_calibration(args):
pattern_size = (args.pattern_cols, args.pattern_rows)
pairs = collect_stereo_pairs(args.image_dir)
obj_template = create_object_points(pattern_size, args.square_size)
obj_points = []
img_points_left = []
img_points_right = []
frame_ids = []
failed = []
image_size = None
print(f"输入目录: {os.path.abspath(args.image_dir)}")
print(f"检测到 {len(pairs)} 组左右图，开始提取棋盘格角点（{args.pattern_cols}x{args.pattern_rows}）...")
for idx, left_path, right_path in pairs:
left_gray = cv2.imread(left_path, cv2.IMREAD_GRAYSCALE)
right_gray = cv2.imread(right_path, cv2.IMREAD_GRAYSCALE)
if left_gray is None or right_gray is None:
failed.append((idx, "image read failed"))
continue
if left_gray.shape != right_gray.shape:
failed.append((idx, "left/right size mismatch"))
continue
current_size = (left_gray.shape[1], left_gray.shape[0])
if image_size is None:
image_size = current_size
elif image_size != current_size:
failed.append((idx, "size differs from previous frames"))
continue
found_l, corners_l = find_corners(left_gray, pattern_size)
found_r, corners_r = find_corners(right_gray, pattern_size)
if not (found_l and found_r):
failed.append((idx, f"corner detection failed left={found_l} right={found_r}"))
continue
obj_points.append(obj_template.copy())
img_points_left.append(corners_l)
img_points_right.append(corners_r)
frame_ids.append(idx)
if len(obj_points) < 5:
raise RuntimeError(f"有效标定组不足: {len(obj_points)} < 5")
print(f"角点提取完成：有效 {len(obj_points)} / {len(pairs)} 组，"
f"图像尺寸 {image_size[0]} x {image_size[1]}")
if failed:
for idx, reason in failed:
print(f" 跳过 {idx}: {reason}")
print(f"\n===== 单目标定（{args.model}）=====")
# 先用全部有效帧做一次单目标定，拿到逐帧误差用于剔除离群帧。
mono_l = calibrate_mono(obj_points, img_points_left, image_size, args.model)
mono_r = calibrate_mono(obj_points, img_points_right, image_size, args.model)
rms_l, K_l, dist_l, rvecs_l, tvecs_l = mono_l
rms_r, K_r, dist_r, rvecs_r, tvecs_r = mono_r
_, per_l = compute_rms(img_points_left, obj_points, rvecs_l, tvecs_l, K_l, dist_l, args.model)
_, per_r = compute_rms(img_points_right, obj_points, rvecs_r, tvecs_r, K_r, dist_r, args.model)
keep = []
for i, (idl, e_l, e_r) in enumerate(zip(frame_ids, per_l, per_r)):
worst = max(e_l, e_r)
drop = worst > args.max_frame_rms
keep.append(not drop)
tag = "DROP" if drop else "ok"
print(f" frame {idl:>4s} left_rms={e_l:6.3f} right_rms={e_r:6.3f} {tag}")
n_dropped = sum(1 for k in keep if not k)
if n_dropped:
print(f"\n剔除 {n_dropped} 组大误差帧，重新标定...")
obj_points = [o for o, k in zip(obj_points, keep) if k]
img_points_left = [p for p, k in zip(img_points_left, keep) if k]
img_points_right = [p for p, k in zip(img_points_right, keep) if k]
frame_ids = [f for f, k in zip(frame_ids, keep) if k]
mono_l = calibrate_mono(obj_points, img_points_left, image_size, args.model)
mono_r = calibrate_mono(obj_points, img_points_right, image_size, args.model)
rms_l, K_l, dist_l, rvecs_l, tvecs_l = mono_l
rms_r, K_r, dist_r, rvecs_r, tvecs_r = mono_r
_, per_l = compute_rms(img_points_left, obj_points, rvecs_l, tvecs_l, K_l, dist_l, args.model)
_, per_r = compute_rms(img_points_right, obj_points, rvecs_r, tvecs_r, K_r, dist_r, args.model)
print(f"\n左目单标 RMS: {rms_l:.6f} px")
print(f"右目单标 RMS: {rms_r:.6f} px")
print(f"左目内参 K:\n{K_l}")
print(f"右目内参 K:\n{K_r}")
print(f"左目畸变: {np.asarray(dist_l).ravel()}")
print(f"右目畸变: {np.asarray(dist_r).ravel()}")
print(f"\n===== 双目标定（{args.model}，fix_intrinsics={args.fix_intrinsics}）=====")
stereo = calibrate_stereo(
obj_points, img_points_left, img_points_right, image_size, args.model,
K_l, dist_l, K_r, dist_r, args.fix_intrinsics,
)
stereo_rms, K_l2, d_l2, K_r2, d_r2, R, T, E, F = stereo
baseline_norm = float(np.linalg.norm(T))
baseline_x = float(abs(np.asarray(T).ravel()[0]))
print(f"双目 RMS(OpenCV): {stereo_rms:.6f} px")
print(f"R:\n{R}")
print(f"T: {np.asarray(T).ravel()} (m)")
print(f"基线 norm(T): {baseline_norm * 1000:.2f} mm")
print(f"基线 abs(Tx): {baseline_x * 1000:.2f} mm")
print(f"预期模组基线: {EXPECTED_BASELINE_M * 1000:.0f} mm (GS130W)")
if abs(baseline_norm - EXPECTED_BASELINE_M) > 0.5 * EXPECTED_BASELINE_M:
print(f" [警告] 基线 {baseline_norm*1000:.1f} mm 与预期 80 mm 差很多，标定可能有问题！")
# 去畸变后直线度校验（radtan 用来确认畸变是否拟合到位；fisheye 用来确认等距模型是否更合适）。
straight_rms, _ = straightness_check(img_points_left, K_l2, d_l2, pattern_size, args.model)
print(f"\n去畸变后直线度 RMS（左目，{args.model}）: {straight_rms:.4f} px（越小越直）")
output_path = write_kalibr_yaml(
args.output, K_l2, d_l2, K_r2, d_r2, R, T, image_size, args.model
)
print(f"\n输出 yaml: {output_path}")
if args.model == "fisheye":
print("[注意] 当前为 fisheye(equidistant) 结果，k1~k4 不能直接当 radtan 填进下游 yaml。")
# 可选：额外跑一遍 fisheye 做对照，帮助判断镜头是否真需要等距模型。
if args.model == "radtan" and args.compare_fisheye:
print(f"\n===== 对照：fisheye 单标 + 直线度 =====")
mono_fl = calibrate_mono(obj_points, img_points_left, image_size, "fisheye")
straight_fl, _ = straightness_check(img_points_left, mono_fl[1], mono_fl[2], pattern_size, "fisheye")
print(f"fisheye 左目单标 RMS: {mono_fl[0]:.6f} px")
print(f"fisheye 去畸变直线度 RMS: {straight_fl:.4f} px")
print(f"radtan 去畸变直线度 RMS: {straight_rms:.4f} px")
return output_path
def main():
args = parse_args()
if not os.path.isdir(args.image_dir):
raise RuntimeError(f"image_dir does not exist: {args.image_dir}")
run_calibration(args)
if name == “main”:
main()
这是输出的内外参数据：
cam0:
T_cam_imu:
- [1, 0, 0, 0]
- [0, 1, 0, 0]
- [0, 0, 1, 0]
- [0, 0, 0, 1]
camera_model: pinhole
distortion_coeffs: [-0.31721662039, 0.117014683228, -0.000181593970357, -0.000248241629583]
distortion_model: radtan
intrinsics: [664.432856919, 664.695802563, 542.557978907, 640.030064614]
resolution: [1088, 1280]
rostopic: /cam0/image_raw
cam1:
T_cn_cnm1:
- [0.999992467237, -0.00331026808085, 0.00202671993326, 6.83110523268e-05]
- [0.00330757127264, 0.99999364214, 0.00133253593532, -0.0800232985085]
- [-0.00203111809883, -0.00132582237702, 0.999997058373, -0.000227819310598]
- [0, 0, 0, 1]
camera_model: pinhole
distortion_coeffs: [-0.329345201684, 0.137607827237, 2.83570959091e-05, 5.53948748956e-05]
distortion_model: radtan
intrinsics: [664.506352575, 664.634680142, 541.638922966, 639.581869587]
resolution: [1088, 1280]
rostopic: /cam1/image_raw
请问这个输出有什么问题吗？
后续想用这个内外参生成”点云“和”矫正后的左图“具体怎么做呢？
