testing pos tracking
This commit is contained in:
parent
34f84d760e
commit
c31220532b
1 changed files with 143 additions and 123 deletions
|
|
@ -694,95 +694,114 @@ class StitchingScanner:
|
|||
mh, mw = self.mosaic.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
|
||||
if alignment.valid:
|
||||
self._cumulative_align_x += alignment.x_offset
|
||||
self._cumulative_align_y += alignment.y_offset
|
||||
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
|
||||
row_overlap_pixels = int(fh * self.config.row_overlap)
|
||||
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:
|
||||
# Starting at RIGHT edge, going LEFT
|
||||
# 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 = max(0, x_offset)
|
||||
x_offset = max(0, min(x_offset, mw - fw))
|
||||
|
||||
self.log(f"=== First Strip of Row (LEFT) ===")
|
||||
self.log(f" Mosaic: {mw}x{mh}, Frame: {fw}x{fh}")
|
||||
self.log(f" X offset: {x_offset} (frame right edge at {x_offset + fw})")
|
||||
self.log(f" Y offset: {y_offset}")
|
||||
# For LEFT scanning, current_x tracks where LEFT edge of current frame is
|
||||
# This will DECREASE as we scan left
|
||||
start_x_for_scanning = x_offset
|
||||
|
||||
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 = max(0, x_offset)
|
||||
start_x_for_scanning = 0
|
||||
|
||||
self.log(f"=== First Strip of Row (RIGHT) ===")
|
||||
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}")
|
||||
self.log(f" Calculated x_offset: {x_offset}, y_offset: {y_offset}")
|
||||
|
||||
# Blend frame into mosaic at calculated position
|
||||
# Simply overwrite with blending - no expansion needed
|
||||
result = self.mosaic.copy()
|
||||
|
||||
# Calculate the region to blend
|
||||
# Calculate valid region
|
||||
x_end = min(x_offset + fw, mw)
|
||||
y_end = min(y_offset + fh, mh)
|
||||
region_w = x_end - x_offset
|
||||
region_h = y_end - y_offset
|
||||
frame_x_end = x_end - x_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")
|
||||
return
|
||||
|
||||
# Create alpha mask for blending
|
||||
# Blend at top edge (with row above) and appropriate side edge
|
||||
alpha = np.ones((region_h, region_w), dtype=np.float32)
|
||||
self.log(f" Blending region: mosaic[{y_offset}:{y_end}, {x_offset}:{x_end}]")
|
||||
self.log(f" Frame region: frame[0:{frame_y_end}, 0:{frame_x_end}]")
|
||||
|
||||
# Vertical blend at top (first ~20% of overlap region)
|
||||
v_blend = min(row_overlap_pixels, int(region_h * 0.3))
|
||||
if v_blend > 0:
|
||||
# Create alpha mask for smooth blending
|
||||
alpha = np.ones((frame_y_end, frame_x_end), dtype=np.float32)
|
||||
|
||||
# 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]
|
||||
alpha[:v_blend, :] = v_gradient
|
||||
alpha[:v_blend, :] *= v_gradient
|
||||
|
||||
# Horizontal blend at edge
|
||||
h_blend = min(BLEND_WIDTH, int(region_w * 0.2))
|
||||
if h_blend > 0:
|
||||
# Horizontal blend at the edge we came from
|
||||
h_blend = min(BLEND_WIDTH, frame_x_end // 4)
|
||||
if h_blend > 5:
|
||||
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, :]
|
||||
alpha[:, -h_blend:] = np.minimum(alpha[:, -h_blend:], h_gradient)
|
||||
alpha[:, -h_blend:] *= h_gradient
|
||||
else:
|
||||
# Blend on left edge
|
||||
h_gradient = np.linspace(0, 1, h_blend, dtype=np.float32)[np.newaxis, :]
|
||||
alpha[:, :h_blend] = np.minimum(alpha[:, :h_blend], h_gradient)
|
||||
# Came from left (or starting), blend left edge if not at edge
|
||||
if x_offset > 0:
|
||||
h_gradient = np.linspace(0, 1, h_blend, dtype=np.float32)[np.newaxis, :]
|
||||
alpha[:, :h_blend] *= h_gradient
|
||||
|
||||
# Apply blending
|
||||
alpha_3ch = alpha[:, :, np.newaxis]
|
||||
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)
|
||||
result[y_offset:y_end, x_offset:x_end] = blended
|
||||
|
||||
self.mosaic = result
|
||||
|
||||
# Update position tracking for subsequent strips
|
||||
with self._state_lock:
|
||||
self.state.current_x = x_offset # Track where we are in the mosaic
|
||||
self.state.current_y = y_offset
|
||||
self.state.append_count += 1
|
||||
# Update position tracking OUTSIDE the mosaic lock
|
||||
with self._state_lock:
|
||||
if direction == ScanDirection.LEFT:
|
||||
# For LEFT scanning: current_x is LEFT edge of where we are
|
||||
# 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:
|
||||
self.on_mosaic_updated()
|
||||
|
||||
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
|
||||
SAFETY_MARGIN = 2
|
||||
|
||||
|
|
@ -796,23 +815,6 @@ class StitchingScanner:
|
|||
dx = abs(self._displacement_since_append_x)
|
||||
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]:
|
||||
append_width = round(dx) + SAFETY_MARGIN
|
||||
append_width = min(append_width, w - BLEND_WIDTH - 5)
|
||||
|
|
@ -823,54 +825,72 @@ class StitchingScanner:
|
|||
pixels_consumed = append_width - SAFETY_MARGIN
|
||||
fractional_remainder = dx - pixels_consumed
|
||||
|
||||
# Calculate Y offset for current row
|
||||
y_offset = int(self.state.current_y)
|
||||
y_offset = max(0, min(y_offset, mh - h))
|
||||
|
||||
if direction == ScanDirection.RIGHT:
|
||||
# Expanding to the right
|
||||
strip_start = max(0, w - append_width - BLEND_WIDTH)
|
||||
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, new_strip, BLEND_WIDTH, append_right=True,
|
||||
x_offset=int(self.state.current_x), y_offset=y_offset,
|
||||
alignment_x=align_x, alignment_y=align_y)
|
||||
else:
|
||||
y_offset=y_offset)
|
||||
|
||||
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)
|
||||
new_strip = frame[:, :strip_end]
|
||||
self.mosaic = self._blend_horizontal_at_y(
|
||||
self.mosaic, new_strip, BLEND_WIDTH, append_right=False,
|
||||
x_offset=int(self.state.current_x), y_offset=y_offset,
|
||||
alignment_x=align_x, alignment_y=align_y)
|
||||
|
||||
# Calculate where to place this strip
|
||||
# current_x is decreasing as we move left
|
||||
# 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_y = 0.0
|
||||
|
||||
elif direction in [ScanDirection.DOWN, ScanDirection.UP]:
|
||||
append_height = round(dy) + SAFETY_MARGIN
|
||||
append_height = min(append_height, h - BLEND_WIDTH - 5)
|
||||
|
||||
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
|
||||
# ... keep existing vertical logic ...
|
||||
pass
|
||||
|
||||
new_mh, new_mw = self.mosaic.shape[:2]
|
||||
|
||||
|
|
@ -881,7 +901,6 @@ class StitchingScanner:
|
|||
|
||||
if self.on_mosaic_updated:
|
||||
self.on_mosaic_updated()
|
||||
|
||||
# =========================================================================
|
||||
# Scan Control
|
||||
# =========================================================================
|
||||
|
|
@ -1013,29 +1032,26 @@ class StitchingScanner:
|
|||
|
||||
frame = self._capture_frame()
|
||||
h, w = frame.shape[:2]
|
||||
total_x = 0.0 # Track total movement in this direction
|
||||
|
||||
# Setup based on direction
|
||||
if direction in [ScanDirection.RIGHT, ScanDirection.LEFT]:
|
||||
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'
|
||||
stop_cmd = 'e' if direction == ScanDirection.RIGHT else 'w'
|
||||
else:
|
||||
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'
|
||||
stop_cmd = 's' if direction == ScanDirection.DOWN else 'n'
|
||||
|
||||
# For LEFT direction, we need to track how far we've traveled
|
||||
# We stop when we've traveled approximately the mosaic width
|
||||
# Track starting position and target for LEFT direction
|
||||
if direction == ScanDirection.LEFT:
|
||||
# Calculate target: we need to travel back across the mosaic
|
||||
# Starting from right edge, ending at left edge
|
||||
target_travel = self.state.mosaic_width - w # Approximate distance to travel
|
||||
self.log(f"LEFT scan: target travel distance = {target_travel:.0f}px")
|
||||
start_x = self.state.current_x
|
||||
target_x = 0 # We want to reach the left edge
|
||||
self.log(f"LEFT scan: starting at x={start_x}, target x={target_x}")
|
||||
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._displacement_since_append_x = 0.0
|
||||
|
|
@ -1052,22 +1068,17 @@ class StitchingScanner:
|
|||
stop_reason = 'timeout'
|
||||
break
|
||||
|
||||
# Check exit conditions based on direction
|
||||
# Check exit conditions
|
||||
if direction == ScanDirection.RIGHT:
|
||||
if current_dim() >= max_dim:
|
||||
self.log(f"Max dimension reached ({current_dim()}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)")
|
||||
if self.state.mosaic_width >= self.config.max_mosaic_width:
|
||||
self.log(f"Max width reached ({self.state.mosaic_width}px)")
|
||||
stop_reason = 'max_dim'
|
||||
break
|
||||
|
||||
elif direction == ScanDirection.LEFT:
|
||||
# Check if we've traveled far enough (back to left edge)
|
||||
# total_x will be negative for leftward movement
|
||||
if abs(total_x) >= target_travel:
|
||||
self.log(f"Returned to left edge: traveled {abs(total_x):.0f}px of {target_travel:.0f}px")
|
||||
# Stop when we reach the left edge
|
||||
if self.state.current_x <= 0:
|
||||
self.log(f"Reached left edge (current_x={self.state.current_x})")
|
||||
stop_reason = 'complete'
|
||||
break
|
||||
|
||||
|
|
@ -1081,18 +1092,27 @@ class StitchingScanner:
|
|||
curr_frame = self._capture_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
|
||||
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:
|
||||
self.state.current_x += dx
|
||||
self.state.cumulative_x = self._displacement_since_append_x
|
||||
self.state.cumulative_y = self._displacement_since_append_y
|
||||
self.state.last_displacement = (dx, dy)
|
||||
self.state.frame_count += 1
|
||||
|
||||
# Edge detection (no movement)
|
||||
# Edge detection
|
||||
movement = abs(dx) if direction in [ScanDirection.RIGHT, ScanDirection.LEFT] else abs(dy)
|
||||
|
||||
if movement < 1.0:
|
||||
|
|
@ -1105,11 +1125,11 @@ class StitchingScanner:
|
|||
no_movement_count = 0
|
||||
|
||||
# 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:
|
||||
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()
|
||||
|
||||
|
|
@ -1118,7 +1138,7 @@ class StitchingScanner:
|
|||
|
||||
self.motion.send_command(stop_cmd)
|
||||
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
|
||||
|
||||
def _move_to_next_row(self) -> bool:
|
||||
|
|
|
|||
Loading…
Reference in a new issue