root/Autoscope-Controller
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

testing pos tracking

Browse source
This commit is contained in:
2ManyProjects 2026-01-11 02:23:41 -06:00
parent 34f84d760e
commit c31220532b
1 changed files with 143 additions and 123 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

260
src/stitching_scanner.py
View file

@ -694,95 +694,114 @@ class StitchingScanner:
mh, mw = self.mosaic.shape[:2] mh, mw = self.mosaic.shape[:2]
fh, fw = frame.shape[:2] fh, fw = frame.shape[:2]
self.log(f"=== First Strip of Row ({direction.value}) ===")
self.log(f" Mosaic: {mw}x{mh}, Frame: {fw}x{fh}")
self.log(f" Alignment input: X={alignment.x_offset:.1f}, Y={alignment.y_offset:.1f}, valid={alignment.valid}")
# Apply alignment to cumulative tracking # Apply alignment to cumulative tracking
if alignment.valid: if alignment.valid:
self._cumulative_align_x += alignment.x_offset self._cumulative_align_x += alignment.x_offset
self._cumulative_align_y += alignment.y_offset self._cumulative_align_y += alignment.y_offset
self._last_strip_alignment = alignment self._last_strip_alignment = alignment
self.log(f"Applied first-strip alignment: X={alignment.x_offset:.1f}, Y={alignment.y_offset:.1f}")
# Calculate Y position - frame overlaps with bottom of mosaic # Calculate Y position - frame overlaps with bottom of mosaic
row_overlap_pixels = int(fh * self.config.row_overlap) row_overlap_pixels = int(fh * self.config.row_overlap)
y_offset = mh - row_overlap_pixels + int(round(self._cumulative_align_y)) y_offset = mh - row_overlap_pixels + int(round(self._cumulative_align_y))
y_offset = max(0, y_offset) y_offset = max(0, min(y_offset, mh - fh)) # Clamp to valid range
if direction == ScanDirection.LEFT: if direction == ScanDirection.LEFT:
# Starting at RIGHT edge, going LEFT
# Frame's RIGHT edge aligns with mosaic's RIGHT edge # Frame's RIGHT edge aligns with mosaic's RIGHT edge
# x_offset is where the LEFT edge of the frame goes
x_offset = mw - fw + int(round(self._cumulative_align_x)) x_offset = mw - fw + int(round(self._cumulative_align_x))
x_offset = max(0, x_offset) x_offset = max(0, min(x_offset, mw - fw))
self.log(f"=== First Strip of Row (LEFT) ===") # For LEFT scanning, current_x tracks where LEFT edge of current frame is
self.log(f" Mosaic: {mw}x{mh}, Frame: {fw}x{fh}") # This will DECREASE as we scan left
self.log(f" X offset: {x_offset} (frame right edge at {x_offset + fw})") start_x_for_scanning = x_offset
self.log(f" Y offset: {y_offset}")
else: # RIGHT else: # RIGHT
# Frame's LEFT edge aligns with mosaic's LEFT edge # Starting at LEFT edge, going RIGHT
x_offset = int(round(self._cumulative_align_x)) x_offset = int(round(self._cumulative_align_x))
x_offset = max(0, x_offset) x_offset = max(0, x_offset)
start_x_for_scanning = 0
self.log(f"=== First Strip of Row (RIGHT) ===") self.log(f" Calculated x_offset: {x_offset}, y_offset: {y_offset}")
self.log(f" Mosaic: {mw}x{mh}, Frame: {fw}x{fh}")
self.log(f" X offset: {x_offset}")
self.log(f" Y offset: {y_offset}")
# Blend frame into mosaic at calculated position # Blend frame into mosaic at calculated position
# Simply overwrite with blending - no expansion needed
result = self.mosaic.copy() result = self.mosaic.copy()
# Calculate the region to blend # Calculate valid region
x_end = min(x_offset + fw, mw) x_end = min(x_offset + fw, mw)
y_end = min(y_offset + fh, mh) y_end = min(y_offset + fh, mh)
region_w = x_end - x_offset frame_x_end = x_end - x_offset
region_h = y_end - y_offset frame_y_end = y_end - y_offset
if region_w <= 0 or region_h <= 0: if frame_x_end <= 0 or frame_y_end <= 0:
self.log(f" WARNING: No valid region to blend") self.log(f" WARNING: No valid region to blend")
return return
# Create alpha mask for blending self.log(f" Blending region: mosaic[{y_offset}:{y_end}, {x_offset}:{x_end}]")
# Blend at top edge (with row above) and appropriate side edge self.log(f" Frame region: frame[0:{frame_y_end}, 0:{frame_x_end}]")
alpha = np.ones((region_h, region_w), dtype=np.float32)
# Vertical blend at top (first ~20% of overlap region) # Create alpha mask for smooth blending
v_blend = min(row_overlap_pixels, int(region_h * 0.3)) alpha = np.ones((frame_y_end, frame_x_end), dtype=np.float32)
if v_blend > 0:
# Vertical blend at top (blending with row above)
v_blend = min(row_overlap_pixels // 2, frame_y_end // 3)
if v_blend > 5:
v_gradient = np.linspace(0, 1, v_blend, dtype=np.float32)[:, np.newaxis] v_gradient = np.linspace(0, 1, v_blend, dtype=np.float32)[:, np.newaxis]
alpha[:v_blend, :] = v_gradient alpha[:v_blend, :] *= v_gradient
# Horizontal blend at edge # Horizontal blend at the edge we came from
h_blend = min(BLEND_WIDTH, int(region_w * 0.2)) h_blend = min(BLEND_WIDTH, frame_x_end // 4)
if h_blend > 0: if h_blend > 5:
if direction == ScanDirection.LEFT: if direction == ScanDirection.LEFT:
# Blend on right edge (where we came from) # Came from right, blend right edge
h_gradient = np.linspace(1, 0, h_blend, dtype=np.float32)[np.newaxis, :] h_gradient = np.linspace(1, 0, h_blend, dtype=np.float32)[np.newaxis, :]
alpha[:, -h_blend:] = np.minimum(alpha[:, -h_blend:], h_gradient) alpha[:, -h_blend:] *= h_gradient
else: else:
# Blend on left edge # Came from left (or starting), blend left edge if not at edge
h_gradient = np.linspace(0, 1, h_blend, dtype=np.float32)[np.newaxis, :] if x_offset > 0:
alpha[:, :h_blend] = np.minimum(alpha[:, :h_blend], h_gradient) h_gradient = np.linspace(0, 1, h_blend, dtype=np.float32)[np.newaxis, :]
alpha[:, :h_blend] *= h_gradient
# Apply blending # Apply blending
alpha_3ch = alpha[:, :, np.newaxis] alpha_3ch = alpha[:, :, np.newaxis]
mosaic_region = result[y_offset:y_end, x_offset:x_end].astype(np.float32) mosaic_region = result[y_offset:y_end, x_offset:x_end].astype(np.float32)
frame_region = frame[:region_h, :region_w].astype(np.float32) frame_region = frame[:frame_y_end, :frame_x_end].astype(np.float32)
blended = (mosaic_region * (1 - alpha_3ch) + frame_region * alpha_3ch).astype(np.uint8) blended = (mosaic_region * (1 - alpha_3ch) + frame_region * alpha_3ch).astype(np.uint8)
result[y_offset:y_end, x_offset:x_end] = blended result[y_offset:y_end, x_offset:x_end] = blended
self.mosaic = result self.mosaic = result
# Update position tracking for subsequent strips # Update position tracking OUTSIDE the mosaic lock
with self._state_lock: with self._state_lock:
self.state.current_x = x_offset # Track where we are in the mosaic if direction == ScanDirection.LEFT:
self.state.current_y = y_offset # For LEFT scanning: current_x is LEFT edge of where we are
self.state.append_count += 1 # Start at right side, will decrease as we move left
self.state.current_x = x_offset
else:
# For RIGHT scanning: current_x is RIGHT edge of mosaic
self.state.current_x = 0
self.log(f" First strip blended at ({x_offset}, {y_offset}), size {region_w}x{region_h}") self.state.current_y = y_offset
self.state.append_count += 1
self.log(f" First strip placed. current_x={self.state.current_x}, current_y={self.state.current_y}")
# Reset displacement tracking for subsequent strips
self._displacement_since_append_x = 0.0
self._displacement_since_append_y = 0.0
self._prev_frame = frame.copy()
if self.on_mosaic_updated: if self.on_mosaic_updated:
self.on_mosaic_updated() self.on_mosaic_updated()
def _append_strip(self, frame: np.ndarray, direction: ScanDirection): def _append_strip(self, frame: np.ndarray, direction: ScanDirection):
"""Append strip to mosaic based on accumulated displacement with continuous alignment.""" """Append strip to mosaic based on accumulated displacement."""
BLEND_WIDTH = 10 BLEND_WIDTH = 10
SAFETY_MARGIN = 2 SAFETY_MARGIN = 2
@ -796,23 +815,6 @@ class StitchingScanner:
dx = abs(self._displacement_since_append_x) dx = abs(self._displacement_since_append_x)
dy = abs(self._displacement_since_append_y) dy = abs(self._displacement_since_append_y)
# Calculate expected position for alignment detection
expected_x = int(self.state.current_x + self._cumulative_align_x)
expected_y = int(self.state.current_y + self._cumulative_align_y)
# Detect alignment for this strip
alignment = self._detect_strip_alignment(frame, direction, expected_x, expected_y)
if alignment.valid:
# Update cumulative alignment
self._cumulative_align_x += alignment.x_offset
self._cumulative_align_y += alignment.y_offset
self._last_strip_alignment = alignment
# Get total alignment offsets
align_x = self._cumulative_align_x
align_y = self._cumulative_align_y
if direction in [ScanDirection.RIGHT, ScanDirection.LEFT]: if direction in [ScanDirection.RIGHT, ScanDirection.LEFT]:
append_width = round(dx) + SAFETY_MARGIN append_width = round(dx) + SAFETY_MARGIN
append_width = min(append_width, w - BLEND_WIDTH - 5) append_width = min(append_width, w - BLEND_WIDTH - 5)
@ -823,54 +825,72 @@ class StitchingScanner:
pixels_consumed = append_width - SAFETY_MARGIN pixels_consumed = append_width - SAFETY_MARGIN
fractional_remainder = dx - pixels_consumed fractional_remainder = dx - pixels_consumed
# Calculate Y offset for current row
y_offset = int(self.state.current_y) y_offset = int(self.state.current_y)
y_offset = max(0, min(y_offset, mh - h))
if direction == ScanDirection.RIGHT: if direction == ScanDirection.RIGHT:
# Expanding to the right
strip_start = max(0, w - append_width - BLEND_WIDTH) strip_start = max(0, w - append_width - BLEND_WIDTH)
new_strip = frame[:, strip_start:] new_strip = frame[:, strip_start:]
self.log(f"RIGHT append: strip from col {strip_start}, width {new_strip.shape[1]}")
self.mosaic = self._blend_horizontal_at_y( self.mosaic = self._blend_horizontal_at_y(
self.mosaic, new_strip, BLEND_WIDTH, append_right=True, self.mosaic, new_strip, BLEND_WIDTH, append_right=True,
x_offset=int(self.state.current_x), y_offset=y_offset, y_offset=y_offset)
alignment_x=align_x, alignment_y=align_y)
else: else: # LEFT - placing within existing mosaic
# current_x is where the LEFT edge of current view is
# We're moving left, so new content is on the LEFT of the frame
# We want to place the LEFT portion of the frame
strip_end = min(w, append_width + BLEND_WIDTH) strip_end = min(w, append_width + BLEND_WIDTH)
new_strip = frame[:, :strip_end] new_strip = frame[:, :strip_end]
self.mosaic = self._blend_horizontal_at_y(
self.mosaic, new_strip, BLEND_WIDTH, append_right=False, # Calculate where to place this strip
x_offset=int(self.state.current_x), y_offset=y_offset, # current_x is decreasing as we move left
alignment_x=align_x, alignment_y=align_y) # The strip goes at current_x - append_width
new_x = int(self.state.current_x) - append_width
new_x = max(0, new_x)
self.log(f"LEFT append: current_x={self.state.current_x}, new_x={new_x}, strip width={new_strip.shape[1]}")
# Blend into existing mosaic
result = self.mosaic.copy()
strip_h, strip_w = new_strip.shape[:2]
x_end = min(new_x + strip_w, mw)
y_end = min(y_offset + strip_h, mh)
actual_w = x_end - new_x
actual_h = y_end - y_offset
if actual_w > BLEND_WIDTH and actual_h > 0:
# Create horizontal blend on RIGHT side (blending with existing content)
alpha = np.ones((actual_h, actual_w), dtype=np.float32)
blend_w = min(BLEND_WIDTH, actual_w // 2)
if blend_w > 0:
h_gradient = np.linspace(1, 0, blend_w, dtype=np.float32)[np.newaxis, :]
alpha[:, -blend_w:] = h_gradient
alpha_3ch = alpha[:, :, np.newaxis]
mosaic_region = result[y_offset:y_end, new_x:x_end].astype(np.float32)
frame_region = new_strip[:actual_h, :actual_w].astype(np.float32)
blended = (mosaic_region * (1 - alpha_3ch) + frame_region * alpha_3ch).astype(np.uint8)
result[y_offset:y_end, new_x:x_end] = blended
self.mosaic = result
# Update current_x to new position (moving left)
with self._state_lock:
self.state.current_x = new_x
self._displacement_since_append_x = fractional_remainder self._displacement_since_append_x = fractional_remainder
self._displacement_since_append_y = 0.0 self._displacement_since_append_y = 0.0
elif direction in [ScanDirection.DOWN, ScanDirection.UP]: elif direction in [ScanDirection.DOWN, ScanDirection.UP]:
append_height = round(dy) + SAFETY_MARGIN # ... keep existing vertical logic ...
append_height = min(append_height, h - BLEND_WIDTH - 5) pass
if append_height < 1:
return
pixels_consumed = append_height - SAFETY_MARGIN
fractional_remainder = dy - pixels_consumed
if direction == ScanDirection.DOWN:
strip_end = min(h, append_height + BLEND_WIDTH)
new_strip = frame[:strip_end, :]
self.mosaic = self._blend_vertical_at_x(
self.mosaic, new_strip, BLEND_WIDTH, append_below=False,
x_off=int(self.state.current_x),
alignment_x=align_x, alignment_y=align_y)
else:
strip_start = max(0, h - append_height - BLEND_WIDTH)
new_strip = frame[strip_start:, :]
self.mosaic = self._blend_vertical_at_x(
self.mosaic, new_strip, BLEND_WIDTH, append_below=True,
x_off=int(self.state.current_x),
alignment_x=align_x, alignment_y=align_y)
self._displacement_since_append_x = 0.0
self._displacement_since_append_y = fractional_remainder
new_mh, new_mw = self.mosaic.shape[:2] new_mh, new_mw = self.mosaic.shape[:2]
@ -881,7 +901,6 @@ class StitchingScanner:
if self.on_mosaic_updated: if self.on_mosaic_updated:
self.on_mosaic_updated() self.on_mosaic_updated()
# ========================================================================= # =========================================================================
# Scan Control # Scan Control
# ========================================================================= # =========================================================================
@ -1013,29 +1032,26 @@ class StitchingScanner:
frame = self._capture_frame() frame = self._capture_frame()
h, w = frame.shape[:2] h, w = frame.shape[:2]
total_x = 0.0 # Track total movement in this direction
# Setup based on direction # Setup based on direction
if direction in [ScanDirection.RIGHT, ScanDirection.LEFT]: if direction in [ScanDirection.RIGHT, ScanDirection.LEFT]:
threshold_pixels = w * self.config.displacement_threshold threshold_pixels = w * self.config.displacement_threshold
max_dim = self.config.max_mosaic_width
current_dim = lambda: self.state.mosaic_width
start_cmd = 'E' if direction == ScanDirection.RIGHT else 'W' start_cmd = 'E' if direction == ScanDirection.RIGHT else 'W'
stop_cmd = 'e' if direction == ScanDirection.RIGHT else 'w' stop_cmd = 'e' if direction == ScanDirection.RIGHT else 'w'
else: else:
threshold_pixels = h * self.config.displacement_threshold threshold_pixels = h * self.config.displacement_threshold
max_dim = self.config.max_mosaic_height
current_dim = lambda: self.state.mosaic_height
start_cmd = 'S' if direction == ScanDirection.DOWN else 'N' start_cmd = 'S' if direction == ScanDirection.DOWN else 'N'
stop_cmd = 's' if direction == ScanDirection.DOWN else 'n' stop_cmd = 's' if direction == ScanDirection.DOWN else 'n'
# For LEFT direction, we need to track how far we've traveled # Track starting position and target for LEFT direction
# We stop when we've traveled approximately the mosaic width
if direction == ScanDirection.LEFT: if direction == ScanDirection.LEFT:
# Calculate target: we need to travel back across the mosaic start_x = self.state.current_x
# Starting from right edge, ending at left edge target_x = 0 # We want to reach the left edge
target_travel = self.state.mosaic_width - w # Approximate distance to travel self.log(f"LEFT scan: starting at x={start_x}, target x={target_x}")
self.log(f"LEFT scan: target travel distance = {target_travel:.0f}px") elif direction == ScanDirection.RIGHT:
start_x = self.state.current_x
target_x = self.config.max_mosaic_width
self.log(f"RIGHT scan: starting at x={start_x}, target x={target_x}")
self._prev_frame = frame.copy() self._prev_frame = frame.copy()
self._displacement_since_append_x = 0.0 self._displacement_since_append_x = 0.0
@ -1052,22 +1068,17 @@ class StitchingScanner:
stop_reason = 'timeout' stop_reason = 'timeout'
break break
# Check exit conditions based on direction # Check exit conditions
if direction == ScanDirection.RIGHT: if direction == ScanDirection.RIGHT:
if current_dim() >= max_dim: if self.state.mosaic_width >= self.config.max_mosaic_width:
self.log(f"Max dimension reached ({current_dim()}px)") self.log(f"Max width reached ({self.state.mosaic_width}px)")
stop_reason = 'max_dim'
break
if abs(self.state.current_x) >= max_dim:
self.log(f"Current X reached max ({self.state.current_x}px)")
stop_reason = 'max_dim' stop_reason = 'max_dim'
break break
elif direction == ScanDirection.LEFT: elif direction == ScanDirection.LEFT:
# Check if we've traveled far enough (back to left edge) # Stop when we reach the left edge
# total_x will be negative for leftward movement if self.state.current_x <= 0:
if abs(total_x) >= target_travel: self.log(f"Reached left edge (current_x={self.state.current_x})")
self.log(f"Returned to left edge: traveled {abs(total_x):.0f}px of {target_travel:.0f}px")
stop_reason = 'complete' stop_reason = 'complete'
break break
@ -1081,18 +1092,27 @@ class StitchingScanner:
curr_frame = self._capture_frame() curr_frame = self._capture_frame()
dx, dy = self._detect_displacement_robust(self._prev_frame, curr_frame) dx, dy = self._detect_displacement_robust(self._prev_frame, curr_frame)
self._displacement_since_append_x += dx # Accumulate displacement magnitude
self._displacement_since_append_x += abs(dx)
self._displacement_since_append_y += dy self._displacement_since_append_y += dy
total_x += dx
# For LEFT direction, current_x DECREASES
# Phase correlation: when camera moves LEFT, content shifts RIGHT, dx > 0
# So for LEFT scanning, we subtract dx from current_x
if direction == ScanDirection.LEFT:
with self._state_lock:
self.state.current_x -= abs(dx) # Decrease as we go left
elif direction == ScanDirection.RIGHT:
with self._state_lock:
self.state.current_x += abs(dx) # Increase as we go right
with self._state_lock: with self._state_lock:
self.state.current_x += dx
self.state.cumulative_x = self._displacement_since_append_x self.state.cumulative_x = self._displacement_since_append_x
self.state.cumulative_y = self._displacement_since_append_y self.state.cumulative_y = self._displacement_since_append_y
self.state.last_displacement = (dx, dy) self.state.last_displacement = (dx, dy)
self.state.frame_count += 1 self.state.frame_count += 1
# Edge detection (no movement) # Edge detection
movement = abs(dx) if direction in [ScanDirection.RIGHT, ScanDirection.LEFT] else abs(dy) movement = abs(dx) if direction in [ScanDirection.RIGHT, ScanDirection.LEFT] else abs(dy)
if movement < 1.0: if movement < 1.0:
@ -1105,11 +1125,11 @@ class StitchingScanner:
no_movement_count = 0 no_movement_count = 0
# Append when threshold reached # Append when threshold reached
disp = abs(self._displacement_since_append_x) if direction in [ScanDirection.RIGHT, ScanDirection.LEFT] else abs(self._displacement_since_append_y) disp = self._displacement_since_append_x if direction in [ScanDirection.RIGHT, ScanDirection.LEFT] else abs(self._displacement_since_append_y)
if disp >= threshold_pixels: if disp >= threshold_pixels:
self._append_strip(curr_frame, direction) self._append_strip(curr_frame, direction)
self.log(f"Appended at total_x={total_x:.1f}, mosaic: {self.state.mosaic_width}x{self.state.mosaic_height}") self.log(f"Appended, current_x={self.state.current_x:.0f}, mosaic: {self.state.mosaic_width}x{self.state.mosaic_height}")
self._prev_frame = curr_frame.copy() self._prev_frame = curr_frame.copy()
@ -1118,7 +1138,7 @@ class StitchingScanner:
self.motion.send_command(stop_cmd) self.motion.send_command(stop_cmd)
time.sleep(self.config.settle_time) time.sleep(self.config.settle_time)
self.log(f"Direction finished: {stop_reason}, total movement: {total_x:.1f}px") self.log(f"Direction finished: {stop_reason}, final current_x={self.state.current_x}")
return stop_reason return stop_reason
def _move_to_next_row(self) -> bool: def _move_to_next_row(self) -> bool: