From 5b12441966a50bae847eef56ae9d473e08c4d2c2 Mon Sep 17 00:00:00 2001 From: Kazadhum Date: Mon, 13 May 2024 15:45:43 +0100 Subject: [PATCH] added verifications (adapted from the cv_eye_in_hand.py script) #939 --- .../other_calibrations/ali_eye_in_hand.py | 40 ++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/atom_evaluation/scripts/other_calibrations/ali_eye_in_hand.py b/atom_evaluation/scripts/other_calibrations/ali_eye_in_hand.py index 7772befe..f674e5db 100755 --- a/atom_evaluation/scripts/other_calibrations/ali_eye_in_hand.py +++ b/atom_evaluation/scripts/other_calibrations/ali_eye_in_hand.py @@ -22,7 +22,7 @@ import math from atom_core.dataset_io import filterCollectionsFromDataset, loadResultsJSON -from atom_core.atom import getTransform +from atom_core.atom import getTransform, getChain from atom_core.geometry import traslationRodriguesToTransform from atom_core.naming import generateKey from atom_core.transformations import compareTransforms @@ -164,6 +164,44 @@ def main(): selected_collection_key = list(dataset["collections"].keys())[0] print("Selected collection key is " + str(selected_collection_key)) + # --------------------------------------- + # Verifications + # --------------------------------------- + + # Check that the camera has rgb modality + if not dataset['sensors'][args['camera']]['modality'] == 'rgb': + atomError('Sensor ' + args['camera'] + ' is not of rgb modality.') + + # Check the given hand link is in the chain from base to camera + chain = getChain(from_frame=args['base_link'], + to_frame=dataset['calibration_config']['sensors'][args['camera']]['link'], + transform_pool=dataset['collections'][selected_collection_key]['transforms']) + + hand_frame_in_chain = False + for transform in chain: + + if args['hand_link'] == transform['parent'] or args['hand_link'] == transform['child']: + hand_frame_in_chain = True + + if not hand_frame_in_chain: + atomError('Selected hand link ' + Fore.BLUE + args['hand_link'] + Style.RESET_ALL + + ' does not belong to the chain from base ' + Fore.BLUE + args['base_link'] + + Style.RESET_ALL + ' to the camera ' + + dataset['calibration_config']['sensors'][args['camera']]['link']) + + # Check the hand to camera chain is composed only of fixed transforms + chain = getChain(from_frame=args['hand_link'], + to_frame=dataset['calibration_config']['sensors'][args['camera']]['link'], + transform_pool=dataset['collections'][selected_collection_key]['transforms']) + + for transform in chain: + if not dataset['transforms'][transform['key']]['type'] == 'fixed': + atomError('Chain from hand link ' + Fore.BLUE + args['hand_link'] + Style.RESET_ALL + + ' to camera link ' + Fore.BLUE + + dataset['calibration_config']['sensors'][args['camera']]['link'] + + Style.RESET_ALL + ' contains non fixed transform ' + Fore.RED + + transform['key'] + Style.RESET_ALL + '. Cannot calibrate.') + ######################################## # DATASET PREPROCESSING #