First interation of a recentre algoritm build on ROM test methods

src/openflexure_microscope_server/things/camera_stage_mapping.py

@@ -277,15 +277,27 @@ class CameraStageMapper(lt.Thing):
 
     @lt.thing_action
     def convert_image_to_stage_coordinates(
-        self, x: float, y: float
+        self, x: float, y: float, **_kwargs: float
     ) -> Mapping[str, int]:
-        """Convert image coordinates to stage coordinates."""
+        """Convert image coordinates to stage coordinates. Only x and y are returned."""
         self.assert_calibrated()
         relative_move: np.ndarray = np.dot(
             np.array([y, x]), np.array(self.image_to_stage_displacement_matrix)
         )
         return {"x": int(relative_move[0]), "y": int(relative_move[1])}
 
+    @lt.thing_action
+    def convert_stage_to_image_coordinates(
+        self, x: float, y: float, **_kwargs: float
+    ) -> Mapping[str, int]:
+        """Convert stage coordinates to image coordinates. Only x and y are returned."""
+        self.assert_calibrated()
+        inverse_matrix = np.linalg.inv(
+            np.array(self.image_to_stage_displacement_matrix)
+        )
+        relative_move = np.dot(np.array([x, y]), inverse_matrix)
+        return {"x": int(relative_move[1]), "y": int(relative_move[0])}
+
     @lt.thing_property
     def thing_state(self) -> Mapping[str, Any]:
         """Summary metadata describing the current state of the Thing."""

src/openflexure_microscope_server/things/stage_measure.py

@@ -97,13 +97,12 @@ class RomDataTracker:
     def find_turning_point(self, axis: Literal["x", "y"]) -> dict[str, int]:
         """Find the turing point from the recorded coordinates."""
         fit_func = self.fit_axis(axis)
-        turning = fit_func.deriv()
+        turning_loc = fit_func.deriv().roots[0]
-        turning_loc = -turning[0] / (turning[1])
         turning_z = fit_func(turning_loc)
 
         # As we only move in 1 direction pull the other axis from the coords.
         other_axis = "x" if axis == "y" else "y"
-        other_coord = self.stage_coords[-1]["other_axis"]
+        other_coord = self.stage_coords[-1][other_axis]
         return {axis: int(turning_loc), other_axis: other_coord, "z": int(turning_z)}
 
 
@@ -235,11 +234,7 @@ class RangeofMotionThing(lt.Thing):
             rom_deps = RomDeps(
                 autofocus=autofocus, stage=stage, csm=csm, cam=cam, logger=logger
             )
-            logger.info(
+            logger.info("Recentring the stage.")
-                "Using the stage to measure the Range of Motion. "
-                "Please ensure you are using a sample that covers the whole range of "
-                "motion. This should be approximately 12 x 12 mm."
-            )
 
             self._set_stream_resolution(cam)
 
@@ -251,6 +246,7 @@ class RangeofMotionThing(lt.Thing):
 
             # Set the central position to (0,0,0)
             rom_deps.stage.set_zero_position()
+            rom_deps.logger.info("Position reset to (0, 0, 0).")
 
         finally:
             self._lock.release()
@@ -294,7 +290,7 @@ class RangeofMotionThing(lt.Thing):
             )
 
             rom_deps.logger.info("Moving the stage in 5 medium sized steps.")
-            self._initial_moves_for_z_prediction(
+            self._moves_for_z_prediction(
                 axis=axis,
                 direction=direction,
                 rom_deps=rom_deps,
@@ -336,19 +332,53 @@ class RangeofMotionThing(lt.Thing):
 
     def _recentre_axis(self, axis: Literal["x", "y"], rom_deps: RomDeps) -> None:
         """Recentre a single axis."""
-        rom_deps.autofocus.looping_autofocus(dz=1000)
+        rom_deps.logger.info(f"Finding centre in {axis}.")
-        self._rom_data.stage_coords.append(rom_deps.stage.position)
+        # A new tracker for this axis.
-
         self._rom_data = RomDataTracker()
-        self._initial_moves_for_z_prediction(
+        # Direction is in image coords, initial assumption is that centre is at (0,0).
-            axis=axis,
+        direction = self._img_dir_from_stage_coords(
-            direction=+1,
+            {"x": 0, "y": 0, "z": 0}, axis, rom_deps
-            rom_deps=rom_deps,
         )
-        estimate = self._rom_data.find_turning_point(axis=axis)
+
-        here = rom_deps.stage.position
+        i = 0
-        # Make a copy of the current for the estimate
+        while True:
-        raise RuntimeError(f"I should move from {here} to {estimate}")
+            i += 1
+            # Find z then make a number of moves (autofocussing and logging position)
+            # 5 moves initially (2 subsequently).
+            rom_deps.autofocus.looping_autofocus(dz=1000)
+            self._rom_data.stage_coords.append(rom_deps.stage.position)
+            self._moves_for_z_prediction(
+                axis=axis,
+                direction=direction,
+                rom_deps=rom_deps,
+                n_moves=5 if i == 1 else 2,
+            )
+            # Try to estimate position/direction the first time, skip to making a big
+            # move in the same direction
+            if i > 1:
+                estimate = self._rom_data.find_turning_point(axis=axis)
+                img_perc = self._distance_in_img_percentage(estimate, axis, rom_deps)
+
+                # if the distance is less than 1 big step away then move to it and exit
+                if abs(img_perc) < BIG_STEP:
+                    rom_deps.logger.info(
+                        f"Estimated centre of {axis}-axis is {estimate[axis]}"
+                    )
+                    rom_deps.stage.move_absolute(**estimate)
+                    break
+                rom_deps.logger.info(
+                    f"Estimated centre {abs(img_perc):.0f}% of a field of view away, "
+                    "that is too far to move in one move."
+                )
+                # Else calculate the desired direction in image coordinates.
+                direction = self._img_dir_from_stage_coords(estimate, axis, rom_deps)
+
+            # If still going at iteration 9 exit
+            if i > 9:
+                raise RuntimeError(f"Couldn't find centre of {axis}-axis")
+
+            # Make a big z-corrected move towards estimate of centre.
+            self._big_z_corrected_movement(axis, direction, rom_deps)
 
     def _img_percentage_to_img_coords(
         self, fov_perc: int, axis: Literal["x", "y"]
@@ -356,7 +386,7 @@ class RangeofMotionThing(lt.Thing):
         """For a given image percentage and axis return the distance in img coords.
 
         :param fov_perc: The percentage of field of view the stage should move by.
-        :param axis: The resolution of the stream from the camera.
+        :param axis: The axis which is being measured. This must be 'x' or 'y'.
         :return: Distance in image coordinates (pixels)
         """
         if self._stream_resolution is None:
@@ -367,6 +397,44 @@ class RangeofMotionThing(lt.Thing):
         img_index = 0 if axis == "x" else 1
         return (fov_perc / 100) * self._stream_resolution[img_index]
 
+    def _img_dir_from_stage_coords(
+        self, target: dict[str, int], axis: Literal["x", "y"], rom_deps: RomDeps
+    ) -> Literal[1, -1]:
+        """For a target location in stage coords return the direction in image coordinates.
+
+        :param target: The target poisiton in stage coordinates
+        :param axis: The axis which is being measured. This must be 'x' or 'y'.
+        :param rom_deps: All dependencies that were passed to the calling Action.
+        :return: Direction to move in image coordinates.
+        """
+        target_im_coords = rom_deps.csm.convert_stage_to_image_coordinates(**target)
+        here = rom_deps.stage.position
+        here_im_coords = rom_deps.csm.convert_stage_to_image_coordinates(**here)
+
+        return -1 if target_im_coords[axis] < here_im_coords[axis] else 1
+
+    def _distance_in_img_percentage(
+        self, target: dict[str, int], axis: Literal["x", "y"], rom_deps: RomDeps
+    ) -> float:
+        """For a target location in stage coords return the distance in percentage of FOV.
+
+        :param target: The target poisiton in stage coordinates
+        :param axis: The axis which is being measured. This must be 'x' or 'y'.
+        :param rom_deps: All dependencies that were passed to the calling Action.
+        :return: Percentage of field of view the stage should move by.
+        """
+        if self._stream_resolution is None:
+            raise RuntimeError(
+                "Stream resolution must be set before converting coords to percentage"
+            )
+
+        here = rom_deps.stage.position
+        move_stage = {key: target[key] - here[key] for key in target}
+        move_img = rom_deps.csm.convert_stage_to_image_coordinates(**move_stage)
+
+        img_index = 0 if axis == "x" else 1
+        return (move_img[axis] / self._stream_resolution[img_index]) * 100
+
     def _movement_in_img_coords(
         self,
         fov_perc: int,
@@ -388,36 +456,37 @@ class RangeofMotionThing(lt.Thing):
             return {"x": distance * direction, "y": 0}
         return {"x": 0, "y": distance * direction}
 
-    def _initial_moves_for_z_prediction(
+    def _moves_for_z_prediction(
         self,
         axis: Literal["x", "y"],
         direction: Literal[1, -1],
         rom_deps: RomDeps,
+        n_moves: int = 5,
     ) -> None:
-        """Perform 5 medium sized moves with autofocus for z feed-forward.
+        """Perform medium sized moves with autofocus for z feed-forward.
 
         z-feed forward allows prediction of the z-position as the stage moves. For the
         feed forward calculation to work an initial number of measurements must be
-        taken. This method performs these initial measurements.
+        taken.
 
         :param direction: The direction the stage moves.
         :param axis: The axis which is being measured. This must be 'x' or 'y'.
         :param rom_deps: All dependencies that were passed to the calling Action
+        :param n_moves: Number of moves to make. Default is 5 which is enough for an
+            initial z estimate.
         """
         movement = self._movement_in_img_coords(
             fov_perc=MEDIUM_STEP, axis=axis, direction=direction
         )
-        for _loop in range(5):
+        for _loop in range(n_moves):
             offset = self._move_and_measure(movement=movement, rom_deps=rom_deps)
-            rom_deps.logger.info(f"Offset measured as {offset[axis]}")
 
             self._rom_data.record_movement(rom_deps.stage.position, offset)
             if _parasitic_motion_detected(movement, offset):
                 raise ParasiticMotionError(
-                    "Parasitic motion detected during initial images to calculate "
+                    "Parasitic motion detected during images to calculate z-curvature. "
-                    "z-curvature. This may indicate you have started at the end of the "
+                    "This may indicate you have started at the end of the range of "
-                    "range of travel, or that camera stage mapping is poorly "
+                    "travel, or that camera stage mapping is poorly calibrated."
-                    "calibrated."
                 )
 
     def _big_z_corrected_movement(
@@ -435,11 +504,11 @@ class RangeofMotionThing(lt.Thing):
             fov_perc=BIG_STEP, axis=axis, direction=direction
         )
         # Convert to stage coordinates
-        stage_movemenet = rom_deps.csm.convert_image_to_stage_coordinates(**movement)
+        stage_movement = rom_deps.csm.convert_image_to_stage_coordinates(**movement)
 
         z_disp = self._rom_data.predict_z_displacement(
             axis=axis,
-            stage_movement=stage_movemenet,
+            stage_movement=stage_movement,
             stage_position=rom_deps.stage.position,
         )
         rom_deps.stage.move_relative(z=z_disp)

tests/test_stage_measure.py

@@ -410,7 +410,7 @@ def test_big_z_corrected_movement(rom_thing, mock_rom_deps):
     assert lat_mov_kwargs == expected_movement
 
 
-def test_initial_moves_for_z_prediction(rom_thing, mock_rom_deps, mocker):
+def test_moves_for_z_prediction(rom_thing, mock_rom_deps, mocker):
     """Check the initial moves are of the correct size and are recorded."""
     # Mock the _offset_from and stage.position to return generated dictionaries that
     # increment each time they are called. (All values 0 the first time, all values 1
@@ -428,7 +428,7 @@ def test_initial_moves_for_z_prediction(rom_thing, mock_rom_deps, mocker):
     rom_thing._rom_data = stage_measure.RomDataTracker()
 
     # Run it!
-    rom_thing._initial_moves_for_z_prediction("x", direction=-1, rom_deps=mock_rom_deps)
+    rom_thing._moves_for_z_prediction("x", direction=-1, rom_deps=mock_rom_deps)
 
     # Check that _rom_data now contains the 5 mocked returns in order.
     assert rom_thing._rom_data.offsets == [{"x": i, "y": i} for i in range(5)]
@@ -450,7 +450,7 @@ def test_move_until_edge_error(rom_thing, mock_rom_deps, mocker):
         return_value=mock_position_dict
     )
     mocker.patch.object(
-        rom_thing, "_initial_moves_for_z_prediction", side_effect=RuntimeError("Mock")
+        rom_thing, "_moves_for_z_prediction", side_effect=RuntimeError("Mock")
     )
 
     # Remove the mock RomData before starting
@@ -493,7 +493,7 @@ def test_move_until_edge(rom_thing, mock_rom_deps, mocker):
     # Mock the main movement functions
     mock_init_moves = mocker.patch.object(
         rom_thing,
-        "_initial_moves_for_z_prediction",
+        "_moves_for_z_prediction",
         side_effect=add_fake_initial_positions,
     )
     mock_big_moves = mocker.patch.object(