Merge pull request #1008 from lardemua/single_tf_noise

Single tf noise

Author: miguelriemoliveira

atom_calibration/scripts/calibrate

@@ -26,7 +26,9 @@ from atom_calibration.calibration.getters_and_setters import (
 from atom_calibration.calibration.objective_function import errorReport, objectiveFunction, replaceTransformsFromJoints
 from atom_calibration.calibration.visualization import setupVisualization, visualizationFunction
 from atom_core.dataset_io import (addNoiseToInitialGuess,
-                                  addNoiseToJointParameters,
+                                  addBiasToJointParameters,
+                                  addNoiseToTF,
+                                  addNoiseFromNoisyTFLinks,
                                   checkIfAtLeastOneLabeledCollectionPerSensor,
                                   filterCollectionsFromDataset,
                                   filterSensorsFromDataset,
@@ -44,6 +46,7 @@ from atom_core.utilities import (atomError, createLambdaExpressionsForArgs,
                                  printComparisonToGroundTruth, saveCommandLineArgsYml)
 from atom_core.xacro_io import saveResultsXacro
 from atom_core.results_yml_io import saveResultsYml
+from atom_core.config_io import parse_list_of_transformations, mutually_inclusive_conditions
 
 
 # -------------------------------------------------------------------------------
@@ -126,6 +129,13 @@ def main():
         type=str, required=False)
     ap.add_argument("-jbv", "--joint_bias_values", nargs='+',
                     help='Operates in tandem with "joint_bias_names"', type=float, required=False)
+    
+    ap.add_argument("-ntfl", "--noisy_tf_links", type=parse_list_of_transformations, help='''Pairs of links defining the transformations to which noise will be added, in the format linkParent1:linkChild2,linkParentA:linkChildB (links defining a transformation separated by
+                    : and different pairs separeted by ,)
+                    Note : This flag overrides the -nig flag''')
+    ap.add_argument("-ntfv", "--noisy_tf_values", nargs=2, type=float, metavar=("translation", "rotation"),
+                    help="Translation(m) and Rotation(rad)(respectively) noise values to add to the transformations specified by-ntfl")
+
     ap.add_argument(
         "-ssf", "--sensor_selection_function", default=None, type=str,
         help="A string to be evaluated into a lambda function that receives a sensor name as input and "
@@ -205,6 +215,9 @@ def main():
     args['dataset_folder'] = os.path.dirname(args['json_file'])
     output_folder = os.path.dirname(args['json_file'])
 
+    # -sce wasn't working because args previously had this key
+    args['output_folder'] = output_folder
+
     # ---------------------------------------
     # --- Read data from file
     # ---------------------------------------
@@ -261,8 +274,10 @@ def main():
 
     addNoiseToInitialGuess(dataset, args, selected_collection_key)
 
+    addNoiseFromNoisyTFLinks(dataset,args,selected_collection_key)
+
     # Must add noise, transfer new joints to transforms, and only after remove unselected joints
-    addNoiseToJointParameters(dataset, args)
+    addBiasToJointParameters(dataset, args)
     replaceTransformsFromJoints(dataset)
     dataset = filterJointsFromDataset(dataset, args)
     dataset = filterJointParametersFromDataset(dataset, args)

atom_core/src/atom_core/config_io.py

@@ -16,6 +16,25 @@
 from atom_core.utilities import atomError
 from atom_core.system import resolvePath, expandToLaunchEnv
 
+def mutually_inclusive_conditions(A,B):
+
+    if A is not None and B is not None:
+        return True
+    elif A is None and B is None:
+        return False
+    else:
+        atomError(f'{Fore.RED}-ntfl{Style.RESET_ALL} and {Fore.RED}-ntfv{Style.RESET_ALL} flags are mutually inclusive') 
+        exit()
+
+def parse_list_of_transformations(s):
+    list_of_transformations = []
+    for pair in s.split(','):
+        elements = pair.split(':')
+        if len(elements) != 2:
+            raise atomError("Each pair of transformations parsed must contain exactly 2 elements separated by ':'")
+        list_of_transformations.append(elements)
+    return list_of_transformations
+
 
 def dictionaries_have_same_keys(d1, d2):
 

atom_core/src/atom_core/dataset_io.py

@@ -24,8 +24,8 @@
 from colorama import Fore, Style
 from rospy_message_converter import message_converter
 from std_msgs.msg import Header
-from atom_core.config_io import uriReader
-from atom_core.naming import generateName, generateKey
+from atom_core.config_io import uriReader, mutually_inclusive_conditions
+from atom_core.naming import generateName, generateKey, generateCollectionKey
 from atom_calibration.collect.label_messages import (
     convertDepthImage32FC1to16UC1, convertDepthImage16UC1to32FC1, numpyFromPointCloudMsg)
 
@@ -787,7 +787,7 @@ def filterAdditionalTfsFromDataset(dataset, args):
     return dataset
 
 
-def addNoiseToJointParameters(dataset, args):
+def addBiasToJointParameters(dataset, args):
     """
     Adds noise
     :param dataset:
@@ -828,6 +828,25 @@ def addNoiseToJointParameters(dataset, args):
             collection['joints'][joint_name][joint_param] = collection['joints'][joint_name][
                 joint_param] + joint_bias
 
+def addNoiseFromNoisyTFLinks(dataset,args,selected_collection_key):
+
+    # Verify both arguments were provided
+    # Unfortunately, mutually inclusive arguments are not built into argparse
+    # https://github.com/python/cpython/issues/55797
+
+    if not mutually_inclusive_conditions(args['noisy_tf_links'], args['noisy_tf_values']):
+        return
+
+
+    # Iterate through pairs of tf's and apply noise
+    translation_tf_noise = args['noisy_tf_values'][0]
+    rotation_tf_noise = args['noisy_tf_values'][1]
+
+    for tf_pair in args['noisy_tf_links']:
+        
+        calibration_parent,calibration_child = tf_pair[0],tf_pair[1]
+        addNoiseToTF(dataset,selected_collection_key,calibration_parent,calibration_child,translation_tf_noise,rotation_tf_noise)
+
 
 def addNoiseToInitialGuess(dataset, args, selected_collection_key):
     """
@@ -836,19 +855,38 @@ def addNoiseToInitialGuess(dataset, args, selected_collection_key):
     :param args: Makes use of nig, i.e., the amount of noise to add to the initial guess atomic transformations to be
                  calibrated
     """
+    # TODO create a issue to discuss if its ok to skip this function call when the noise is 0
+    # if args['noisy_initial_guess'] == [0,0]:
+    #     print("No noise added to transform's initial guess")
+    #     return
+
     if args['sample_seed'] is not None:
         np.random.seed(args['sample_seed'])
 
     nig_trans = args['noisy_initial_guess'][0]
     nig_rot = args['noisy_initial_guess'][1]
 
+
+    # Checking if tf to add noise is also defined in -ntfl, in which case the noise shouldn't be added here
+    # Determining membership in sets is much faster than lists
+    ntfl_tfs = set()
+    if args['noisy_tf_links']:
+        for tf_pair in args['noisy_tf_links']:
+            ntfl_tfs.add(generateKey(tf_pair[0],tf_pair[1]))
+
     # add noise to additional tfs for simulation
     if dataset['calibration_config']['additional_tfs'] is not None:
         for _, additional_tf in dataset['calibration_config']['additional_tfs'].items():
+
             calibration_child = additional_tf['child_link']
             calibration_parent = additional_tf['parent_link']
-            addNoiseToTF(dataset, selected_collection_key, calibration_parent,
-                         calibration_child, nig_trans, nig_rot)
+
+            tf_to_add_noise = generateKey(calibration_parent,calibration_child)
+            if tf_to_add_noise in ntfl_tfs:
+                atomWarn(f'Not adding initial guess noise to {tf_to_add_noise} because its defined in -ntfl')
+                continue
+
+            addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibration_child, nig_trans, nig_rot)
 
     # add noise to sensors tfs for simulation
     for sensor_key, sensor in dataset['sensors'].items():
@@ -858,38 +896,63 @@ def addNoiseToInitialGuess(dataset, args, selected_collection_key):
         if sensor_key != dataset['calibration_config']['anchored_sensor']:
             calibration_child = sensor['calibration_child']
             calibration_parent = sensor['calibration_parent']
-            addNoiseToTF(dataset, selected_collection_key, calibration_parent,
-                         calibration_child, nig_trans, nig_rot)
 
+            tf_to_add_noise = generateKey(calibration_parent,calibration_child)
+            if tf_to_add_noise in ntfl_tfs:
+                atomWarn(f'Not adding initial guess noise to {tf_to_add_noise} because its defined in -ntfl')
+                continue
+
+            addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibration_child, nig_trans, nig_rot)
 
-def addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibration_child, nig_trans, nig_rot):
+# TODO make a basic function called by fixed and multiple
 
+def computeNoise(initial_translation,initial_euler_angles,translation_noise_magnitude,rotation_noise_magnitude):
+    '''
+    Computes both the translation and rotation noise given a certain magnitude and returns the new noisy translation/rotation vectors
+    '''
+    # Translation
+
+    v = np.random.uniform(-1.0, 1.0, 3)
+    v = v / np.linalg.norm(v)
+    translation_delta = v * translation_noise_magnitude
+
+    new_translation = initial_translation + translation_delta
+    # Rotation
+
+    # Its necessary to redefine 'v' to ensure that the translation and rotation noise aren't always the same given the same magnitude
+    v = np.random.uniform(-1.0, 1.0, 3)
+    v = v / np.linalg.norm(v)
+    rotation_delta = v * rotation_noise_magnitude
+
+    new_euler_angles = initial_euler_angles + rotation_delta
+
+    return new_translation,new_euler_angles
+
+
+def addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibration_child, noise_trans, noise_rot):
+
+    print(Fore.RED + 'Transformation parent ' + calibration_parent + ' child ' + calibration_child + Style.RESET_ALL)
     transform_key = generateKey(calibration_parent, calibration_child, suffix='')
 
+    atomWarn(f'Adding noise bigger than 1.0m/rad right now is bugged and might yield unexpected results. Check issue #929 for more information')
+
     # because of #900, and for retrocompatibility with old datasets, we will assume that if the transforms field does
     # not exist in the dataset, then the transformation is fixed
     if 'transforms' not in dataset or dataset['transforms'][transform_key]['type'] == 'fixed':
 
         # Get original transformation
         quat = dataset['collections'][selected_collection_key]['transforms'][transform_key]['quat']
-        translation = dataset['collections'][selected_collection_key]['transforms'][
-            transform_key]['trans']
-
-        # Add noise to the 6 pose parameters
-        v = np.random.uniform(-1.0, 1.0, 3)
-        v = v / np.linalg.norm(v)
-        new_translation = translation + v * nig_trans
+        translation = dataset['collections'][selected_collection_key]['transforms'][transform_key]['trans']
 
-        v = np.random.choice([-1.0, 1.0], 3) * nig_rot
         euler_angles = tf.transformations.euler_from_quaternion(quat)
-        new_angles = euler_angles + v
+
+
+        new_translation,new_euler_angles = computeNoise(translation,euler_angles,noise_trans,noise_rot)
 
         # Replace the original atomic transformations by the new noisy ones
-        new_quat = tf.transformations.quaternion_from_euler(
-            new_angles[0], new_angles[1], new_angles[2])
+        new_quat = tf.transformations.quaternion_from_euler(new_euler_angles[0], new_euler_angles[1], new_euler_angles[2])
         dataset['collections'][selected_collection_key]['transforms'][transform_key]['quat'] = new_quat
-        dataset['collections'][selected_collection_key]['transforms'][transform_key]['trans'] = list(
-            new_translation)
+        dataset['collections'][selected_collection_key]['transforms'][transform_key]['trans'] = list(new_translation)
 
         # Copy randomized transform to all collections
         for collection_key, collection in dataset['collections'].items():
@@ -902,26 +965,18 @@ def addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibrati
 
         for collection_key, collection in dataset["collections"].items():
 
-            # Get original transformation
             quat = dataset['collections'][collection_key]['transforms'][transform_key]['quat']
-            translation = dataset['collections'][collection_key]['transforms'][transform_key][
-                'trans']
+            translation = dataset['collections'][collection_key]['transforms'][transform_key]['trans']
 
-            # Add noise to the 6 pose parameters
-            v = np.random.uniform(-1.0, 1.0, 3)
-            v = v / np.linalg.norm(v)
-            new_translation = translation + v * nig_trans
-
-            v = np.random.choice([-1.0, 1.0], 3) * nig_rot
             euler_angles = tf.transformations.euler_from_quaternion(quat)
-            new_angles = euler_angles + v
+
+            new_translation,new_euler_angles = computeNoise(translation,euler_angles,noise_trans,noise_rot)
+
+            new_quat = tf.transformations.quaternion_from_euler(new_euler_angles[0], new_euler_angles[1], new_euler_angles[2])
 
             # Replace the original atomic transformations by the new noisy ones
-            new_quat = tf.transformations.quaternion_from_euler(
-                new_angles[0], new_angles[1], new_angles[2])
             dataset['collections'][collection_key]['transforms'][transform_key]['quat'] = new_quat
-            dataset['collections'][collection_key]['transforms'][transform_key]['trans'] = list(
-                new_translation)
+            dataset['collections'][collection_key]['transforms'][transform_key]['trans'] = list(new_translation)
 
 
 def copyTFToDataset(calibration_parent, calibration_child, source_dataset, target_dataset):

atom_core/src/atom_core/naming.py

@@ -12,6 +12,8 @@ def generateName(name, prefix='', suffix='', separator='_'):
 def generateKey(parent, child, suffix=''):
     return parent + '-' + child + suffix
 
+def generateCollectionKey(collection_number):
+    return f"{collection_number:03d}"
 
 def generateLabeledTopic(topic, collection_key=None, type='2d', suffix=''):
     """Returns a standarized string for labeled sensor msg topics.

atom_core/src/atom_core/utilities.py

@@ -25,7 +25,7 @@
 from pynput import keyboard
 import yaml
 
-from tf.transformations import quaternion_matrix, euler_from_matrix
+from tf.transformations import quaternion_matrix, euler_from_matrix, euler_from_quaternion
 
 from atom_core.naming import generateKey
 from atom_core.system import execute, removeColorsFromText
@@ -53,6 +53,7 @@ def compareAtomTransforms(transform_1, transform_2):
         transform_2: transformation 2
     """
 
+
     # Create a 4x4 transformation for transform_1
     t1 = quaternion_matrix(transform_1['quat'])
     t1[0:3, 3] = transform_1['trans']
@@ -61,21 +62,33 @@ def compareAtomTransforms(transform_1, transform_2):
     t2 = quaternion_matrix(transform_2['quat'])
     t2[0:3, 3] = transform_2['trans']
 
-    # Method: We will use the following method. If T1 and T2 are the same, then multiplying one by the inverse of the other will produce and identity matrix, with zero translation and rotation. So we will do the multiplication and then evaluation the amount of rotation and translation in the resulting matrix.
+    v = t2[0:3, 3] - t1[0:3, 3]
+
+    # Method: We will use the following method. If T1 and T2 are the same, then multiplying one by the inverse of the other will produce and identity matrix, with zero translation and rotation. So we will do the multiplication and then evaluation of the amount of rotation and translation in the resulting matrix.
     # print('Comparing \nt1= ' + str(t1) + ' \n\nt2=' + str(t2))
 
     t_delta = np.dot(np.linalg.inv(t1), t2)
     # print('t_delta = ' + str(t_delta))
 
     rotation_delta = t_delta[0:3, 0:3]
-    roll, pitch, yaw = euler_from_matrix(rotation_delta)
-
+    # roll, pitch, yaw = euler_from_matrix(rotation_delta)
     translation_delta = t_delta[0:3, 3]
-    # print('translation_delta = ' + str(translation_delta))
+
+    euler_angles_init = euler_from_quaternion(transform_1['quat'])
+    euler_angles_final = euler_from_quaternion(transform_2['quat'])
+
+
+    deuler = np.subtract(euler_angles_final,euler_angles_init)
+    rotation_error = np.linalg.norm(deuler)
 
     # global metrics
-    translation_error = float(abs(np.average(translation_delta)))
-    rotation_error = float(np.average([abs(roll), abs(pitch), abs(yaw)]))
+    # translation_error = float(abs(np.average(translation_delta)))
+    translation_error = np.linalg.norm(translation_delta)
+    # rotation_error = np.linalg.norm([roll, pitch, yaw])
+
+
+    # print('translation error = ' + str(translation_error))
+    # print('rotation error = ' + str(rotation_error))
 
     return translation_error, rotation_error
 
@@ -187,7 +200,7 @@ def printComparisonToGroundTruth(
     if dataset['calibration_config']['additional_tfs'] is not None:
         for additional_tf_key, additional_tf in dataset['calibration_config']['additional_tfs'].items():
 
-            transform_key = generateKey(additional_tf["parent_link"], sensor["child_link"])
+            transform_key = generateKey(additional_tf["parent_link"], additional_tf["child_link"])
             row = [transform_key, Fore.LIGHTCYAN_EX + additional_tf_key + Style.RESET_ALL]
 
 

atom_evaluation/scripts/other_calibrations/cv_eye_to_hand.py

@@ -26,7 +26,7 @@
 
 from atom_calibration.calibration.visualization import getCvImageFromCollectionSensor
 from atom_core.atom import getChain, getTransform
-from atom_core.dataset_io import addNoiseToInitialGuess, filterCollectionsFromDataset, loadResultsJSON, saveAtomDataset, addNoiseToJointParameters
+from atom_core.dataset_io import addNoiseToInitialGuess, filterCollectionsFromDataset, loadResultsJSON, saveAtomDataset, addBiasToJointParameters
 from atom_core.geometry import matrixToTranslationRotation, translationRotationToTransform, traslationRodriguesToTransform, translationQuaternionToTransform
 from atom_core.naming import generateKey
 from atom_core.transformations import compareTransforms
@@ -136,7 +136,7 @@ def main():
     # --- Add noise to the joint parameters to be calibrated.
     # ---------------------------------------
     addNoiseToInitialGuess(dataset, args, selected_collection_key)
-    addNoiseToJointParameters(dataset, args)
+    addBiasToJointParameters(dataset, args)
 
     # Apply new joint values to the tfs
     if args['joint_bias_names'] is not None:

atom_evaluation/scripts/other_calibrations/cv_eye_to_hand_robot_world.py

@@ -26,7 +26,7 @@
 
 from atom_calibration.calibration.visualization import getCvImageFromCollectionSensor
 from atom_core.atom import getChain, getTransform
-from atom_core.dataset_io import addNoiseToInitialGuess, filterCollectionsFromDataset, loadResultsJSON, saveAtomDataset, addNoiseToJointParameters
+from atom_core.dataset_io import addNoiseToInitialGuess, filterCollectionsFromDataset, loadResultsJSON, saveAtomDataset, addBiasToJointParameters
 from atom_core.geometry import matrixToTranslationRotation, translationRotationToTransform, traslationRodriguesToTransform, translationQuaternionToTransform
 from atom_core.naming import generateKey
 from atom_core.transformations import compareTransforms
@@ -136,7 +136,7 @@ def main():
     # --- Add noise to the joint parameters to be calibrated.
     # ---------------------------------------
     addNoiseToInitialGuess(dataset, args, selected_collection_key)
-    addNoiseToJointParameters(dataset, args)
+    addBiasToJointParameters(dataset, args)
 
     # Apply new joint values to the tfs
     if args['joint_bias_names'] is not None: