From c9958d43e30ca920dd133d75e7267cd1d746021e Mon Sep 17 00:00:00 2001 From: Cx330 <1487537121@qq.com> Date: Sat, 9 May 2026 14:42:47 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=8E=B0=E5=AD=98=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main/camera.py | 137 +++++++++++-------------------------------------- 1 file changed, 30 insertions(+), 107 deletions(-) diff --git a/main/camera.py b/main/camera.py index 2bf9391..0d46d87 100644 --- a/main/camera.py +++ b/main/camera.py @@ -16,18 +16,11 @@ FRAME_HEIGHT = 240 GUIDE_CENTER_X = 160 GUIDE_CENTER_Y = 120 CONTROL_DEADZONE = 15 -CONTROL_BASE_SPEED = 0.55 -CONTROL_MAX_SPEED = 1.0 -CONTROL_LOOKAHEAD_FRAMES = 1.4 -CONTROL_LOOKAHEAD_GAIN = 0.06 MIN_CONTOUR_AREA = 90 MAX_CONTOUR_AREA = 6000 EDGE_IGNORE = 12 TARGET_LOCK_DURATION = 3.0 -TRACK_MAX_MISSES = 4 -TRACK_FAST_MOVE_PX = 24.0 -TRACK_CENTER_WINDOW = 28 @dataclass @@ -259,38 +252,18 @@ def update_track_state(track_state, candidate): bbox=candidate["bbox"], ) - raw_dx = candidate["cx"] - track_state.cx - raw_dy = candidate["cy"] - track_state.cy - motion_mag = math.hypot(raw_dx, raw_dy) - crossed_guide_x = (track_state.cx - GUIDE_CENTER_X) * (candidate["cx"] - GUIDE_CENTER_X) < 0 - near_guide_x = ( - abs(track_state.cx - GUIDE_CENTER_X) < TRACK_CENTER_WINDOW - or abs(candidate["cx"] - GUIDE_CENTER_X) < TRACK_CENTER_WINDOW - ) - if track_state.misses > 0: - smooth_prev = 0.30 + smooth_prev = 0.35 else: - smooth_prev = 0.58 - - if motion_mag > TRACK_FAST_MOVE_PX: - smooth_prev = min(smooth_prev, 0.34) - if crossed_guide_x and near_guide_x: - smooth_prev = min(smooth_prev, 0.18) + smooth_prev = 0.60 new_cx = int(smooth_prev * track_state.cx + (1.0 - smooth_prev) * candidate["cx"]) new_cy = int(smooth_prev * track_state.cy + (1.0 - smooth_prev) * candidate["cy"]) raw_vx = new_cx - track_state.cx raw_vy = new_cy - track_state.cy - velocity_keep = 0.60 - if motion_mag > TRACK_FAST_MOVE_PX: - velocity_keep = 0.42 - if crossed_guide_x and near_guide_x: - velocity_keep = 0.30 - - new_vx = velocity_keep * track_state.vx + (1.0 - velocity_keep) * raw_vx - new_vy = velocity_keep * track_state.vy + (1.0 - velocity_keep) * raw_vy + new_vx = 0.6 * track_state.vx + 0.4 * raw_vx + new_vy = 0.6 * track_state.vy + 0.4 * raw_vy track_state.cx = new_cx track_state.cy = new_cy @@ -303,46 +276,6 @@ def update_track_state(track_state, candidate): return track_state -def predict_track_state(track_state): - if track_state is None: - return None - - x, y, w, h = track_state.bbox - next_cx = int(np.clip(track_state.cx + track_state.vx, 0, FRAME_WIDTH - 1)) - next_cy = int(np.clip(track_state.cy + track_state.vy, 0, FRAME_HEIGHT - 1)) - next_x = int(np.clip(x + track_state.vx, 0, max(0, FRAME_WIDTH - w))) - next_y = int(np.clip(y + track_state.vy, 0, max(0, FRAME_HEIGHT - h))) - - track_state.cx = next_cx - track_state.cy = next_cy - track_state.bbox = (next_x, next_y, w, h) - track_state.vx *= 0.82 - track_state.vy *= 0.82 - track_state.misses += 1 - track_state.visible = False - return track_state - - -def predicted_control_point(track_state): - if track_state is None: - return GUIDE_CENTER_X, GUIDE_CENTER_Y - - speed = math.hypot(track_state.vx, track_state.vy) - lookahead = CONTROL_LOOKAHEAD_FRAMES + min(1.3, speed * CONTROL_LOOKAHEAD_GAIN) - px = int(np.clip(track_state.cx + track_state.vx * lookahead, 0, FRAME_WIDTH - 1)) - py = int(np.clip(track_state.cy + track_state.vy * lookahead, 0, FRAME_HEIGHT - 1)) - return px, py - - -def control_speed(error, velocity): - magnitude = abs(error) + abs(velocity) * 1.3 - if magnitude <= CONTROL_DEADZONE: - return 0.0 - - normalized = min(1.0, (magnitude - CONTROL_DEADZONE) / 90.0) - return CONTROL_BASE_SPEED + normalized * (CONTROL_MAX_SPEED - CONTROL_BASE_SPEED) - - def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remaining): frame_width = frame.shape[1] cv2.circle(frame, (GUIDE_CENTER_X, GUIDE_CENTER_Y), 5, (255, 0, 0), -1) @@ -375,8 +308,6 @@ def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remain color = (0, 255, 0) if track_state.visible else (0, 165, 255) cv2.rectangle(frame, (x, y), (x + w, y + h), color, 2) cv2.circle(frame, (track_state.cx, track_state.cy), 5, (0, 0, 255), -1) - predicted_x, predicted_y = predicted_control_point(track_state) - cv2.circle(frame, (predicted_x, predicted_y), 4, (0, 255, 255), -1) status = "track" if track_state.visible else f"hold:{track_state.misses}" cv2.putText( @@ -397,38 +328,34 @@ def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remain def drive_to_target(track_state, lock_active=False): - if lock_active or track_state is None: + if lock_active or track_state is None or not track_state.visible: motor.stop() return - target_x, target_y = predicted_control_point(track_state) - dx = target_x - GUIDE_CENTER_X - dy = target_y - GUIDE_CENTER_Y - speed_x = control_speed(dx, track_state.vx) - speed_y = control_speed(dy, track_state.vy) + dx = track_state.cx - GUIDE_CENTER_X + dy = track_state.cy - GUIDE_CENTER_Y - if speed_x == 0.0 and speed_y == 0.0: + if abs(dx) < CONTROL_DEADZONE and abs(dy) < CONTROL_DEADZONE: motor.stop() - elif speed_x == 0.0: - if dy > 0: - motor.move_right(speed=speed_y) + elif abs(dx) < CONTROL_DEADZONE: + if dy > CONTROL_DEADZONE: + motor.move_right(speed=1.0) else: - motor.move_left(speed=speed_y) - elif speed_y == 0.0: - if dx > 0: - motor.backward(speed=speed_x) + motor.move_left(speed=1.0) + elif abs(dy) < CONTROL_DEADZONE: + if dx > CONTROL_DEADZONE: + motor.forward(speed=1.0) else: - motor.forward(speed=speed_x) + motor.backward(speed=1.0) else: - speed = max(speed_x, speed_y) - if dx > 0 and dy > 0: - motor.move_right_backward(speed=speed) - elif dx > 0 and dy < 0: - motor.move_left_backward(speed=speed) - elif dx < 0 and dy > 0: - motor.move_right_forward(speed=speed) + if dx > CONTROL_DEADZONE and dy > CONTROL_DEADZONE: + motor.move_right_backward(speed=1.0) + elif dx > CONTROL_DEADZONE and dy < -CONTROL_DEADZONE: + motor.move_left_backward(speed=1.0) + elif dx < -CONTROL_DEADZONE and dy > CONTROL_DEADZONE: + motor.move_right_forward(speed=1.0) else: - motor.move_left_forward(speed=speed) + motor.move_left_forward(speed=1.0) print("Starting camera tracking...") @@ -485,18 +412,14 @@ while True: selected = None else: selected = select_target(candidates, track_state) - if selected is not None: - track_state = update_track_state(track_state, selected) - elif track_state is not None: - track_state = predict_track_state(track_state) - if track_state.misses > TRACK_MAX_MISSES: - motor.stop() - track_state = None - lock_until = now + TARGET_LOCK_DURATION - lock_remaining = TARGET_LOCK_DURATION - lock_active = True - else: + if track_state is not None and selected is None: + motor.stop() track_state = None + lock_until = now + TARGET_LOCK_DURATION + lock_remaining = TARGET_LOCK_DURATION + lock_active = True + else: + track_state = update_track_state(track_state, selected) draw_debug(frame, motion_mask, track_state, motion_level, inlier_count, lock_remaining) drive_to_target(track_state, lock_active=lock_active)