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))
|