Merge pull request #568 from lardemua:JorgeFernandes-Git/issue543

orgeFernandes-Git/issue543

Author: miguelriemoliveira

atom_calibration/scripts/calibrate

@@ -26,7 +26,7 @@ from atom_core.dataset_io import (addNoiseToInitialGuess, checkIfAtLeastOneLabel
                                   filterCollectionsFromDataset, filterSensorsFromDataset, loadResultsJSON,
                                   saveResultsJSON)
 from atom_core.naming import generateName, generateKey
-from atom_core.utilities import waitForKeyPress2
+from atom_core.utilities import waitForKeyPress2, checkAdditionalTfs
 from atom_core.xacro_io import saveResultsXacro
 
 
@@ -91,6 +91,9 @@ def main():
                     help="Draw a ghost mesh with the systems initial pose. Good for debugging.")
     ap.add_argument("-oj", "--output_json", help="Full path to output json file.",
                     type=str, required=False, default=None)
+    
+    # save results of calibration in a csv file
+    ap.add_argument("-sfr", "--save_file_results", help="Output folder to where the results will be stored.", type=str, required=False)
 
     # Roslaunch adds two arguments (__name and __log) that break our parser. Lets remove those.
     arglist = [x for x in sys.argv[1:] if not x.startswith("__")]
@@ -199,6 +202,11 @@ def main():
         transform_key = generateKey(sensor["calibration_parent"], sensor["calibration_child"])
         transforms_set.add(transform_key)
 
+    if checkAdditionalTfs(dataset):
+        for _, additional_tf in dataset['calibration_config']['additional_tfs'].items():
+            transform_key = generateKey(additional_tf['parent_link'], additional_tf['child_link'])
+            transforms_set.add(transform_key)            
+
     for transform_key in transforms_set:  # push six parameters for each transform to be optimized.
         initial_transform = getterTransform(dataset, transform_key=transform_key,
                                             collection_name=selected_collection_key)
@@ -321,9 +329,17 @@ def main():
                 continue
 
             # Sensor related parameters
-            sensors_transform_key = generateKey(
-                sensor["calibration_parent"], sensor["calibration_child"])
-            params = opt.getParamsContainingPattern(sensors_transform_key)
+            # sensors_transform_key = generateKey(
+            #     sensor["calibration_parent"], sensor["calibration_child"])
+            # params = opt.getParamsContainingPattern(sensors_transform_key)
+
+            # Issue #543: Create the list of transformations that influence this residual by analyzing the transformation chain.
+            params = []
+            transforms_list = list(transforms_set) # because not sure if we can use a set for some of the next steps
+            for transform_in_chain in sensor['chain']:
+                transform_key = generateKey(transform_in_chain["parent"], transform_in_chain["child"])
+                if transform_key in transforms_list:
+                    params.extend(opt.getParamsContainingPattern(transform_key))
 
             # Intrinsics parameters
             if sensor["modality"] == "rgb" and args["optimize_intrinsics"]:
@@ -434,14 +450,14 @@ def main():
     options = {"ftol": 1e-4, "xtol": 1e-4, "gtol": 1e-4, "diff_step": None, "x_scale": "jac", }
     # options = {'ftol': 1e-5, 'xtol': 1e-5, 'gtol': 1e-5, 'diff_step': None, 'x_scale': 'jac'}
     # options = {'ftol': 1e-6, 'xtol': 1e-6, 'gtol': 1e-6, 'diff_step': None, 'x_scale': 'jac'}
-    # options = {'ftol': 1e-8, 'xtol': 1e-8, 'gtol': 1e-8, 'diff_step': None, 'x_scale': 'jac'o
+    # options = {'ftol': 1e-8, 'xtol': 1e-8, 'gtol': 1e-8, 'diff_step': None, 'x_scale': 'jac'}
     opt.startOptimization(optimization_options=options)
 
     if args["phased_execution"]:
         waitForKeyPress2(opt.callObjectiveFunction, timeout=0.1,
                          message=Fore.BLUE + "Optimization finished" + Style.RESET_ALL)
     # Final error report
-    errorReport(dataset=dataset, residuals=objectiveFunction(opt.data_models), normalizer=normalizer)
+    errorReport(dataset=dataset, residuals=objectiveFunction(opt.data_models), normalizer=normalizer, args=args)
 
     # ---------------------------------------
     # --- Save updated JSON file
@@ -462,7 +478,8 @@ def main():
     # ---------------------------------------
     # --- Save updated xacro
     # ---------------------------------------
-    saveResultsXacro(dataset, selected_collection_key)
+    # saveResultsXacro(dataset, selected_collection_key)
+    saveResultsXacro(dataset, selected_collection_key, list(transforms_set))
 
 
 if __name__ == "__main__":

atom_calibration/scripts/collect_data

@@ -122,7 +122,7 @@ if __name__ == "__main__":
                          'returns True or False to indicate if the sensor should be labelled.'
                          ' The Syntax is lambda name: f(x), where f(x) is the function in python '
                          'language. Example: lambda name: name in ["lidar1", "camera2"] , to avoid labeling of sensors'
-                         ' lida1 and camera2.')
+                         ' lidar1 and camera2.')
 
     args = vars(ap.parse_args(args=atom_core.ros_utils.filterLaunchArguments(sys.argv)))
     print("\nArgument list=" + str(args) + '\n')

atom_calibration/scripts/configure_calibration_pkg

@@ -248,6 +248,49 @@ if __name__ == "__main__":
         for frame in ['child_link', 'parent_link', 'link']:
             frames_to_verify[sensor_key + '.' + frame] = config['sensors'][sensor_key][frame]
 
+    # Verify if exist additional tfs to be estimated
+    if 'additional_tfs' in config: 
+        if config['additional_tfs'] != '':
+            # Check if link, child_link and parent_link exist in the bagfile.
+            for additional_tf_key in config['additional_tfs']:
+                for frame in ['child_link', 'parent_link']:
+                    frames_to_verify[additional_tf_key + '.' +
+                                    frame] = config['additional_tfs'][additional_tf_key][frame]
+
+            # Verify that the parent and child links for each additional_tfs are a direct transformation
+            for additional_tf_key, additional_tf in config['additional_tfs'].items():
+                edge_in_config = (config['additional_tfs'][additional_tf_key]['parent_link'],
+                                config['additional_tfs'][additional_tf_key]['child_link'])
+
+                if not edge_in_config in gx.edges():
+                    raise ValueError(
+                        'Error: additional_tfs ' + Fore.BLUE + additional_tf_key + Style.RESET_ALL + ' parent and child links are ' + Fore.YELLOW +
+                        edge_in_config[0] + Fore.BLACK + ' and ' + Fore.YELLOW + edge_in_config[1] + Fore.BLACK +
+                        ' but there is no such atomic transformation in the tf tree.' + Style.RESET_ALL)
+
+            for additional_tf_key, additional_tf in config['additional_tfs'].items():
+                # path between root and additional_tfs data frame
+                paths = nx.shortest_path(gx, config['world_link'], additional_tf['child_link'])
+                print('Path from ' + Fore.RED + config['world_link'] + Fore.RESET + ' to additional_tfs ' + Fore.LIGHTMAGENTA_EX +
+                    additional_tf_key + Fore.RESET + ':')
+                at = '['
+                for path in paths:
+                    if path == config['world_link']:
+                        at += Fore.RED + path + Fore.RESET + ', '
+                    elif path == additional_tf['parent_link'] or path == additional_tf['child_link']:
+                        at += Fore.BLUE + path + Fore.RESET + ', '
+                    else:
+                        for joint in description.joints:  # atomic transformations are given by the joints
+                            if path == joint.parent or path == joint.child:
+                                if joint.type == 'fixed':
+                                    at += path + ', '
+                                else:
+                                    at += Fore.GREEN + path + Fore.RESET + ', '
+                                break
+                at = at[:-2]
+                at += ']'
+                print(at)
+
     print('Checking if all tf frames exist ...')
     print(frames_to_verify)
     for key, frame in frames_to_verify.items():
@@ -350,6 +393,13 @@ if __name__ == "__main__":
                 label += 'To be calibrated\n'
                 rgb = matplotlib.colors.rgb2hex([0, 0, 1])
 
+        if 'additional_tfs' in config: 
+            if config['additional_tfs'] != '':
+                for additional_tfs_key, additional_tfs in config['additional_tfs'].items():
+                    if additional_tfs['parent_link'] == parent and additional_tfs['child_link'] == child:
+                        label += 'To be calibrated\n'
+                        rgb = matplotlib.colors.rgb2hex([0, 0, 1])
+                        
         label += type
 
         g.edge(parent, child, color=rgb, style='solid', _attributes={'penwidth': '1', 'fontcolor': rgb},

atom_calibration/scripts/set_initial_estimate

@@ -22,6 +22,8 @@ from urdf_parser_py.urdf import Pose as URDFPose
 from atom_core.ros_utils import filterLaunchArguments
 from atom_core.config_io import loadConfig
 from atom_calibration.initial_estimate.sensor import Sensor
+from atom_calibration.initial_estimate.additional_tf import Additional_tf
+
 
 
 class InteractiveFirstGuess(object):
@@ -31,6 +33,7 @@ class InteractiveFirstGuess(object):
         self.args = args  # command line arguments
         self.config = None  # calibration config
         self.sensors = []  # sensors
+        self.additional_tfs = []  # additional_tfs
         self.urdf = None  # robot URDF
         self.server = None  # interactive markers server
         self.menu = None
@@ -54,13 +57,30 @@ class InteractiveFirstGuess(object):
 
         print('Number of sensors: ' + str(len(self.config['sensors'])))
 
+        if self.checkAdditionalTfs():
+            print('Number of additional_tfs: ' +
+                str(len(self.config['additional_tfs'])))
+
         # Init interaction
         self.server = InteractiveMarkerServer('set_initial_estimate')
         self.menu = MenuHandler()
 
         self.createInteractiveMarker(self.config['world_link'])
-        self.menu.insert("Save sensors configuration", callback=self.onSaveInitialEstimate)
-        self.menu.insert("Reset all sensors to initial configuration", callback=self.onResetAll)
+
+        if self.checkAdditionalTfs():
+            # sensors and additional tfs to be calibrated
+            self.menu.insert("Save sensors and additional transformations configuration",
+                            callback=self.onSaveInitialEstimate)
+            self.menu.insert(
+                    "Reset all sensors and additional transformations to initial configuration", callback=self.onResetAll)
+                
+        else:
+            # only sensors to be calibrated
+            self.menu.insert("Save sensors configuration",
+                callback=self.onSaveInitialEstimate)
+            self.menu.insert(
+                "Reset all sensors to initial configuration", callback=self.onResetAll)
+
         self.menu.reApply(self.server)
 
         # Waiting for the loading of the bagfile
@@ -69,6 +89,11 @@ class InteractiveFirstGuess(object):
         # Create a colormap so that we have one color per sensor
         self.cm_sensors = cm.Pastel2(np.linspace(0, 1, len(self.config['sensors'].keys())))
 
+        # Create a colormap so that we have one color per additional_tfs
+        if self.checkAdditionalTfs():
+            self.cm_additional_tf = cm.Pastel2(np.linspace(
+                0, 1, len(self.config['additional_tfs'].keys())))
+
         # For each node generate an interactive marker.
         sensor_idx = 0
         for name, sensor in self.config['sensors'].items():
@@ -87,6 +112,24 @@ class InteractiveFirstGuess(object):
             sensor_idx += 1
             print('... done')
 
+        # For each node generate an interactive marker.
+        if self.checkAdditionalTfs():
+            additional_tf_idx = 0
+            for name, additional_tf in self.config['additional_tfs'].items():
+                print(Fore.BLUE + '\nAdditional_tfs name is ' +
+                    name + Style.RESET_ALL)
+                params = {
+                    "frame_world": self.config['world_link'],
+                    "frame_opt_parent": additional_tf['parent_link'],
+                    "frame_opt_child": additional_tf['child_link'],
+                    "frame_additional_tf": additional_tf['child_link'],
+                    "additional_tf_color": tuple(self.cm_additional_tf[additional_tf_idx, :]),
+                    "marker_scale": self.args['marker_scale']}
+                # Append to the list of additional_tfs
+                self.additional_tfs.append(Additional_tf(name, self.server, self.menu, **params))
+                additional_tf_idx += 1
+                print('... done')
+
         self.server.applyChanges()
 
     def onSaveInitialEstimate(self, feedback):
@@ -115,10 +158,33 @@ class InteractiveFirstGuess(object):
             print("Writing fist guess urdf to '{}'".format(outfile))
             out.write(self.urdf.to_xml_string())
 
+        if self.checkAdditionalTfs():
+            for additional_tfs in self.additional_tfs:
+                # find corresponding joint for this additional_tfs
+                for joint in self.urdf.joints:
+                    if additional_tfs.opt_child_link == joint.child and additional_tfs.opt_parent_link == joint.parent:
+                        trans = additional_tfs.optT.getTranslation()
+                        euler = additional_tfs.optT.getEulerAngles()
+                        joint.origin.xyz = list(trans)
+                        joint.origin.rpy = list(euler)
+
+            # Write the urdf file with atom_calibration's
+            # source path as base directory.
+            rospack = rospkg.RosPack()
+            # outfile = rospack.get_path('atom_calibration') + os.path.abspath('/' + self.args['filename'])
+            outfile = self.args['filename']
+            with open(outfile, 'w') as out:
+                print("Writing fist guess urdf to '{}'".format(outfile))
+                out.write(self.urdf.to_xml_string())
+
     def onResetAll(self, feedback):
         for sensor in self.sensors:
             sensor.resetToInitalPose()
 
+        if self.checkAdditionalTfs():
+            for additional_tfs in self.additional_tfs:
+                additional_tfs.resetToInitalPose()
+
     def createInteractiveMarker(self, world_link):
         marker = InteractiveMarker()
         marker.header.frame_id = world_link
@@ -186,8 +252,19 @@ class InteractiveFirstGuess(object):
         marker.controls.append(control)
 
         self.server.insert(marker, self.onSaveInitialEstimate)
+
+        if self.checkAdditionalTfs():
+            self.server.insert(marker, self.onSaveInitialEstimate)
+        
+
         self.menu.apply(self.server, marker.name)
 
+    def checkAdditionalTfs(self):
+        if 'additional_tfs' in self.config: 
+            if self.config['additional_tfs'] != '':
+                return True
+            else:
+                return False
 
 if __name__ == "__main__":
     # Parse command line arguments

atom_calibration/src/atom_calibration/calibration/objective_function.py

@@ -34,7 +34,7 @@
 # -------------------------------------------------------------------------------
 # --- FUNCTIONS
 # -------------------------------------------------------------------------------
-def errorReport(dataset, residuals, normalizer):
+def errorReport(dataset, residuals, normalizer, args):
 
     from prettytable import PrettyTable
     table_header = ['Collection']
@@ -56,28 +56,35 @@ def errorReport(dataset, residuals, normalizer):
             table_header.append(sensor_key + units)
 
     table = PrettyTable(table_header)
+    table_to_save = PrettyTable(table_header) # table to save. This table was created, because the original has colors and the output csv save them as random characters
 
     # Build each row in the table
     keys = sorted(dataset['collections'].keys(), key=lambda x: int(x))
     for collection_key in keys:
         row = [collection_key]
+        row_save = [collection_key]
         v = []
         for sensor_key, sensor in dataset['sensors'].items():
             keys = [k for k in residuals.keys() if ('c' + collection_key) == k.split('_')[0] and sensor_key in k]
             v = [residuals[k] * normalizer[sensor['modality']] for k in keys if residuals[k]]
             if v:
                 value = '%.4f' % np.mean(v)
                 row.append(value)
+                row_save.append(value)
             else:
                 row.append(Fore.LIGHTBLACK_EX + '---' + Style.RESET_ALL)
+                row_save.append('---')
 
         table.add_row(row)
+        table_to_save.add_row(row)
 
     # Compute averages and add a bottom row
     bottom_row = []  # Compute averages and add bottom row to table
+    bottom_row_save = []
     for col_idx, _ in enumerate(table_header):
         if col_idx == 0:
             bottom_row.append(Fore.BLUE + Style.BRIGHT + 'Averages' + Fore.BLACK + Style.NORMAL)
+            bottom_row_save.append('Averages')
             continue
 
         total = 0
@@ -97,8 +104,10 @@ def errorReport(dataset, residuals, normalizer):
             value = '---'
 
         bottom_row.append(Fore.BLUE + value + Fore.BLACK)
+        bottom_row_save.append(value)
 
     table.add_row(bottom_row)
+    table_to_save.add_row(bottom_row_save)
 
     # Put larger errors in red per column (per sensor)
     for col_idx, _ in enumerate(table_header):
@@ -121,11 +130,17 @@ def errorReport(dataset, residuals, normalizer):
         table.rows[max_row_idx][col_idx] = Fore.RED + table.rows[max_row_idx][col_idx] + Style.RESET_ALL
 
     table.align = 'c'
+    table_to_save.align = 'c'
     print(Style.BRIGHT + 'Errors per collection' + Style.RESET_ALL + ' (' + Fore.YELLOW + 'anchored sensor' +
           Fore.BLACK + ', ' + Fore.RED + ' max error per sensor' + Fore.BLACK + ', ' + Fore.LIGHTBLACK_EX +
           'not detected as \"---\")' + Style.RESET_ALL)
     print(table)
 
+    # save results in csv file 
+    if args['save_file_results'] != None: 
+        with open(args['save_file_results'] + 'calibration_results.csv', 'w', newline='') as f_output:
+            f_output.write(table_to_save.get_csv_string())
+
 
 @Cache(args_to_ignore=['_dataset'])
 def getPointsInPatternAsNPArray(_collection_key, _sensor_key, _dataset):
@@ -694,6 +709,6 @@ def objectiveFunction(data):
                 raise ValueError("Unknown sensor msg_type or modality")
 
     if args['verbose'] and data['status']['is_iteration']:
-        errorReport(dataset=dataset, residuals=r, normalizer=normalizer)
+        errorReport(dataset=dataset, residuals=r, normalizer=normalizer, args=args)
 
     return r  # Return the residuals