修复现存问题
This commit is contained in:
parent
0b540c197f
commit
c9958d43e3
137
main/camera.py
137
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)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user