diff --git a/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py b/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py index 950930e..7472706 100644 --- a/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py +++ b/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py @@ -73,7 +73,8 @@ def spot_main() -> None: images, poses = get_multiple_perspective_camera_calibration_dataset( auto_cam_cal_robot=in_hand_bot, max_num_images=args.max_num_images, - distances=np.arange(*args.dist_from_board_viewpoint_range), + distances_z=np.arange(*args.dist_from_board_viewpoint_range), + distances_x=np.arange(*args.dist_along_board_width), x_axis_rots=np.arange(*args.x_axis_rot_viewpoint_range), y_axis_rots=np.arange(*args.y_axis_rot_viewpoint_range), z_axis_rots=np.arange(*args.z_axis_rot_viewpoint_range), @@ -131,6 +132,19 @@ def calibrate_robot_cli(parser: argparse.ArgumentParser = None) -> argparse.Argu "These are used to sample viewpoints for automatic collection. " ), ) + parser.add_argument( + "--dist_along_board_width", + "-dabw", + nargs="+", + type=float, + dest="dist_along_board_width", + default=[-0.3, 0.4, 0.2], + help=( + "What distances to conduct calibrations at relative to the board. (along the board width) " + "Three value array arg defines the [Start, Stop), step. for the viewpoint sweep. " + "These are used to sample viewpoints for automatic collection. " + ), + ) group = parser.add_mutually_exclusive_group() group.add_argument( "--degrees", diff --git a/spot_wrapper/calibration/calibration_util.py b/spot_wrapper/calibration/calibration_util.py index 297dfad..19cc33a 100644 --- a/spot_wrapper/calibration/calibration_util.py +++ b/spot_wrapper/calibration/calibration_util.py @@ -104,7 +104,8 @@ def create_charuco_board( def get_multiple_perspective_camera_calibration_dataset( auto_cam_cal_robot: AutomaticCameraCalibrationRobot, max_num_images: int = 10000, - distances: Optional[np.ndarray] = None, + distances_x: Optional[np.ndarray] = None, + distances_z: Optional[np.ndarray] = None, x_axis_rots: Optional[np.ndarray] = None, y_axis_rots: Optional[np.ndarray] = None, z_axis_rots: Optional[np.ndarray] = None, @@ -161,7 +162,8 @@ def get_multiple_perspective_camera_calibration_dataset( viewpoints = get_relative_viewpoints_from_board_pose_and_param( R_vision_to_target, tvec_vision_to_target, - distances=distances, + distances_x=distances_x, + distances_z=distances_z, x_axis_rots=x_axis_rots, y_axis_rots=y_axis_rots, z_axis_rots=z_axis_rots, @@ -883,7 +885,8 @@ def convert_camera_t_viewpoint_to_origin_t_planning_frame( def get_relative_viewpoints_from_board_pose_and_param( R_board: np.ndarray, tvec: np.ndarray, - distances: Optional[np.ndarray] = None, + distances_x: Optional[np.ndarray] = None, + distances_z: Optional[np.ndarray] = None, x_axis_rots: Optional[np.ndarray] = None, y_axis_rots: Optional[np.ndarray] = None, z_axis_rots: Optional[np.ndarray] = None, @@ -936,8 +939,10 @@ def get_relative_viewpoints_from_board_pose_and_param( List[np.ndarray]: a list of 4x4 homogenous transforms to visit in the "principal" cameras frame """ - if distances is None: - distances = np.arange(0.5, 0.7, 0.1) + if distances_x is None: + distances_x = np.arange(0.3, 0.7, 0.1) + if distances_z is None: + distances_z = np.arange(-0.2, 0.3, 0.1) if x_axis_rots is None: x_axis_rots = np.arange(-20, 21, 5) if y_axis_rots is None: @@ -951,7 +956,8 @@ def get_relative_viewpoints_from_board_pose_and_param( x_axis_rots = [radians(deg) for deg in x_axis_rots] y_axis_rots = [radians(deg) for deg in y_axis_rots] z_axis_rots = [radians(deg) for deg in z_axis_rots] - translations = [tvec + R_board[:, 2] * dist for dist in distances] + + translations = [(tvec + R_board[:, 2] * dist + R_board[:, 0] * d2) for dist in distances_z for d2 in distances_x] R_base = R_board @ R_align_board_frame_with_camera def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: