127 lines
4.9 KiB
Python
127 lines
4.9 KiB
Python
"""平面控制点标定和像素/世界坐标转换。"""
|
||
import math
|
||
|
||
|
||
class CalibrationError(ValueError):
|
||
pass
|
||
|
||
|
||
def _cv():
|
||
try:
|
||
import cv2
|
||
import numpy as np
|
||
return cv2, np
|
||
except Exception as exc:
|
||
raise CalibrationError("OpenCV/Numpy 不可用，无法执行标定") from exc
|
||
|
||
|
||
def map_point(homography, u, v):
|
||
"""使用 3x3 Homography 将像素点映射到世界平面。"""
|
||
_cv2, np = _cv()
|
||
matrix = np.asarray(homography, dtype=np.float64)
|
||
if matrix.shape != (3, 3) or not np.isfinite(matrix).all():
|
||
raise CalibrationError("Homography 格式无效")
|
||
p = matrix.dot(np.asarray([float(u), float(v), 1.0], dtype=np.float64))
|
||
if abs(float(p[2])) < 1e-10:
|
||
raise CalibrationError("像素点无法投影到地面")
|
||
return float(p[0] / p[2]), float(p[1] / p[2])
|
||
|
||
|
||
def _area_ratio(points, frame_width, frame_height):
|
||
cv2, np = _cv()
|
||
hull = cv2.convexHull(np.asarray(points, dtype=np.float32))
|
||
denom = max(1.0, float(frame_width) * float(frame_height))
|
||
return float(cv2.contourArea(hull)) / denom
|
||
|
||
|
||
def solve_planar_calibration(observations, frame_width, frame_height,
|
||
max_validation_mean_m=1.0):
|
||
"""根据拟合点求 H，并使用完全独立的验证点评价世界坐标误差。"""
|
||
cv2, np = _cv()
|
||
try:
|
||
fw, fh = int(frame_width), int(frame_height)
|
||
except Exception as exc:
|
||
raise CalibrationError("图像尺寸无效") from exc
|
||
if fw <= 0 or fh <= 0:
|
||
raise CalibrationError("图像尺寸必须大于 0")
|
||
|
||
fit = [p for p in observations if p.get("role", "fit") == "fit"]
|
||
verify = [p for p in observations if p.get("role") == "verify"]
|
||
if len(fit) < 4:
|
||
raise CalibrationError("至少需要 4 个拟合点")
|
||
if len(verify) < 3:
|
||
raise CalibrationError("至少需要 3 个独立验证点")
|
||
|
||
def pixels(rows):
|
||
return np.asarray([[float(p["u"]), float(p["v"])] for p in rows], dtype=np.float64)
|
||
|
||
def worlds(rows):
|
||
return np.asarray([[float(p["x"]), float(p["y"])] for p in rows], dtype=np.float64)
|
||
|
||
src, dst = pixels(fit), worlds(fit)
|
||
if not np.isfinite(src).all() or not np.isfinite(dst).all():
|
||
raise CalibrationError("控制点包含非有限数值")
|
||
if _area_ratio(src, fw, fh) < 0.01:
|
||
raise CalibrationError("拟合点共线或过度集中，请扩大点位分布")
|
||
if float(cv2.contourArea(cv2.convexHull(dst.astype(np.float32)))) < 0.01:
|
||
raise CalibrationError("世界坐标点共线或过度集中")
|
||
|
||
matrix, mask = cv2.findHomography(src, dst, cv2.RANSAC, 0.75)
|
||
if matrix is None or not np.isfinite(matrix).all() or abs(float(np.linalg.det(matrix))) < 1e-12:
|
||
raise CalibrationError("无法计算稳定的 Homography")
|
||
|
||
fit_projection = cv2.perspectiveTransform(src.reshape(-1, 1, 2), matrix).reshape(-1, 2)
|
||
fit_errors = np.linalg.norm(fit_projection - dst, axis=1)
|
||
verify_src, verify_dst = pixels(verify), worlds(verify)
|
||
verify_projection = cv2.perspectiveTransform(verify_src.reshape(-1, 1, 2), matrix).reshape(-1, 2)
|
||
verify_errors = np.linalg.norm(verify_projection - verify_dst, axis=1)
|
||
|
||
fit_rmse = math.sqrt(float(np.mean(np.square(fit_errors))))
|
||
validation_mean = float(np.mean(verify_errors))
|
||
validation_max = float(np.max(verify_errors))
|
||
warnings = []
|
||
if len(fit) < 8:
|
||
warnings.append("拟合点少于建议的 8 个")
|
||
coverage = _area_ratio(src, fw, fh)
|
||
if coverage < 0.15:
|
||
warnings.append("拟合点仅覆盖画面 %.1f%%，建议增加边缘和远端点" % (coverage * 100.0))
|
||
inliers = int(mask.sum()) if mask is not None else len(fit)
|
||
if inliers < len(fit):
|
||
warnings.append("RANSAC 排除了 %d 个异常拟合点" % (len(fit) - inliers))
|
||
|
||
detail = []
|
||
fit_index = verify_index = 0
|
||
for p in observations:
|
||
row = dict(p)
|
||
if p.get("role", "fit") == "fit":
|
||
row["error_m"] = float(fit_errors[fit_index])
|
||
fit_index += 1
|
||
else:
|
||
row["error_m"] = float(verify_errors[verify_index])
|
||
verify_index += 1
|
||
detail.append(row)
|
||
|
||
return {
|
||
"homography": matrix.tolist(),
|
||
"fit_rmse_m": fit_rmse,
|
||
"validation_mean_m": validation_mean,
|
||
"validation_max_m": validation_max,
|
||
"coverage_ratio": coverage,
|
||
"inlier_count": inliers,
|
||
"is_valid": validation_mean <= float(max_validation_mean_m),
|
||
"warnings": warnings,
|
||
"observations": detail,
|
||
}
|
||
|
||
|
||
def foot_point_world(box, homography, width_m, height_m, boundary_margin_m=1.0):
|
||
if not box or len(box) != 4:
|
||
return None
|
||
u = (float(box[0]) + float(box[2])) * 0.5
|
||
v = float(box[3])
|
||
x, y = map_point(homography, u, v)
|
||
margin = max(0.0, float(boundary_margin_m))
|
||
if x < -margin or y < -margin or x > float(width_m) + margin or y > float(height_m) + margin:
|
||
return None
|
||
return max(0.0, min(float(width_m), x)), max(0.0, min(float(height_m), y))
|