|
|
# ============================================================
|
|
|
# INTEGRATION_GUIDE.py
|
|
|
# Гайд по интеграции модулей перехвата в main.py
|
|
|
#
|
|
|
# НЕ ЗАПУСКАТЬ — это документация с фрагментами кода.
|
|
|
# Каждый блок показывает, куда именно вставить код в main.py.
|
|
|
# ============================================================
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 1: ИМПОРТЫ (добавить в начало main.py, после существующих)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
import config_intercept # новый конфиг
|
|
|
from config_intercept import *
|
|
|
from range_estimation import RangeEstimator
|
|
|
from proportional_navigation import ProportionalNavigator
|
|
|
from intercept_fsm import InterceptFSM, InterceptPhase
|
|
|
from autopilot_bridge import AutopilotBridge
|
|
|
from imm_filter import IMMFilter
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 2: ИНИЦИАЛИЗАЦИЯ (в main(), после создания guidance_ctrl)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# --- Intercept modules ---
|
|
|
range_est = RangeEstimator()
|
|
|
print(range_est.status_line())
|
|
|
|
|
|
pn_nav = ProportionalNavigator()
|
|
|
print(pn_nav.status_line())
|
|
|
|
|
|
intercept_fsm = InterceptFSM()
|
|
|
print(intercept_fsm.status_line())
|
|
|
|
|
|
autopilot = AutopilotBridge()
|
|
|
autopilot.start()
|
|
|
print(autopilot.status_line())
|
|
|
|
|
|
imm = IMMFilter()
|
|
|
print(imm.status_line())
|
|
|
|
|
|
# Состояния для нового пайплайна
|
|
|
range_state = None
|
|
|
pn_state = None
|
|
|
intercept_params = None
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 3: KALMAN → IMM (опционально, если IMM_REPLACE_KALMAN=True)
|
|
|
# Заменяет строку: kf = Kalman8D()
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
if IMM_REPLACE_KALMAN and IMM_ENABLE:
|
|
|
kf = imm # IMMFilter имеет тот же API что Kalman8D
|
|
|
else:
|
|
|
kf = Kalman8D()
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 4: RANGE ESTIMATION (внутри главного цикла while True,
|
|
|
# ПОСЛЕ того как locked_box_eff обновлён и guidance_state
|
|
|
# вычислен, т.е. перед отрисовкой)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# --- Range estimation ---
|
|
|
if confirmed and locked_box_eff is not None:
|
|
|
range_state = range_est.update(
|
|
|
locked_box_eff, frame_ts, dt, confirmed=True
|
|
|
)
|
|
|
elif pred_box_eff is not None:
|
|
|
range_state = range_est.update(
|
|
|
pred_box_eff, frame_ts, dt, confirmed=False
|
|
|
)
|
|
|
else:
|
|
|
range_state = range_est.update(None, frame_ts, dt)
|
|
|
|
|
|
# При потере цели — сброс
|
|
|
if miss_streak >= MAX_MISSES:
|
|
|
range_est.reset()
|
|
|
range_state = None
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 5: INTERCEPT FSM (после range estimation)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# --- Intercept FSM ---
|
|
|
intercept_params = intercept_fsm.update(
|
|
|
confirmed=confirmed,
|
|
|
hit_streak=hit_streak,
|
|
|
miss_streak=miss_streak,
|
|
|
range_state=range_state,
|
|
|
guidance_state=guidance_state,
|
|
|
frame_id=frame_id,
|
|
|
)
|
|
|
|
|
|
# FSM может переопределить det_every
|
|
|
if intercept_params.get("det_every_override") is not None:
|
|
|
det_every = min(int(det_every),
|
|
|
int(intercept_params["det_every_override"]))
|
|
|
|
|
|
# FSM может заставить fullscan
|
|
|
if intercept_params.get("force_fullscan"):
|
|
|
need_fullscan = True
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 6: PROPORTIONAL NAVIGATION (после FSM, перед autopilot)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# --- Proportional Navigation ---
|
|
|
pn_state = None
|
|
|
if intercept_params.get("use_pn") and confirmed:
|
|
|
# Если есть 3D-оценка дальности
|
|
|
if (range_state is not None
|
|
|
and range_state.get("range_m") is not None
|
|
|
and range_state["confidence"] > 0.3):
|
|
|
|
|
|
# Позиция цели в метрах (упрощённо: проекция из пикселей)
|
|
|
target_center = box_center(locked_box_eff if locked_box_eff
|
|
|
is not None else pred_box_eff)
|
|
|
my_center = np.array([ew * 0.5, eh * 0.5], dtype=np.float32)
|
|
|
|
|
|
# Пиксели → метры (через range и FOV)
|
|
|
range_m = float(range_state["range_m"])
|
|
|
px_to_m = range_m / max(1.0, range_est.focal_px)
|
|
|
|
|
|
target_pos_m = (target_center - my_center) * px_to_m
|
|
|
my_pos_m = np.zeros(2, dtype=np.float32)
|
|
|
|
|
|
# Скорости
|
|
|
vx_px = float(kf.x[4, 0]) if kf.initialized else 0.0
|
|
|
vy_px = float(kf.x[5, 0]) if kf.initialized else 0.0
|
|
|
target_vel_m = np.array([vx_px, vy_px], dtype=np.float32) * px_to_m
|
|
|
my_vel_m = np.zeros(2, dtype=np.float32) # неизвестна без IMU
|
|
|
|
|
|
pn_state = pn_nav.update_3d(
|
|
|
my_pos_m, my_vel_m,
|
|
|
target_pos_m, target_vel_m,
|
|
|
closing_vel=range_state.get("closing_vel", 0.0),
|
|
|
timestamp=frame_ts,
|
|
|
)
|
|
|
else:
|
|
|
# Fallback: screen-space PN
|
|
|
target_center = None
|
|
|
if locked_box_eff is not None:
|
|
|
target_center = box_center(locked_box_eff)
|
|
|
elif pred_box_eff is not None:
|
|
|
target_center = box_center(pred_box_eff)
|
|
|
|
|
|
target_vel_px = None
|
|
|
if kf.initialized:
|
|
|
target_vel_px = np.array(
|
|
|
[float(kf.x[4, 0]), float(kf.x[5, 0])],
|
|
|
dtype=np.float32,
|
|
|
)
|
|
|
|
|
|
if target_center is not None:
|
|
|
pn_state = pn_nav.update_screen(
|
|
|
target_center, ew, eh,
|
|
|
target_vel_px, frame_ts,
|
|
|
)
|
|
|
else:
|
|
|
pn_nav.reset()
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 7: AUTOPILOT BRIDGE (после PN)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# --- Autopilot bridge ---
|
|
|
autopilot_cmd = autopilot.update(
|
|
|
frame_id=frame_id,
|
|
|
timestamp=frame_ts,
|
|
|
guidance_state=guidance_state,
|
|
|
intercept_params=intercept_params,
|
|
|
range_state=range_state,
|
|
|
pn_state=pn_state,
|
|
|
confirmed=confirmed,
|
|
|
miss_streak=miss_streak,
|
|
|
)
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 8: ОТРИСОВКА (после существующей отрисовки, перед writer)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# --- Intercept overlays ---
|
|
|
if range_state is not None:
|
|
|
range_est.draw_overlay(frame_orig, range_state, sx, sy)
|
|
|
|
|
|
if pn_state is not None:
|
|
|
pn_nav.draw_overlay(frame_orig, pn_state, sx, sy)
|
|
|
|
|
|
if intercept_params is not None:
|
|
|
intercept_fsm.draw_overlay(frame_orig, intercept_params)
|
|
|
|
|
|
if IMM_ENABLE and IMM_REPLACE_KALMAN:
|
|
|
imm.draw_overlay(frame_orig)
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 9: ЛОГИРОВАНИЕ (расширить track_logger.log_frame)
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
# Добавить к существующему вызову track_logger.log_frame():
|
|
|
# (нужно расширить FIELDNAMES в decision_logger.py)
|
|
|
|
|
|
# Или создать отдельный лог:
|
|
|
if intercept_params and range_state:
|
|
|
# Дополнительные метрики для анализа
|
|
|
pass # см. decision_logger.py → добавить поля
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 10: CLEANUP (после главного цикла, перед cap.release())
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
autopilot.stop()
|
|
|
# Остальной cleanup как прежде:
|
|
|
autogaze_worker.stop()
|
|
|
yolo_worker.stop()
|
|
|
...
|
|
|
"""
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# ШАГ 11: СБРОС ПРИ ПОТЕРЕ ЦЕЛИ
|
|
|
# В блоках где происходит полный reset (miss_streak >= MAX_MISSES
|
|
|
# и другие блоки сброса в main.py), добавить:
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
"""
|
|
|
range_est.reset()
|
|
|
pn_nav.reset()
|
|
|
intercept_fsm.reset()
|
|
|
range_state = None
|
|
|
pn_state = None
|
|
|
intercept_params = None
|
|
|
"""
|
|
|
|
|
|
# =============================================================
|
|
|
# ПОРЯДОК ВЫЗОВОВ В КАЖДОМ КАДРЕ (итоговый пайплайн):
|
|
|
# =============================================================
|
|
|
#
|
|
|
# 1. Чтение кадра, get_effective_frame
|
|
|
# 2. Camera motion compensation
|
|
|
# 3. Kalman/IMM predict
|
|
|
# 4. KLT update
|
|
|
# 5. YOLO submit / get
|
|
|
# 6. ByteTrack update
|
|
|
# 7. Target selection (существующая логика)
|
|
|
# 8. Kalman/IMM update (chosen box)
|
|
|
# 9. Guidance update (screen guidance) ← существующий
|
|
|
# 10. Range estimation update ← НОВЫЙ
|
|
|
# 11. Intercept FSM update ← НОВЫЙ
|
|
|
# 12. Proportional Navigation update ← НОВЫЙ
|
|
|
# 13. Autopilot bridge update ← НОВЫЙ
|
|
|
# 14. Decision logger
|
|
|
# 15. Draw overlays
|
|
|
# 16. Video writer / display
|
|
|
# =============================================================
|
|
|
|
|
|
# =============================================================
|
|
|
# КАЛИБРОВКА КАМЕРЫ
|
|
|
# =============================================================
|
|
|
#
|
|
|
# Для точной оценки дальности нужно измерить focal_length_px.
|
|
|
#
|
|
|
# Метод 1 (рекомендуемый):
|
|
|
# 1. Поставить объект известного размера (например, 30см линейку)
|
|
|
# на известном расстоянии (например, 5 метров)
|
|
|
# 2. Сфотографировать, измерить размер в пикселях
|
|
|
# 3. focal_px = (pixel_size * distance) / real_size
|
|
|
# Пример: линейка 30см = 42px на расстоянии 5м:
|
|
|
# focal_px = 42 * 5.0 / 0.3 = 700
|
|
|
#
|
|
|
# Метод 2 (из спецификации камеры):
|
|
|
# focal_px = focal_mm * image_width_px / sensor_width_mm
|
|
|
# Типичные FPV камеры:
|
|
|
# - Caddx Ratel 2: 2.1mm lens, 1/1.8" sensor → ~350px
|
|
|
# - RunCam Phoenix: 2.1mm lens, 1/2" sensor → ~315px
|
|
|
# - DJI O3 Air: 2.1mm lens → ~420px
|
|
|
#
|
|
|
# Метод 3 (автокалибровка, грубый):
|
|
|
# Если знаете размер цели (0.27м для FPV дрона) и примерно
|
|
|
# расстояние при первом захвате, можно подобрать focal_px
|
|
|
# итеративно.
|
|
|
# =============================================================
|