testing pos tracking
This commit is contained in:
parent
8ebe7a6e97
commit
34f84d760e
1 changed files with 98 additions and 137 deletions
|
|
@ -380,8 +380,8 @@ class StitchingScanner:
|
|||
y_offset = y_offset - int(round(alignment_y))
|
||||
|
||||
# Clamp x_offset to valid range
|
||||
# x_offset = max(0, min(x_offset, w_base - blend_w))
|
||||
x_offset = 0 - min(x_offset, w_base)
|
||||
x_offset = max(0, min(x_offset, w_base - blend_w))
|
||||
# x_offset = 0 - min(x_offset, w_base)
|
||||
|
||||
# Handle strip cropping if y_offset is negative (strip protrudes above frame)
|
||||
strip_y_start = 0 # How much to crop from top of strip
|
||||
|
|
@ -680,12 +680,12 @@ class StitchingScanner:
|
|||
|
||||
def _append_first_strip_of_row(self, frame: np.ndarray, direction: ScanDirection, alignment: AlignmentOffset):
|
||||
"""
|
||||
Append the first strip of a new row with special handling for large overlap.
|
||||
Append the first strip of a new row with proper positioning.
|
||||
|
||||
This uses the alignment detected by _detect_row_start_alignment to properly
|
||||
position the first strip, which sets the baseline for the entire row.
|
||||
For LEFT direction: Frame overlaps with bottom-right of mosaic
|
||||
For RIGHT direction: Frame overlaps with bottom-left of mosaic
|
||||
"""
|
||||
BLEND_WIDTH = 10
|
||||
BLEND_WIDTH = 20
|
||||
|
||||
with self._mosaic_lock:
|
||||
if self.mosaic is None:
|
||||
|
|
@ -701,127 +701,83 @@ class StitchingScanner:
|
|||
self._last_strip_alignment = alignment
|
||||
self.log(f"Applied first-strip alignment: X={alignment.x_offset:.1f}, Y={alignment.y_offset:.1f}")
|
||||
|
||||
align_x = self._cumulative_align_x
|
||||
align_y = self._cumulative_align_y
|
||||
|
||||
# Calculate Y position - we're at the bottom of existing mosaic
|
||||
# The frame overlaps with the last ~row_overlap portion of existing content
|
||||
# Calculate Y position - frame overlaps with bottom of mosaic
|
||||
row_overlap_pixels = int(fh * self.config.row_overlap)
|
||||
y_offset = mh - row_overlap_pixels
|
||||
|
||||
# Apply Y alignment correction
|
||||
y_offset = y_offset + int(round(align_y))
|
||||
y_offset = mh - row_overlap_pixels + int(round(self._cumulative_align_y))
|
||||
y_offset = max(0, y_offset)
|
||||
|
||||
if direction == ScanDirection.LEFT:
|
||||
# Starting at right edge - place frame aligned with right side of mosaic
|
||||
# The frame's right edge should align with mosaic's right edge (with X correction)
|
||||
x_offset = mw - fw + int(round(align_x))
|
||||
x_offset = max(0, min(x_offset, mw - BLEND_WIDTH))
|
||||
# Frame's RIGHT edge aligns with mosaic's RIGHT edge
|
||||
x_offset = mw - fw + int(round(self._cumulative_align_x))
|
||||
x_offset = max(0, x_offset)
|
||||
|
||||
self.log(f"=== First Strip of Row (LEFT) ===")
|
||||
self.log(f" Mosaic: {mw}x{mh}, Frame: {fw}x{fh}")
|
||||
self.log(f" Placement: X={x_offset}, Y={y_offset}")
|
||||
self.log(f" Alignment: X={align_x:.1f}, Y={align_y:.1f}")
|
||||
self.log(f" X offset: {x_offset} (frame right edge at {x_offset + fw})")
|
||||
self.log(f" Y offset: {y_offset}")
|
||||
|
||||
# Blend this frame into the mosaic at the calculated position
|
||||
# We're placing within existing bounds, so just blend in place
|
||||
|
||||
# Calculate what portion of the frame to use
|
||||
# Most of it overlaps, we just need to blend it properly
|
||||
blend_region_width = min(fw, mw - x_offset)
|
||||
|
||||
# Create blended result
|
||||
result = self.mosaic.copy()
|
||||
|
||||
# Blend the overlapping region
|
||||
frame_to_place = frame[:, :blend_region_width]
|
||||
|
||||
# Vertical blend at top edge (blending with row above)
|
||||
v_blend_h = min(row_overlap_pixels, BLEND_WIDTH * 2)
|
||||
if v_blend_h > 0 and y_offset > 0:
|
||||
alpha_v = np.linspace(0, 1, v_blend_h, dtype=np.float32)[:, np.newaxis, np.newaxis]
|
||||
|
||||
blend_y_start = y_offset
|
||||
blend_y_end = min(y_offset + v_blend_h, mh)
|
||||
actual_blend_h = blend_y_end - blend_y_start
|
||||
|
||||
if actual_blend_h > 0:
|
||||
mosaic_overlap = result[blend_y_start:blend_y_end, x_offset:x_offset + blend_region_width].astype(np.float32)
|
||||
frame_overlap = frame_to_place[:actual_blend_h, :].astype(np.float32)
|
||||
|
||||
# Resize alpha if needed
|
||||
alpha_v_actual = np.linspace(0, 1, actual_blend_h, dtype=np.float32)[:, np.newaxis, np.newaxis]
|
||||
|
||||
min_w_blend = min(mosaic_overlap.shape[1], frame_overlap.shape[1])
|
||||
mosaic_overlap = mosaic_overlap[:, :min_w_blend]
|
||||
frame_overlap = frame_overlap[:, :min_w_blend]
|
||||
|
||||
blended = (mosaic_overlap * (1 - alpha_v_actual) + frame_overlap * alpha_v_actual).astype(np.uint8)
|
||||
result[blend_y_start:blend_y_end, x_offset:x_offset + min_w_blend] = blended
|
||||
|
||||
# Place rest of frame below blend zone
|
||||
if y_offset + v_blend_h < mh:
|
||||
remaining_h = min(fh - v_blend_h, mh - (y_offset + v_blend_h))
|
||||
if remaining_h > 0:
|
||||
result[y_offset + v_blend_h:y_offset + v_blend_h + remaining_h,
|
||||
x_offset:x_offset + min_w_blend] = frame_to_place[v_blend_h:v_blend_h + remaining_h, :min_w_blend]
|
||||
|
||||
self.mosaic = result
|
||||
|
||||
else: # RIGHT direction
|
||||
# Starting at left edge - place frame aligned with left side of mosaic
|
||||
x_offset = int(round(align_x))
|
||||
else: # RIGHT
|
||||
# Frame's LEFT edge aligns with mosaic's LEFT edge
|
||||
x_offset = int(round(self._cumulative_align_x))
|
||||
x_offset = max(0, x_offset)
|
||||
|
||||
self.log(f"=== First Strip of Row (RIGHT) ===")
|
||||
self.log(f" Mosaic: {mw}x{mh}, Frame: {fw}x{fh}")
|
||||
self.log(f" Placement: X={x_offset}, Y={y_offset}")
|
||||
self.log(f" Alignment: X={align_x:.1f}, Y={align_y:.1f}")
|
||||
|
||||
# Similar blending logic for right direction
|
||||
result = self.mosaic.copy()
|
||||
blend_region_width = min(fw, mw - x_offset)
|
||||
frame_to_place = frame[:, :blend_region_width]
|
||||
|
||||
# Vertical blend at top edge
|
||||
v_blend_h = min(row_overlap_pixels, BLEND_WIDTH * 2)
|
||||
if v_blend_h > 0 and y_offset > 0:
|
||||
blend_y_start = y_offset
|
||||
blend_y_end = min(y_offset + v_blend_h, mh)
|
||||
actual_blend_h = blend_y_end - blend_y_start
|
||||
|
||||
if actual_blend_h > 0:
|
||||
mosaic_overlap = result[blend_y_start:blend_y_end, x_offset:x_offset + blend_region_width].astype(np.float32)
|
||||
frame_overlap = frame_to_place[:actual_blend_h, :].astype(np.float32)
|
||||
|
||||
alpha_v_actual = np.linspace(0, 1, actual_blend_h, dtype=np.float32)[:, np.newaxis, np.newaxis]
|
||||
|
||||
min_w_blend = min(mosaic_overlap.shape[1], frame_overlap.shape[1])
|
||||
mosaic_overlap = mosaic_overlap[:, :min_w_blend]
|
||||
frame_overlap = frame_overlap[:, :min_w_blend]
|
||||
|
||||
blended = (mosaic_overlap * (1 - alpha_v_actual) + frame_overlap * alpha_v_actual).astype(np.uint8)
|
||||
result[blend_y_start:blend_y_end, x_offset:x_offset + min_w_blend] = blended
|
||||
|
||||
if y_offset + v_blend_h < mh:
|
||||
remaining_h = min(fh - v_blend_h, mh - (y_offset + v_blend_h))
|
||||
if remaining_h > 0:
|
||||
result[y_offset + v_blend_h:y_offset + v_blend_h + remaining_h,
|
||||
x_offset:x_offset + min_w_blend] = frame_to_place[v_blend_h:v_blend_h + remaining_h, :min_w_blend]
|
||||
|
||||
self.mosaic = result
|
||||
self.log(f" X offset: {x_offset}")
|
||||
self.log(f" Y offset: {y_offset}")
|
||||
|
||||
# Update current position for subsequent strips
|
||||
with self._state_lock:
|
||||
# Blend frame into mosaic at calculated position
|
||||
result = self.mosaic.copy()
|
||||
|
||||
# Calculate the region to blend
|
||||
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
|
||||
|
||||
if region_w <= 0 or region_h <= 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)
|
||||
|
||||
# Vertical blend at top (first ~20% of overlap region)
|
||||
v_blend = min(row_overlap_pixels, int(region_h * 0.3))
|
||||
if v_blend > 0:
|
||||
v_gradient = np.linspace(0, 1, v_blend, dtype=np.float32)[:, np.newaxis]
|
||||
alpha[:v_blend, :] = v_gradient
|
||||
|
||||
# Horizontal blend at edge
|
||||
h_blend = min(BLEND_WIDTH, int(region_w * 0.2))
|
||||
if h_blend > 0:
|
||||
if direction == ScanDirection.LEFT:
|
||||
self.state.current_x = x_offset
|
||||
# Blend on right edge (where we came from)
|
||||
h_gradient = np.linspace(1, 0, h_blend, dtype=np.float32)[np.newaxis, :]
|
||||
alpha[:, -h_blend:] = np.minimum(alpha[:, -h_blend:], h_gradient)
|
||||
else:
|
||||
self.state.current_x = x_offset + blend_region_width
|
||||
# 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)
|
||||
|
||||
# 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)
|
||||
|
||||
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
|
||||
|
||||
self.log(f" First strip placed. Position: ({self.state.current_x}, {self.state.current_y})")
|
||||
self.log(f" First strip blended at ({x_offset}, {y_offset}), size {region_w}x{region_h}")
|
||||
|
||||
if self.on_mosaic_updated:
|
||||
self.on_mosaic_updated()
|
||||
|
|
@ -1057,7 +1013,7 @@ class StitchingScanner:
|
|||
|
||||
frame = self._capture_frame()
|
||||
h, w = frame.shape[:2]
|
||||
total_x = 0
|
||||
total_x = 0.0 # Track total movement in this direction
|
||||
|
||||
# Setup based on direction
|
||||
if direction in [ScanDirection.RIGHT, ScanDirection.LEFT]:
|
||||
|
|
@ -1073,6 +1029,14 @@ class StitchingScanner:
|
|||
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
|
||||
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")
|
||||
|
||||
self._prev_frame = frame.copy()
|
||||
self._displacement_since_append_x = 0.0
|
||||
self._displacement_since_append_y = 0.0
|
||||
|
|
@ -1081,29 +1045,31 @@ class StitchingScanner:
|
|||
no_movement_count = 0
|
||||
max_no_movement = 50
|
||||
stop_reason = 'stopped'
|
||||
self.log(f"Scanning 2..")
|
||||
|
||||
while self.running and not self.paused:
|
||||
if time.time() - start_time > self.config.max_scan_time:
|
||||
self.log("Scan timeout")
|
||||
stop_reason = 'timeout'
|
||||
break
|
||||
|
||||
if current_dim() >= max_dim and direction == ScanDirection.RIGHT:
|
||||
self.log(f"Max dimension reached ({current_dim()}px)")
|
||||
stop_reason = 'max_dim'
|
||||
break
|
||||
|
||||
if self.state.current_x >= 0 and direction == ScanDirection.LEFT:
|
||||
self.log(f"Returned to start ({self.config.max_mosaic_width}px)")
|
||||
self.log(f"Current X offset ({self.state.current_x}px) total_x ({total_x}px)")
|
||||
stop_reason = 'max_dim'
|
||||
break
|
||||
|
||||
if abs(self.state.current_x) >= self.config.max_mosaic_width and direction == ScanDirection.RIGHT:
|
||||
self.log(f"Max dimension reached ({self.config.max_mosaic_width}px)")
|
||||
self.log(f"Current X offset ({self.state.current_x}px)")
|
||||
stop_reason = 'max_dim'
|
||||
break
|
||||
# Check exit conditions based on direction
|
||||
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)")
|
||||
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_reason = 'complete'
|
||||
break
|
||||
|
||||
# Pulse motor
|
||||
self.motion.send_command(start_cmd)
|
||||
|
|
@ -1115,23 +1081,20 @@ class StitchingScanner:
|
|||
curr_frame = self._capture_frame()
|
||||
dx, dy = self._detect_displacement_robust(self._prev_frame, curr_frame)
|
||||
|
||||
self.log(f"Scanning dx{dx} dy{dy}..")
|
||||
self._displacement_since_append_x += dx
|
||||
self._displacement_since_append_y += dy
|
||||
total_x += dx
|
||||
with self._state_lock:
|
||||
self.state.current_x += dx
|
||||
|
||||
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
|
||||
# Edge detection (no movement)
|
||||
movement = abs(dx) if direction in [ScanDirection.RIGHT, ScanDirection.LEFT] else abs(dy)
|
||||
|
||||
self.log(f"Scanning movement{movement}..")
|
||||
if movement < 1.0:
|
||||
no_movement_count += 1
|
||||
if no_movement_count >= max_no_movement:
|
||||
|
|
@ -1141,14 +1104,12 @@ class StitchingScanner:
|
|||
else:
|
||||
no_movement_count = 0
|
||||
|
||||
# Append when threshold reached (with continuous alignment)
|
||||
# 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)
|
||||
|
||||
self.log(f"Scanning disp{disp}..")
|
||||
if disp >= threshold_pixels:
|
||||
self.log(f"Scanning threshold_pixels..")
|
||||
self._append_strip(curr_frame, direction)
|
||||
self.log(f"Appended {disp:.1f}px, mosaic: {self.state.mosaic_width}x{self.state.mosaic_height}, align: ({self._cumulative_align_x:.1f}, {self._cumulative_align_y:.1f})")
|
||||
self.log(f"Appended at total_x={total_x:.1f}, mosaic: {self.state.mosaic_width}x{self.state.mosaic_height}")
|
||||
|
||||
self._prev_frame = curr_frame.copy()
|
||||
|
||||
|
|
@ -1157,7 +1118,7 @@ class StitchingScanner:
|
|||
|
||||
self.motion.send_command(stop_cmd)
|
||||
time.sleep(self.config.settle_time)
|
||||
self.log(f"Direction finished: {stop_reason}")
|
||||
self.log(f"Direction finished: {stop_reason}, total movement: {total_x:.1f}px")
|
||||
return stop_reason
|
||||
|
||||
def _move_to_next_row(self) -> bool:
|
||||
|
|
|
|||
Loading…
Reference in a new issue