Gettting noise functionality ready for batch experiments #929 #928 #926

- Bigger noises are still bugged but now gives a atomWarn()
- Encapsulated ntfv in a specific function
- -ntfv now overrides -nig
- addNoiseToTF now calls a subfunction computeNoise() to reduce boilerplate code
- other misc small qol improvements

Author: brunofavs

atom_calibration/scripts/calibrate

@@ -27,6 +27,7 @@ from atom_calibration.calibration.visualization import setupVisualization, visua
 from atom_core.dataset_io import (addNoiseToInitialGuess,
                                   addBiasToJointParameters,
                                   addNoiseToTF,
+                                  addNoiseFromNoisyTFLinks,
                                   checkIfAtLeastOneLabeledCollectionPerSensor,
                                   filterCollectionsFromDataset,
                                   filterSensorsFromDataset,
@@ -40,7 +41,7 @@ from atom_core.naming import generateName, generateKey
 from atom_core.utilities import (atomError, createLambdaExpressionsForArgs,
                                  waitForKeyPress2,
                                  atomStartupPrint,
-                                 verifyAnchoredSensor,
+                              verifyAnchoredSensor,
                                  printComparisonToGroundTruth, saveCommandLineArgsYml)
 from atom_core.xacro_io import saveResultsXacro
 from atom_core.results_yml_io import saveResultsYml
@@ -53,6 +54,7 @@ from atom_core.config_io import parse_list_of_transformations, mutually_inclusiv
 def signal_handler(sig, frame):
     print("Stopping optimization (Ctrl+C pressed)")
     sys.exit(0)
+    
 
 
 # -------------------------------------------------------------------------------
@@ -111,9 +113,9 @@ def main():
     ap.add_argument("-nig", "--noisy_initial_guess", nargs=2, metavar=("translation", "rotation"),
                     help="Magnitude of noise to add to the initial guess atomic transformations set before starting optimization [meters, radians].", type=float, default=[0.0, 0.0],),
 
-    # TODO make note in help that this overrides nig flag.
-    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 link1:link2,linkA:linkB (links defining a transformation separated by
-                    : and different pairs separeted by ,)''')
+    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")
 
@@ -195,6 +197,7 @@ def main():
     # make a copy before filtering out collections and sensors
     dataset_original = copy.deepcopy(dataset)
 
+
     # ---------------------------------------
     # --- Filter some collections, sensors, joints and additional tfs from the dataset
     # ---------------------------------------
@@ -238,20 +241,9 @@ def main():
     dataset_ground_truth = deepcopy(dataset)  # make a copy before adding noise
 
     addNoiseToInitialGuess(dataset, args, selected_collection_key)
-    # addBiasToJointParameters(dataset, args)
-
-    # TODO Put this encapsulated in a single function
-    # Verify both arguments were provided
-    # Unfortunately, mutually inclusive arguments are not built into argparse
-    # https://github.com/python/cpython/issues/55797
-
-    if mutually_inclusive_conditions(args['noisy_tf_links'], args['noisy_tf_values']):
-        # 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]
+    addBiasToJointParameters(dataset, args)
+    addNoiseFromNoisyTFLinks(dataset,args,selected_collection_key)
 
-        # for tf_pair in args['noisy_tf_links']:
-        # addNoiseToTF(dataset,selected_collection_key,tf_pair[0],tf_pair[1],translation_tf_noise,rotation_tf_noise)
 
     # ---------------------------------------
     # --- SETUP OPTIMIZER: Create data models

atom_core/src/atom_core/dataset_io.py

@@ -24,7 +24,7 @@
 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.config_io import uriReader,mutually_inclusive_conditions
 from atom_core.naming import generateName, generateKey
 from atom_calibration.collect.label_messages import (convertDepthImage32FC1to16UC1, convertDepthImage16UC1to32FC1,
                                                      numpyFromPointCloudMsg)
@@ -782,6 +782,24 @@ def addBiasToJointParameters(dataset, args):
 
 # TODO Create a new function called addNoiseFromNoisyTFLinks
 
+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):
     """
@@ -801,14 +819,26 @@ def addNoiseToInitialGuess(dataset, args, selected_collection_key):
     nig_trans = args['noisy_initial_guess'][0]
     nig_rot = args['noisy_initial_guess'][1]
 
-    # TODO if the transform to get get is also in the args noisy_tf_links, then we should not add noise to it.
-    # skip it. with a warn
+
+    # 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']
+
+            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
@@ -819,8 +849,12 @@ 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']
-            print(calibration_child)
-            print(calibration_parent)
+
+            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)
 
 # TODO make a basic function called by fixed and multiple
@@ -853,6 +887,8 @@ def addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibrati
     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':

atom_core/src/atom_core/utilities.py

@@ -66,16 +66,17 @@ def compareAtomTransforms(transform_1, transform_2):
 
     rotation_delta = t_delta[0:3, 0:3]
     # roll, pitch, yaw = euler_from_matrix(rotation_delta)
+    translation_delta = t_delta[0:3, 3]
 
     import tf
     euler_angles_init = tf.transformations.euler_from_quaternion(transform_1['quat'])
     euler_angles_final = tf.transformations.euler_from_quaternion(transform_2['quat'])
 
 
     deuler = np.subtract(euler_angles_final,euler_angles_init)
+    rotation_error = np.linalg.norm(deuler)
 
 
-    translation_delta = t_delta[0:3, 3]
     # print('translation_delta = ' + str(translation_delta))
     # nor = np.linalg.norm(translation_delta)
     # print('nor = ' + str(nor))
@@ -84,7 +85,6 @@ def compareAtomTransforms(transform_1, transform_2):
     # translation_error = float(abs(np.average(translation_delta)))
     translation_error = np.linalg.norm(translation_delta)
     # rotation_error = np.linalg.norm([roll, pitch, yaw])
-    rotation_error = np.linalg.norm(deuler)
 
     # print(Fore.GREEN + str(translation_error) + Style.RESET_ALL)
     # print(Fore.GREEN + str(translation_error) + Style.RESET_ALL)