26 lines
1.2 KiB
Python
26 lines
1.2 KiB
Python
"""像素偏移 → 视线角（针孔模型）。
|
|
|
|
对应 C++ 参考实现 reference-c++/visual_track_control/src/line_of_sight.cpp
|
|
（规格：docs/物理追踪思路.md §4.1 水平 β / §5.1 垂直 α）。
|
|
|
|
全程使用精确针孔模型 angle = atan(offset · 2·tan(θ/2) / size)：原线性近似
|
|
双路切换在边界两侧不连续（θ=60°、边界比例 0.2 处约 8% 台阶），会向控制
|
|
环路注入突变；atan 代价可忽略。构造期预计算针孔系数，Convert 仅一次乘加。
|
|
"""
|
|
|
|
import math
|
|
|
|
|
|
class PixelToAngle:
|
|
def __init__(self, size_px: float, fov_rad: float) -> None:
|
|
# 构造期校验：非法参数直接抛异常（初始化抛异常规范）
|
|
if not (size_px > 0.0):
|
|
raise ValueError("PixelToAngle: size_px 必须为正")
|
|
if not (0.0 < fov_rad < math.pi):
|
|
raise ValueError("PixelToAngle: fov_rad 必须在 (0, π) 内")
|
|
self._pinhole_scale = 2.0 * math.tan(fov_rad / 2.0) / size_px
|
|
|
|
def convert(self, offset_px: float) -> float:
|
|
"""偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）。"""
|
|
return math.atan(offset_px * self._pinhole_scale)
|