DuOtto
148 lines
4.6 KiB
Python
148 lines
4.6 KiB
Python
import cv2, numpy as np
|
||
from calibration import object_calibration
|
||
from hand_detection import detect_hands, check_hand
|
||
from helper import compute_distance
|
||
from hud import find_hud_placement
|
||
import math
|
||
|
||
from config import PIXELS_PER_INCH, CIRCLE_TOUCH_THRESHOLD
|
||
from config import FRAME_INTERVAL, TARGET_FPS
|
||
from config import DEBUG
|
||
from setup import InitCameras
|
||
#from hud import draw_hud_box, draw_pdf_page
|
||
import time
|
||
|
||
# ----- Global State -----
|
||
measuring = False # measurement in progress
|
||
start_pt = None # measurement start point
|
||
calibrating = False # pinch-based calibration flag
|
||
object_calibrating = False # object calibration flag
|
||
fingertip_idx_global = None # last detected fingertip position
|
||
|
||
hud_pos = None
|
||
hud_rot = None
|
||
register_hud = False
|
||
|
||
# ----- Per-camera Processing -----
|
||
def process_frame(frame, camera_id: int = 0):
|
||
global measuring, start_pt, calibrating
|
||
global object_calibrating
|
||
global fingertip_idx_global
|
||
|
||
global register_hud, hud_pos, hud_rot
|
||
|
||
frame_vis = cv2.flip(frame, 1)
|
||
|
||
# Hand detection
|
||
results = detect_hands(frame_vis)
|
||
|
||
fingertip_idx = None
|
||
if results.multi_hand_landmarks:
|
||
hand = results.multi_hand_landmarks[0]
|
||
fingertip_idx, measuring, start_pt = check_hand(hand, frame_vis, object_calibrating)
|
||
|
||
fingertip_idx_global = fingertip_idx
|
||
|
||
# object calibration
|
||
if object_calibrating and fingertip_idx is not None:
|
||
object_calibration(frame_vis, fingertip_idx, CIRCLE_TOUCH_THRESHOLD)
|
||
object_calibrating = False
|
||
|
||
# place hud
|
||
res = None
|
||
if hud_pos is None:
|
||
res = find_hud_placement(frame_vis)
|
||
else:
|
||
res = hud_pos, hud_rot
|
||
|
||
# create proj output
|
||
projection_out = np.zeros_like(frame)
|
||
|
||
# measuring
|
||
if measuring and start_pt and fingertip_idx_global:
|
||
cv2.line(projection_out, start_pt, fingertip_idx_global, (255,0,0),2)
|
||
px = compute_distance(start_pt, fingertip_idx_global)
|
||
inch = px/PIXELS_PER_INCH; cm = inch*2.54
|
||
mid = ((start_pt[0]+fingertip_idx_global[0])//2,(start_pt[1]+fingertip_idx_global[1])//2)
|
||
cv2.putText(projection_out,f"{inch:.2f}in/{cm:.1f}cm",(mid[0]+10,mid[1]-10), cv2.FONT_HERSHEY_SIMPLEX,0.7,(255,0,255),2)
|
||
|
||
# hud
|
||
if res is not None or hud_pos:
|
||
if hud_pos:
|
||
arrow_tip = hud_pos
|
||
dir_vec = hud_rot
|
||
else:
|
||
arrow_tip, dir_vec = res
|
||
|
||
dx, dy = dir_vec
|
||
arrow_angle = math.atan2(dy, dx) # result in radians
|
||
#quad = draw_hud_box(projection_out, arrow_tip, arrow_angle, size=(400,400))
|
||
#draw_pdf_page(projection_out, page_index=0, dst_quad=quad)
|
||
|
||
if register_hud:
|
||
hud_pos = arrow_tip
|
||
hud_rot = dir_vec
|
||
register_hud = False
|
||
|
||
|
||
mask = cv2.cvtColor(projection_out, cv2.COLOR_BGR2GRAY) > 0
|
||
frame_vis[mask] = projection_out[mask]
|
||
|
||
return frame_vis, projection_out
|
||
|
||
# ----- Main ----- -------------------------------------------------------------------------------------
|
||
def main():
|
||
all_captures = InitCameras()
|
||
|
||
print(f"Running at up to {TARGET_FPS} FPS (interval={FRAME_INTERVAL:.3f}s)")
|
||
print("Press 'o' for circle calib, 'q' to quit.")
|
||
|
||
last_time = time.time()
|
||
while True:
|
||
processed_frames = []
|
||
|
||
for camera_id, capture in enumerate(all_captures):
|
||
ok, frame = capture.read()
|
||
if not ok or frame is None:
|
||
continue
|
||
|
||
projection, debug = process_frame(frame, camera_id)
|
||
processed_frames.append((camera_id, projection, debug))
|
||
|
||
if not processed_frames:
|
||
break
|
||
|
||
for camera_id, projection, debug in processed_frames:
|
||
if DEBUG:
|
||
cv2.imshow(f'Debug Output (cam {camera_id})', debug)
|
||
cv2.imshow(f'Projector Output (cam {camera_id})', projection)
|
||
|
||
key = cv2.waitKey(1) & 0xFF
|
||
|
||
if key == ord('q'):
|
||
break
|
||
elif key == ord('o'):
|
||
global object_calibrating
|
||
object_calibrating = True
|
||
print("Entered circle calibration mode.")
|
||
elif key == ord('j'):
|
||
global register_hud, hud_pos
|
||
register_hud = True
|
||
hud_pos = None
|
||
|
||
# ——— frame‐rate limiting ———
|
||
now = time.time()
|
||
elapsed = now - last_time
|
||
to_wait = FRAME_INTERVAL - elapsed
|
||
if to_wait > 0:
|
||
time.sleep(to_wait)
|
||
last_time = time.time()
|
||
|
||
for capture in all_captures:
|
||
capture.release()
|
||
|
||
cv2.destroyAllWindows()
|
||
|
||
if __name__ == '__main__':
|
||
main()
|