diff --git a/src/openflexure_microscope_server/things/stage_measure.py b/src/openflexure_microscope_server/things/stage_measure.py index fea5db7a..948a8c68 100644 --- a/src/openflexure_microscope_server/things/stage_measure.py +++ b/src/openflexure_microscope_server/things/stage_measure.py @@ -26,6 +26,12 @@ CamDep = direct_thing_client_dependency(StreamingPiCamera2, "/camera/") CSMDep = direct_thing_client_dependency(CameraStageMapper, "/camera_stage_mapping/") AutofocusDep = direct_thing_client_dependency(AutofocusThing, "/autofocus/") +def quadratic(x, a, b, c): + ''' + Quadratic function + ''' + return a * x**2 + b * x + c + def dict_generate(dict_steps, stream_resolution, dir, factor = 1): ''' Creates a single dictionary of step sizes @@ -45,8 +51,10 @@ def steps_generate(small_step, z_perc, big_step, dir, stream_resolution): minimum_offset_small = dict_generate(small_step, stream_resolution, dir, factor = 0.8) z_steps = dict_generate(z_perc, stream_resolution, dir) minimum_offset_z = dict_generate(z_perc, stream_resolution, dir, factor = 0.8) + wrong_axis_max_small = dict_generate(small_step, stream_resolution, dir, factor = 0.1) + wrong_axis_max_z = dict_generate(z_perc, stream_resolution, dir, factor = 0.1) - return step_sizes_small, minimum_offset_small, z_steps, minimum_offset_z, step_sizes_big + return step_sizes_small, minimum_offset_small, z_steps, minimum_offset_z, step_sizes_big, wrong_axis_max_small, wrong_axis_max_z class RangeofMotionThing(Thing): def rom_axis( @@ -65,10 +73,27 @@ class RangeofMotionThing(Thing): """ try: + if direction == 1: + dir_word = "positive" + else: + dir_word = "negative" + + logger.info(f"Beginning the {axis}-axis in the {dir_word} direction") + + stage_coords = [] + cor_lat_steps = [] + focused_positions = [] + axis_results = {} + # Generate required dictionaries for step sizes and minimum offsets stream_resolution = cam.stream_resolution - step_sizes_small, minimum_offset_small, z_steps, minimum_offset_z, step_sizes_big = steps_generate(20, 50, 200, direction, stream_resolution=stream_resolution) + res_dic = { + 'x': stream_resolution[0], + 'y': stream_resolution[1] + } + + step_sizes_small, minimum_offset_small, z_steps, minimum_offset_z, step_sizes_big, wrong_axis_max_small, wrong_axis_max_z = steps_generate(20, 50, 200, direction, stream_resolution=stream_resolution) delta = { 'x':0, @@ -81,6 +106,10 @@ class RangeofMotionThing(Thing): starting_position = list(stage.position.values()) + parasitic_motion = False + + stage_coords.append(stage.position) + logger.info("Moving the stage in 4 medium sized steps.") # Medium sized steps @@ -88,23 +117,54 @@ class RangeofMotionThing(Thing): image1 = cv2.resize(np.array(Image.open(cam.grab_jpeg().open())), dsize=(0,0), fx= 1, fy= 1) if axis == 'x': csm.move_in_image_coordinates(x = z_steps['x'], y = 0) + wrong_axis = 'y' else: csm.move_in_image_coordinates(x = 0, y = z_steps['y']) + wrong_axis = 'x' focus_data = autofocus.looping_autofocus(dz = 800) image2 = cv2.resize(np.array(Image.open(cam.grab_jpeg().open())), dsize=(0,0), fx= 1, fy= 1) offset = [x * 1 for x in fft_image_tracking.displacement_between_images(image_0 = image1, image_1 = image2, sigma=10, fractional_threshold=0.1, pad=True)] # Units is pixels delta['x'] = int(offset[1]) delta['y'] = int(offset[0]) logger.info(f"Offset measured as {delta[axis]}") + stage_coords.append(stage.position) + cor_lat_steps.append(offset) + focused_positions.append(stage.position) + + # Check for parasitic motion + + if np.abs(delta[wrong_axis]) > np.abs(wrong_axis_max_z[wrong_axis]): + logger.info(f"Parasitic motion detected in {wrong_axis}-axis whilst measuring {axis}-axis.") + parasitic_motion = True + break + + pixel_per_step = ((1/abs(csm.image_to_stage_displacement_matrix[0][1])) + + (1/abs(csm.image_to_stage_displacement_matrix[1][0])))/4 + lateral_positions = [i[axis] for i in focused_positions] + z_positions = [i['z'] for i in focused_positions] + parameters, covariance = curve_fit(quadratic, lateral_positions, z_positions) + relative_move = direction * 2 * res_dic[axis]/(2*pixel_per_step) # This is the number of steps to cover 200% of the FOV. + z_dest = quadratic(stage.position[axis] + relative_move, *parameters) + z_diff = z_dest - stage.position['z'] + + logger.info(f"Z calibration complete.") # 1 big step followed by 3 small steps - while np.abs(delta[axis]) > minimum_offset_small[axis]: + while np.abs(delta[axis]) > np.abs(minimum_offset_small[axis]) and parasitic_motion == False: + + stage.move_relative(z = z_diff) + + logger.info(f"Moved in z by {z_diff}") if axis == 'x': csm.move_in_image_coordinates(x = step_sizes_big['x'], y = 0) else: csm.move_in_image_coordinates(x = 0, y = step_sizes_big['y']) + stage_coords.append(stage.position) + + failure_count = 0 + for loop in range(3): image1 = cv2.resize(np.array(Image.open(cam.grab_jpeg().open())), dsize=(0,0), fx= 1, fy= 1) if axis == 'x': @@ -117,8 +177,28 @@ class RangeofMotionThing(Thing): delta['x'] = int(offset[1]) delta['y'] = int(offset[0]) logger.info(f"Offset measured as {delta[axis]}") + + while np.abs(delta[axis]) < minimum_offset_small[axis] and failure_count < 3: + logger.info(f"Correlation failed. Refocusing to check. Attempt {failure_count + 1}/3") + focus_data = autofocus.looping_autofocus(dz = 1000) + image2 = cv2.resize(np.array(Image.open(cam.grab_jpeg().open())), dsize=(0,0), fx= 1, fy= 1) + failure_count += 1 + offset = [x * 1 for x in fft_image_tracking.displacement_between_images( + image_0 = image1, image_1 = image2, sigma=10, fractional_threshold=0.1, pad=True)] # Units is pixels + delta['x'] = int(offset[1]) + delta['y'] = int(offset[0]) + logger.info(f"Displacement found was {np.abs(delta[axis])}. Minimum offset is {minimum_offset_small[axis]}") - if np.abs(delta[axis]) < minimum_offset_small[axis]: # this means the edge has been found + stage_coords.append(stage.position) + cor_lat_steps.append(offset) + focused_positions.append(stage.position) + + if np.abs(delta[wrong_axis]) > np.abs(wrong_axis_max_small[wrong_axis]): + logger.info(f"Parasitic motion detected in {wrong_axis}-axis whilst measuring {axis}-axis.") + parasitic_motion = True + break + + if np.abs(delta[axis]) < np.abs(minimum_offset_small[axis]): # this means the edge has been found logger.info(f"Edge has been found.") break @@ -151,13 +231,25 @@ class RangeofMotionThing(Thing): logger.info("Motion detected.") break + stage_coords[np.shape(stage_coords)[0] - 1] = stage.position + + final_pos = stage.position + + axis_results = { + "correlation_lateral_steps": cor_lat_steps, + "stage_positions": stage_coords, + "final_position": final_pos + } + stage.move_absolute(x = starting_position[0], y = starting_position[1], z = starting_position[2], block_cancellation=True) + + except: logger.error("Stopping measurement because it was cancelled by the user") stage.move_absolute(x = starting_position[0], y = starting_position[1], z = starting_position[2], block_cancellation=True) raise Exception - return focus_data + return axis_results @thing_action def rom_main( @@ -183,6 +275,17 @@ class RangeofMotionThing(Thing): axis = 'x', direction = 1 ) + # for axis_dir in [['x', 1],['x', -1],['y', 1],['y', -1]]: + # axis_dir_results = self.rom_axis( + # autofocus, + # stage, + # cam, + # csm, + # cancel, + # logger, + # axis = axis_dir[0], + # direction = axis_dir[1] + # ) return x_pos_results