displacement too large

src/stitching_scanner.py

@@ -106,6 +106,10 @@ class StitchingScanner:
         self._cumulative_align_x: float = 0.0
         self._cumulative_align_y: float = 0.0
         
+        # Track the Y position in the mosaic where the current row starts
+        # This is critical for placing strips at the correct vertical position
+        self._row_start_y: int = 0
+        
         # Last strip's alignment for continuity
         self._last_strip_alignment = AlignmentOffset()
         
@@ -179,18 +183,13 @@ class StitchingScanner:
     
     def _detect_row_start_alignment(self, frame: np.ndarray, direction: ScanDirection) -> AlignmentOffset:
         """
-        Detect alignment at the START of a new row by comparing against BOTH:
+        Detect alignment at the START of a new row.
-        1. The bottom edge of the mosaic (for Y alignment from vertical movement)
-        2. The appropriate side edge (for X alignment)
         
-        This is called after a row transition to properly position the first strip.
+        After a DOWN transition with prepend, the mosaic layout is:
+        - Y=0 to Y≈(transition_amount): NEW content from transition
+        - Y≈(transition_amount) to Y=mosaic_height: OLD row 1 content (shifted down)
         
-        Args:
+        The new frame should overlap visually with OLD row 1 content.
-            frame: Current camera frame at the start of the new row
-            direction: The horizontal scan direction for this row (LEFT or RIGHT)
-        
-        Returns:
-            AlignmentOffset with combined X/Y correction needed
         """
         offset = AlignmentOffset()
         
@@ -201,103 +200,129 @@ class StitchingScanner:
             mh, mw = self.mosaic.shape[:2]
             fh, fw = frame.shape[:2]
             
-            # Use larger overlap regions for better alignment at row start
+            # After prepending during row transition:
-            max_overlap = 300  # Increased from 200 for better detection
+            # Old row 1 content was originally at Y=0 to Y=fh in the old mosaic
-            min_overlap = 50   # Increased minimum for reliability
+            # Now it's shifted down by (mh - fh) pixels
+            # So old row 1 starts at approximately Y = mh - fh
+            old_row1_start = mh - fh
+            
+            self.log(f"  Row-start alignment: mosaic {mw}x{mh}, frame {fw}x{fh}")
+            self.log(f"  Old row 1 estimated start: Y={old_row1_start}")
+            
+            vertical_overlap = min(200, fh // 3)
+            min_overlap = 50
             
             # =============================================
-            # Step 1: Detect Y alignment from bottom edge
+            # Step 1: Detect Y alignment 
             # =============================================
-            vertical_overlap = min(fh // 2, max_overlap)
+            # Compare frame's bottom with the TOP of old row 1 content
+            # Frame's bottom: frame[fh-overlap:fh]
+            # Old row 1's top: mosaic[old_row1_start:old_row1_start+overlap]
             
-            if vertical_overlap >= min_overlap:
+            if old_row1_start >= 0 and old_row1_start + vertical_overlap <= mh:
-                # Get the position where we expect the frame to be
+                expected_x = 0 if direction == ScanDirection.RIGHT else max(0, mw - fw)
-                # After row transition, the frame should overlap with the bottom of mosaic
+                x_end = min(expected_x + fw, mw)
-                expected_x = int(self.state.current_x)
                 
-                # Clamp X position
+                # Frame's bottom portion (will overlap with old row 1's top)
-                if direction == ScanDirection.LEFT:
+                frame_bottom = frame[fh - vertical_overlap:fh, :x_end - expected_x]
-                    # For LEFT scanning, we're at the right edge of the mosaic
-                    expected_x = max(0, mw - fw)
-                else:
-                    # For RIGHT scanning, we're at the left edge
-                    expected_x = 0
                 
-                # Extract bottom of mosaic
+                # Old row 1's top portion in the mosaic
-                mosaic_bottom = self.mosaic[mh - vertical_overlap:mh, 
+                mosaic_top_of_old = self.mosaic[old_row1_start:old_row1_start + vertical_overlap, 
-                                           expected_x:min(expected_x + fw, mw)]
+                                                expected_x:x_end]
-                frame_top = frame[:vertical_overlap, :mosaic_bottom.shape[1]]
                 
-                if mosaic_bottom.shape[0] >= min_overlap and mosaic_bottom.shape[1] >= min_overlap:
+                min_w = min(frame_bottom.shape[1], mosaic_top_of_old.shape[1])
-                    # Ensure same size
+                min_h = min(frame_bottom.shape[0], mosaic_top_of_old.shape[0])
-                    min_h = min(mosaic_bottom.shape[0], frame_top.shape[0])
-                    min_w = min(mosaic_bottom.shape[1], frame_top.shape[1])
                 
-                    if min_h >= min_overlap and min_w >= min_overlap:
+                if min_w >= min_overlap and min_h >= min_overlap:
-                        mosaic_bottom = mosaic_bottom[:min_h, :min_w]
+                    frame_bottom = frame_bottom[:min_h, :min_w]
-                        frame_top = frame_top[:min_h, :min_w]
+                    mosaic_top_of_old = mosaic_top_of_old[:min_h, :min_w]
                     
+                    # Detect displacement: how is frame_bottom shifted relative to mosaic_top_of_old?
                     dx_v, dy_v, conf_v = self._detect_displacement_with_confidence(
-                            mosaic_bottom, frame_top)
+                        mosaic_top_of_old, frame_bottom)
                     
-                        self.log(f"  Row-start vertical alignment: dx={dx_v:.1f}, dy={dy_v:.1f}, conf={conf_v:.3f}")
+                    self.log(f"  Row-start Y alignment: dx={dx_v:.1f}, dy={dy_v:.1f}, conf={conf_v:.3f}")
+                    self.log(f"    Compared frame[{fh-vertical_overlap}:{fh}] with mosaic[{old_row1_start}:{old_row1_start+vertical_overlap}]")
                     
-                        if conf_v > 0.05:  # Lower threshold for row start
+                    if conf_v > 0.1:
+                        # dy > 0 means frame_bottom is shifted DOWN relative to mosaic_top_of_old
+                        # To correct in placement: move content UP
+                        # blend formula: y_offset = row_start_y - alignment_y
+                        # So positive alignment_y decreases y_offset, moving content UP
                         offset.y_offset = dy_v
                         offset.confidence = conf_v
             
             # =============================================
-            # Step 2: Detect X alignment from side edge
+            # Step 2: Detect X alignment
             # =============================================
-            horizontal_overlap = min(fw // 2, max_overlap)
+            horizontal_overlap = min(200, fw // 3)
-            
-            if horizontal_overlap >= min_overlap:
-                # Calculate where we expect to align horizontally
-                # The frame's bottom portion should overlap with mosaic's bottom
-                expected_y = max(0, mh - fh)  # Y position based on row overlap
             
             if direction == ScanDirection.LEFT:
-                    # For LEFT scan: compare left portion of frame with right edge of mosaic
+                # For LEFT scan: frame starts at right edge
-                    mosaic_right = self.mosaic[expected_y:min(expected_y + fh, mh), 
+                # Compare frame's right edge with mosaic's right edge at OLD row 1 position
-                                              mw - horizontal_overlap:mw]
+                if old_row1_start >= 0 and mw >= horizontal_overlap:
-                    frame_left = frame[:mosaic_right.shape[0], :horizontal_overlap]
+                    y_start = old_row1_start
-                else:
+                    y_end = min(old_row1_start + fh, mh)
-                    # For RIGHT scan: compare right portion of frame with left edge of mosaic
-                    # (This is mainly for row 0, but kept for completeness)
-                    mosaic_left = self.mosaic[expected_y:min(expected_y + fh, mh),
-                                             :horizontal_overlap]
-                    frame_right = frame[:mosaic_left.shape[0], fw - horizontal_overlap:]
-                    mosaic_right = mosaic_left
-                    frame_left = frame_right
                     
-                if mosaic_right.shape[0] >= min_overlap and mosaic_right.shape[1] >= min_overlap:
+                    mosaic_edge = self.mosaic[y_start:y_end, mw - horizontal_overlap:mw]
-                    # Ensure same size
+                    frame_edge = frame[:mosaic_edge.shape[0], fw - horizontal_overlap:fw]
-                    min_h = min(mosaic_right.shape[0], frame_left.shape[0])
+                    
-                    min_w = min(mosaic_right.shape[1], frame_left.shape[1])
+                    min_h = min(mosaic_edge.shape[0], frame_edge.shape[0])
+                    min_w = min(mosaic_edge.shape[1], frame_edge.shape[1])
                     
                     if min_h >= min_overlap and min_w >= min_overlap:
-                        mosaic_right = mosaic_right[:min_h, :min_w]
+                        mosaic_edge = mosaic_edge[:min_h, :min_w]
-                        frame_left = frame_left[:min_h, :min_w]
+                        frame_edge = frame_edge[:min_h, :min_w]
                         
                         dx_h, dy_h, conf_h = self._detect_displacement_with_confidence(
-                            mosaic_right, frame_left)
+                            mosaic_edge, frame_edge)
                         
-                        self.log(f"  Row-start horizontal alignment: dx={dx_h:.1f}, dy={dy_h:.1f}, conf={conf_h:.3f}")
+                        self.log(f"  Row-start X alignment: dx={dx_h:.1f}, dy={dy_h:.1f}, conf={conf_h:.3f}")
                         
-                        if conf_h > 0.05:  # Lower threshold for row start
+                        if conf_h > 0.1:
-                            offset.x_offset = dx_h
+                            # dx > 0 means frame shifted RIGHT
-                            # Use higher confidence of the two
+                            # To correct: move LEFT (decrease x_offset)
+                            # blend: x_offset = x_offset + alignment_x
+                            # So negative alignment_x decreases x_offset
+                            offset.x_offset = -dx_h
+                            if conf_h > offset.confidence:
+                                offset.confidence = conf_h
+            else:
+                # For RIGHT scan at row start
+                if old_row1_start >= 0 and horizontal_overlap > 0:
+                    y_start = old_row1_start
+                    y_end = min(old_row1_start + fh, mh)
+                    
+                    mosaic_edge = self.mosaic[y_start:y_end, :horizontal_overlap]
+                    frame_edge = frame[:min(y_end - y_start, fh), :horizontal_overlap]
+                    
+                    min_h = min(mosaic_edge.shape[0], frame_edge.shape[0])
+                    min_w = min(mosaic_edge.shape[1], frame_edge.shape[1])
+                    
+                    if min_h >= min_overlap and min_w >= min_overlap:
+                        mosaic_edge = mosaic_edge[:min_h, :min_w]
+                        frame_edge = frame_edge[:min_h, :min_w]
+                        
+                        dx_h, dy_h, conf_h = self._detect_displacement_with_confidence(
+                            mosaic_edge, frame_edge)
+                        
+                        self.log(f"  Row-start X alignment: dx={dx_h:.1f}, dy={dy_h:.1f}, conf={conf_h:.3f}")
+                        
+                        if conf_h > 0.1:
+                            offset.x_offset = -dx_h
                             if conf_h > offset.confidence:
                                 offset.confidence = conf_h
             
-            # Validate combined offset
+            # Limit maximum adjustment
-            max_adjust = 100  # Allow larger adjustment at row start
+            max_adjust = 80
-            if abs(offset.x_offset) > max_adjust or abs(offset.y_offset) > max_adjust:
+            if abs(offset.x_offset) > max_adjust:
-                self.log(f"  Row-start alignment: offset too large ({offset.x_offset:.1f}, {offset.y_offset:.1f}), limiting")
+                self.log(f"  Limiting X offset from {offset.x_offset:.1f} to ±{max_adjust}")
                 offset.x_offset = max(-max_adjust, min(max_adjust, offset.x_offset))
+            if abs(offset.y_offset) > max_adjust:
+                self.log(f"  Limiting Y offset from {offset.y_offset:.1f} to ±{max_adjust}")
                 offset.y_offset = max(-max_adjust, min(max_adjust, offset.y_offset))
             
-            offset.valid = offset.confidence > 0.05
+            offset.valid = offset.confidence > 0.1
             
             if offset.valid:
                 self.log(f"  Row-start alignment FINAL: X={offset.x_offset:.1f}, Y={offset.y_offset:.1f}, conf={offset.confidence:.3f}")
@@ -405,7 +430,7 @@ class StitchingScanner:
             dx, dy, confidence = self._detect_displacement_with_confidence(mosaic_region, frame_region)
             
             # Sanity check - reject large displacements
-            max_adjust = 50  # Max pixels to adjust
+            max_adjust = 400  # Max pixels to adjust
             if abs(dx) > max_adjust or abs(dy) > max_adjust:
                 self.log(f"Strip alignment: displacement too large ({dx:.1f}, {dy:.1f}), ignoring")
                 return offset
@@ -437,6 +462,9 @@ class StitchingScanner:
         self._cumulative_align_y = 0.0
         self._last_strip_alignment = AlignmentOffset()
         
+        # Row 0 starts at Y=0
+        self._row_start_y = 0
+        
         with self._state_lock:
             h, w = frame.shape[:2]
             self.state.mosaic_width = w
@@ -514,8 +542,11 @@ class StitchingScanner:
             
             # Apply alignment offsets (continuous correction)
             x_offset = x_offset + int(round(alignment_x))
+            y_offset_before = y_offset
             y_offset = y_offset - int(round(alignment_y))
             
+            self.log(f"  Y offset computation: {y_offset_before} - {int(round(alignment_y))} = {y_offset}")
+            
             # Clamp x_offset to valid range
             x_offset = 0 - min(x_offset, w_base)
             
@@ -545,6 +576,7 @@ class StitchingScanner:
             self.log(f"  x_offset: {x_offset}, y_offset: {y_offset}, blend_w: {blend_w}")
             self.log(f"  alignment: X={alignment_x:.1f}, Y={alignment_y:.1f}")
             self.log(f"  cumulative: X={self._cumulative_align_x:.1f}, Y={self._cumulative_align_y:.1f}")
+            self.log(f"  row_start_y: {self._row_start_y}")
             self.log(f"  Strip crop: rows [{strip_y_start}:{strip_y_end}] -> height {h_cropped}")
             
             # Result is same size as base (no expansion when going left)
@@ -743,8 +775,9 @@ class StitchingScanner:
             dy = abs(self._displacement_since_append_y)
             
             # Calculate expected position for alignment detection
+            # Use _row_start_y for the Y position since that's where this row's content belongs
             expected_x = int(self.state.current_x + self._cumulative_align_x)
-            expected_y = int(self.state.current_y + self._cumulative_align_y)
+            expected_y = int(self._row_start_y + self._cumulative_align_y)
             
             # Detect alignment for this strip
             alignment = self._detect_strip_alignment(frame, direction, expected_x, expected_y)
@@ -769,8 +802,9 @@ class StitchingScanner:
                 pixels_consumed = append_width - SAFETY_MARGIN
                 fractional_remainder = dx - pixels_consumed
                 
-                # Calculate Y offset for current row
+                # Use _row_start_y for Y position - this is the Y position in the mosaic
-                y_offset = int(self.state.current_y)
+                # where the current row's content belongs
+                y_offset = self._row_start_y
                 
                 if direction == ScanDirection.RIGHT:
                     strip_start = max(0, w - append_width - BLEND_WIDTH)
@@ -856,6 +890,9 @@ class StitchingScanner:
         self._cumulative_align_y = 0.0
         self._last_strip_alignment = AlignmentOffset()
         
+        # Reset row start Y position
+        self._row_start_y = 0
+        
         self._thread = threading.Thread(target=self._scan_loop, daemon=True)
         self._thread.start()
         
@@ -905,6 +942,7 @@ class StitchingScanner:
                     self.state.total_rows = row + 1
                 
                 self.log(f"=== Row {row + 1} ===")
+                self.log(f"Row start Y position: {self._row_start_y}")
                 self.log(f"Cumulative alignment at row start: X={self._cumulative_align_x:.1f}, Y={self._cumulative_align_y:.1f}")
                 
                 # Serpentine: even rows right, odd rows left
@@ -1074,11 +1112,15 @@ class StitchingScanner:
         frame = self._capture_frame()
         h, w = frame.shape[:2]
         
+        # Record mosaic height before transition - needed to calculate new row Y position
+        mosaic_height_before = self.state.mosaic_height
+        
         # Target: move (1 - overlap) * frame_height
         target_displacement = h * (1 - self.config.row_overlap)
         threshold_pixels = h * self.config.displacement_threshold
         
         self.log(f"Target Y: {target_displacement:.0f}px, threshold: {threshold_pixels:.0f}px")
+        self.log(f"Mosaic height before row transition: {mosaic_height_before}")
         
         with self._state_lock:
             self.state.direction = 'down'
@@ -1135,6 +1177,15 @@ class StitchingScanner:
                     self.log(f"Row transition complete: {abs(total_y):.1f}px")
                     self.log(f"Alignment after row transition: X={self._cumulative_align_x:.1f}, Y={self._cumulative_align_y:.1f}")
                     
+                    # Calculate the Y position in the mosaic where the new row starts
+                    # Since we use append_below=False during DOWN movement, new content
+                    # is PREPENDED to the TOP of the mosaic. So the new row starts at Y=0
+                    # (or close to it, accounting for overlap)
+                    overlap_pixels = int(h * self.config.row_overlap)
+                    self._row_start_y = 0  # New row is at the TOP after prepending
+                    
+                    self.log(f"New row Y position: {self._row_start_y} (mosaic height: {self.state.mosaic_height})")
+                    
                     with self._state_lock:
                         self.state.current_y = 0
                     self.motion.send_command('s')
@@ -1188,6 +1239,7 @@ class StitchingScanner:
         return {
             'cumulative_x': self._cumulative_align_x,
             'cumulative_y': self._cumulative_align_y,
+            'row_start_y': self._row_start_y,
             'last_alignment': {
                 'x': self._last_strip_alignment.x_offset,
                 'y': self._last_strip_alignment.y_offset,
@@ -1390,7 +1442,7 @@ class StitchingScanner:
             
             frame = self._capture_frame()
             expected_x = int(self.state.current_x + self._cumulative_align_x)
-            expected_y = int(self.state.current_y + self._cumulative_align_y)
+            expected_y = int(self._row_start_y + self._cumulative_align_y)
             
             # Test for both directions
             for direction in [ScanDirection.RIGHT, ScanDirection.LEFT]: