[Tools]: add support to lidar calibration with localization pose

modules/tools/sensor_calibration/extract_data.py

@@ -36,7 +36,7 @@ from cyber.proto import record_pb2
 from configuration_yaml_generator import ConfigYaml
 from extract_static_data import get_subfolder_list, select_static_image_pcd
 from modules.tools.sensor_calibration.proto import extractor_config_pb2
-from sensor_msg_extractor import GpsParser, ImageParser, PointCloudParser
+from sensor_msg_extractor import GpsParser, ImageParser, PointCloudParser, PoseParser
 
 #from scripts.record_bag import SMALL_TOPICS
 
@@ -108,7 +108,7 @@ def get_sensor_channel_list(record_file):
     """Get the channel list of sensors for calibration."""
     record_reader = RecordReader(record_file)
     return set(channel_name for channel_name in record_reader.get_channellist()
-               if 'sensor' in channel_name)
+               if 'sensor' in channel_name or '/localization/pose' in channel_name)
 
 def validate_channel_list(channels, dictionary):
     ret = True
@@ -131,6 +131,8 @@ def build_parser(channel, output_path):
         parser = PointCloudParser(output_path=output_path, instance_saving=True)
     elif channel.endswith("/gnss/odometry"):
         parser = GpsParser(output_path=output_path, instance_saving=False)
+    elif channel.endswith("/localization/pose"):
+        parser = PoseParser(output_path=output_path, instance_saving=False)
     else:
         raise ValueError("Not Support this channel type: %s" %channel)
 
@@ -368,8 +370,8 @@ def reorganize_extracted_data(tmp_data_path, task_name, remove_input_data_cache=
     if task_name == 'lidar_to_gnss':
         print (get_subfolder_list(tmp_data_path))
         subfolders = [x for x in get_subfolder_list(tmp_data_path)
-                    if '_apollo_sensor_' in x]
-        odometry_subfolders = [x for x in subfolders if'_odometry' in x]
+                    if '_apollo_sensor_' in x or '_localization_pose']
+        odometry_subfolders = [x for x in subfolders if '_odometry' in x or '_pose' in x]
         lidar_subfolders = [x for x in subfolders if '_PointCloud2' in x]
         print(lidar_subfolders)
         print(odometry_subfolders)
@@ -405,11 +407,11 @@ def reorganize_extracted_data(tmp_data_path, task_name, remove_input_data_cache=
         # data selection.
         pair_data_folder_name = 'camera-lidar-pairs'
         cameras, lidar = select_static_image_pcd(path=tmp_data_path,
-                                    min_distance=5, stop_times=5,
+                                    min_distance=5, stop_times=4,
                                     wait_time=3, check_range=50,
                                     image_static_diff_threshold=0.005,
-                                    image_suffix='.png', pcd_suffix='.pcd',
-                                    output_folder_name=pair_data_folder_name)
+                                    output_folder_name=pair_data_folder_name,
+                                    image_suffix='.jpg', pcd_suffix='.pcd')
         lidar_name = get_substring(str=lidar, prefix='_apollo_sensor_', suffix='_PointCloud2')
         for camera in cameras:
             camera_name = get_substring(str=camera, prefix='_apollo_sensor_', suffix='_image')
@@ -512,9 +514,7 @@ def main():
 
     ret = extract_data(valid_record_list, output_abs_path, channels,
                        start_timestamp, end_timestamp, extraction_rates)
-    if not ret:
-        raise ValueError('Failed to extract data!')
-
+    # output_abs_path='/apollo/data/extracted_data/CoolHigh-2019-09-20/lidar_to_gnss-2019-12-10-15-56/tmp'
     reorganize_extracted_data(tmp_data_path=output_abs_path,
                             task_name=config.io_config.task_name)
     # generate_compressed_file(input_path=config.io_config.output_path,

modules/tools/sensor_calibration/extract_static_data.py

@@ -40,7 +40,8 @@ from modules.tools.sensor_calibration.proto import extractor_config_pb2
 CYBER_PATH = os.environ['CYBER_PATH']
 CYBER_RECORD_HEADER_LENGTH = 2048
 
-Z_FILL_LEN = 5
+Z_FILL_LEN = 4
+Z_DEFAULT_LEN = 5
 
 def mkdir_p(path):
     if not os.path.isdir(path):
@@ -139,7 +140,7 @@ def get_difference_score_between_images(path, file_indexes,
     image_diff = np.zeros(len(file_indexes), dtype=np.float32)
     image_thumbnails = []
     for c, idx in enumerate(file_indexes):
-        image_file = os.path.join(path, str(int(idx)).zfill(Z_FILL_LEN) + suffix)
+        image_file = os.path.join(path, str(int(idx)).zfill(Z_DEFAULT_LEN) + suffix)
         image = cv2.imread(image_file)
         image_thumbnails.append(cv2.resize(image,
             (thumbnail_size, thumbnail_size),interpolation=cv2.INTER_AREA))
@@ -164,7 +165,7 @@ def check_static_by_odometry(data, index, check_range=40,
     start_idx = np.maximum(index - check_range, 0)
     end_idx = np.minimum(index + check_range, data.shape[0]-1)
     #  skip if start and end index are too nearby.
-    if end_idx - start_idx <= check_range:
+    if end_idx - start_idx < 2 * check_range:
         return False
     #  calculate x-y plane distance
     distance = get_distance_by_odometry(data, start_idx, end_idx)
@@ -174,12 +175,15 @@ def check_static_by_odometry(data, index, check_range=40,
 def select_static_image_pcd(path, min_distance=5, stop_times=5,
                             wait_time=3, check_range=50,
                             image_static_diff_threshold=0.005,
-                            image_suffix='.png', pcd_suffix='.pcd',
-                            output_folder_name='camera-lidar-pairs'):
-    """select pairs of images and pcds"""
+                            output_folder_name='camera-lidar-pairs',
+                            image_suffix='.jpg', pcd_suffix='.pcd'):
+    """
+    select pairs of images and pcds, odometry information may
+    come from /apollolocalization/pose as well
+    """
     subfolders = [x for x in get_subfolder_list(path) if '_apollo_sensor_' in x]
     lidar_subfolder = [x for x in subfolders if '_PointCloud2' in x]
-    odometry_subfolder = [x for x in subfolders if'_odometry' in x]
+    odometry_subfolder = [x for x in subfolders if'_odometry' in x  or '_pose' in x]
     camera_subfolders = [x for x in subfolders if'_image' in x]
     if len(lidar_subfolder) is not 1 or \
         len(odometry_subfolder) is not 1:
@@ -223,7 +227,7 @@ def select_static_image_pcd(path, min_distance=5, stop_times=5,
         camera_folder_path = os.path.join(path, camera)
         print('foder: {}'.format(camera_folder_path))
         camera_diff = get_difference_score_between_images(
-            camera_folder_path, timestamp_dict[camera][:,0])
+            camera_folder_path, timestamp_dict[camera][:,0], suffix=image_suffix)
         valid_image_indexes =  [x for x, v in enumerate(camera_diff)
                                 if v <= image_static_diff_threshold]
         valid_images = (timestamp_dict[camera][valid_image_indexes, 0]).astype(int)
@@ -242,6 +246,7 @@ def select_static_image_pcd(path, min_distance=5, stop_times=5,
                 if last_idx is -1:
                     last_idx = i
                     last_odometry_idx = odometry_idx
+                    candidate_idx.append(i)
                     continue
                 time_interval = timestamp_dict[camera][i,1] - timestamp_dict[camera][last_idx, 1]
                 odomerty_interval = \
@@ -276,13 +281,13 @@ def select_static_image_pcd(path, min_distance=5, stop_times=5,
         mkdir_p(output_path)
         for count, i in enumerate(image_idx):
             #  save images
-            in_file = os.path.join(camera_folder_path, str(int(i)).zfill(Z_FILL_LEN) + image_suffix)
-            out_file = os.path.join(output_path, str(int(count)).zfill(Z_FILL_LEN) + image_suffix)
+            in_file = os.path.join(camera_folder_path, str(int(i)).zfill(Z_DEFAULT_LEN) + image_suffix)
+            out_file = os.path.join(output_path, str(int(count)).zfill(Z_FILL_LEN) + '_00' + image_suffix)
             copyfile(in_file, out_file)
             j = camera_lidar_nearest_pairs[i]
             #  save pcd
-            in_file = os.path.join(path, lidar_subfolder, str(int(j)).zfill(Z_FILL_LEN) + pcd_suffix)
-            out_file = os.path.join(output_path, str(int(count)).zfill(Z_FILL_LEN) + pcd_suffix)
+            in_file = os.path.join(path, lidar_subfolder, str(int(j)).zfill(Z_DEFAULT_LEN) + pcd_suffix)
+            out_file = os.path.join(output_path, str(int(count)).zfill(Z_FILL_LEN) + '_00' + pcd_suffix)
             copyfile(in_file, out_file)
             print("generate image-lidar-pair:[%d, %d]" % (i, j))
     return camera_subfolders, lidar_subfolder
@@ -324,8 +329,7 @@ def main():
 
     select_static_image_pcd(path=args.workspace, min_distance=5, stop_times=5,
                             wait_time=3, check_range=50,
-                            image_static_diff_threshold=0.005,
-                            image_suffix='.png', pcd_suffix='.pcd')
+                            image_static_diff_threshold=0.005)
 
 if __name__ == '__main__':
     main()

modules/tools/sensor_calibration/sensor_msg_extractor.py

@@ -29,6 +29,7 @@ import struct
 from modules.drivers.proto import sensor_image_pb2
 from modules.drivers.proto import pointcloud_pb2
 from modules.localization.proto import gps_pb2
+from modules.localization.proto import localization_pb2
 
 from data_file_object import TimestampFileObject, OdometryFileObject
 
@@ -121,13 +122,47 @@ class GpsParser(SensorMessageParser):
         odometry_file_obj.save_to_file(self._parsed_data)
         return True
 
+class PoseParser(GpsParser):
+    """
+    inherit similar data saver and data structure from GpsParser
+    save the ego-localization information same as odometry
+    """
+    def _init_parser(self):
+        self._msg_parser = localization_pb2.LocalizationEstimate()
+
+    def parse_sensor_message(self, msg):
+        """ parse localization information from localization estimate channel"""
+        loc_est = self._msg_parser
+        loc_est.ParseFromString(msg.message)
+
+        # all double, except point_type is int32
+        ts = loc_est.header.timestamp_sec
+        self._timestamps.append(ts)
+
+        point_type = 56
+        qw = loc_est.pose.orientation.qw
+        qx = loc_est.pose.orientation.qx
+        qy = loc_est.pose.orientation.qy
+        qz = loc_est.pose.orientation.qz
+        x = loc_est.pose.position.x
+        y = loc_est.pose.position.y
+        z = loc_est.pose.position.z
+        # save 9 values as a tuple, for eaisier struct packing during storage
+        if self._instance_saving:
+            raise ValueError("localization--pseudo odometry-- should be saved in a file")
+        else:
+            self._parsed_data.append((ts, point_type, qw, qx, qy, qz, x, y, z))
+
+        return True
+
 class PointCloudParser(SensorMessageParser):
     """
     class to parse apollo/$(lidar)/PointCloud2 channels.
     saving separately each parsed msg
     """
-    def __init__(self, output_path, instance_saving=True):
+    def __init__(self, output_path, instance_saving=True, suffix='.pcd'):
         super(PointCloudParser, self).__init__(output_path, instance_saving)
+        self._suffix = suffix
 
     def convert_xyzit_pb_to_array(self, xyz_i_t, data_type):
         arr = np.zeros(len(xyz_i_t), dtype=data_type)
@@ -189,7 +224,7 @@ class PointCloudParser(SensorMessageParser):
         self._parsed_data = self.make_xyzit_point_cloud(pointcloud.point)
 
         if self._instance_saving:
-            file_name = "%05d.pcd" % self.get_msg_count()
+            file_name = "%05d" % self.get_msg_count() + self._suffix
             output_file = os.path.join(self._output_path, file_name)
             self.save_pointcloud_meta_to_file(pc_meta=self._parsed_data, pcd_file=output_file)
         else:
@@ -203,9 +238,9 @@ class ImageParser(SensorMessageParser):
     class to parse apollo/$(camera)/image channels.
     saving separately each parsed msg
     """
-    def __init__(self, output_path, instance_saving=True):
+    def __init__(self, output_path, instance_saving=True, suffix='.jpg'):
         super(ImageParser, self).__init__(output_path, instance_saving)
-
+        self._suffix = suffix
     def _init_parser(self):
         self._msg_parser = sensor_image_pb2.Image()
 
@@ -242,7 +277,7 @@ class ImageParser(SensorMessageParser):
             (image.height, image.width, channel_num))
 
         if self._instance_saving:
-            file_name = "%05d.png" % self.get_msg_count()
+            file_name = "%05d" % self.get_msg_count() + self._suffix
             output_file = os.path.join(self._output_path, file_name)
             self.save_image_mat_to_file(image_file=output_file)
         else: