├── README.md └── rosbag_creator ├── VIO_bag.py └── LIO_bag .py /README.md: -------------------------------------------------------------------------------- 1 | # CARLA-Loc 2 | 3 | ## Update Nov 2024 4 | 5 | - Dataset is now available at: [Google Drive](https://drive.google.com/drive/folders/1KnKzNv6BcOjhhzZ-AtoHgN09j6znPhCn?usp=share_link) 6 | - Project Page: https://yuhang1008.github.io/CARLA-Loc_page/ 7 | - Arxiv: https://arxiv.org/abs/2309.08909v2 8 | 9 | --- 10 | 11 | The code for converting raw dataset to rosbags has been attached in this repo. You can tune the following parameters to fit your need. 12 | `VIO_bag.py`: 13 | ![image](https://github.com/user-attachments/assets/097eb658-ab5a-4259-be9c-80e7320d3f58) 14 | 15 | `LIO_bag.py`: 16 | ![image](https://github.com/user-attachments/assets/c0c3e7d1-d898-465c-8812-29f6860da668) 17 | 18 | 19 | 20 | 21 | 22 | --- 23 | 24 | 25 | 26 | The source code for customized dataset generation will be released upon acceptance of our paper. 27 | 28 | Please consider to cite our preprint if CARLA-Loc is helpful to you work. Thanks. 29 | ```bibtex 30 | @misc{han2023carlaloc, 31 | title={CARLA-Loc: Synthetic SLAM Dataset with Full-stack Sensor Setup in Challenging Weather and Dynamic Environments}, 32 | author={Yuhang Han and Zhengtao Liu and Shuo Sun and Dongen Li and Jiawei Sun and Chengran Yuan and Marcelo H. Ang Jr}, 33 | year={2023}, 34 | eprint={2309.08909}, 35 | archivePrefix={arXiv}, 36 | primaryClass={cs.RO} 37 | } 38 | ``` 39 | Please contact us at yuhang_han@u.nus.edu if you want to discuss about this work. 40 | -------------------------------------------------------------------------------- /rosbag_creator/VIO_bag.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import time, sys, os, glob, re 4 | from ros import rosbag 5 | import rospy 6 | import roslib 7 | roslib.load_manifest('sensor_msgs') 8 | from sensor_msgs.msg import Imu, Image, NavSatFix 9 | import pandas as pd 10 | from PIL import ImageFile 11 | import argparse 12 | import cv2 as cv 13 | import pickle 14 | from cv_bridge import CvBridge 15 | import sys 16 | bridge = CvBridge() 17 | 18 | 19 | def sort_files_in_folder(folder_path): 20 | # Get a list of all files in the folder 21 | file_list = os.listdir(folder_path) 22 | sorted_files = sorted(file_list, key=lambda x: int(re.findall(r'\d+', x)[0])) 23 | sorted_array = [] 24 | for file in sorted_files: 25 | file_path = os.path.join(folder_path, file) 26 | sorted_array.append(file_path) 27 | 28 | return sorted_array 29 | 30 | def split_float_to_secs_nsecs(float_number): 31 | # Split the number into integer and fractional parts 32 | str_number = str(float_number) 33 | integer_part, decimal_part = str_number.split('.') 34 | decimal_as_nsecs = round(float("0." + decimal_part) * 1000000000) 35 | # Use the built-in `format` function to ensure we get 9 digits 36 | decimal_as_nsecs_str = format(int(decimal_as_nsecs), '09') 37 | # Return the parts as integers 38 | return int(integer_part), int(decimal_as_nsecs_str) 39 | 40 | def main(): 41 | 42 | argparser = argparse.ArgumentParser(description= 'for genarating raw data') 43 | argparser.add_argument('--root_path',type=str,default='/media/lde/yuhang/DynaCARLA',help='dataset root path') 44 | argparser.add_argument('--output_path',type=str,default='/media/lde/yuhang/DynaCARLA/rosbags',help='output path to save ros bag') 45 | args = argparser.parse_args() 46 | 47 | 48 | map_names = ['Town01', 'Town02', 'Town03', 'Town04', 'Town05', 'Town06', 'Town07', 'Town10HD'] 49 | weathers = ['ClearNoon', 'FoggyNoon', 'RainyNight'] 50 | dyna_types = ['dynamic', 'static'] 51 | 52 | 53 | for i in range(8): 54 | map_name = map_names[i] 55 | for weather in weathers: 56 | for dyna_type in dyna_types: 57 | seq_root = os.path.join(args.root_path, map_name) 58 | common_data_path = os.path.join(seq_root, 'common_data.pkl') 59 | cam_root_path = os.path.join(seq_root, weather+'/'+dyna_type) 60 | bag_name = 'VIO_'+map_name+'_'+weather+'_'+dyna_type+'_.bag' 61 | bag_path = os.path.join(args.output_path,map_name,'visual',bag_name) 62 | 63 | with open(common_data_path, 'rb') as f: 64 | common_data = pickle.load(f) 65 | 66 | with rosbag.Bag(bag_path, 'w') as bag: 67 | print('now processing: '+bag_name) 68 | #----------------------imu data-------------------- 69 | # carla: angular veocity in left hand coordinate!!! 70 | print('converting imu data......') 71 | for i in range(len(common_data['imu']['acc'])): 72 | secs, nano_secs = split_float_to_secs_nsecs(common_data['imu']['timestamp'][i]) 73 | timestamp = rospy.Time(secs= secs, nsecs=nano_secs) 74 | imu_msg = Imu() 75 | imu_msg.header.seq = i 76 | if i ==1: 77 | i = 2 78 | imu_msg.header.frame_id = "imu" 79 | imu_msg.header.stamp = timestamp 80 | imu_msg.linear_acceleration.x = float(common_data['imu']['acc'][i][0]) 81 | imu_msg.linear_acceleration.y = float(common_data['imu']['acc'][i][1]) 82 | imu_msg.linear_acceleration.z = float(common_data['imu']['acc'][i][2]) 83 | imu_msg.angular_velocity.x = float(common_data['imu']['gyro'][i][0]) 84 | imu_msg.angular_velocity.y = float(common_data['imu']['gyro'][i][1]) 85 | imu_msg.angular_velocity.z = float(common_data['imu']['gyro'][i][2]) 86 | bag.write("/imu", imu_msg, timestamp) 87 | sys.stdout.write('\r'+str(i)+' of '+str(len(common_data['imu']['acc']))) 88 | sys.stdout.flush() 89 | 90 | #---------------------camera data------------------- 91 | print('converting left cam data......') 92 | print(os.path.join(cam_root_path, 'stereo_l')) 93 | img_paths = sort_files_in_folder(os.path.join(cam_root_path, 'stereo_l')) 94 | for i in range(len(img_paths)): 95 | img = cv.imread(img_paths[i]) 96 | # encoding = "bgr8" 97 | cv_image = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 98 | encoding = "mono8" 99 | image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) 100 | secs, nano_secs = split_float_to_secs_nsecs(common_data['cam_left']['timestamp'][i]) 101 | timestamp = rospy.Time(secs= secs, nsecs=nano_secs) 102 | image_message.header.stamp = timestamp 103 | image_message.header.frame_id = 'cam_left' 104 | image_message.header.seq = i 105 | bag.write('/cam0_raw', image_message, timestamp) 106 | sys.stdout.write('\r'+str(i)+' of '+str(len(img_paths))) 107 | sys.stdout.flush() 108 | 109 | print('converting right cam data......') 110 | img_paths = sort_files_in_folder(os.path.join(cam_root_path, 'stereo_r')) 111 | for i in range(len(img_paths)): 112 | img = cv.imread(img_paths[i]) 113 | img = cv.imread(img_paths[i]) 114 | # encoding = "bgr8" 115 | cv_image = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 116 | encoding = "mono8" 117 | image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) 118 | secs, nano_secs = split_float_to_secs_nsecs(common_data['cam_right']['timestamp'][i]) 119 | timestamp = rospy.Time(secs= secs, nsecs=nano_secs) 120 | image_message.header.stamp = timestamp 121 | image_message.header.frame_id = 'cam_right' 122 | image_message.header.seq = i 123 | bag.write('/cam1_raw', image_message, timestamp) 124 | sys.stdout.write('\r'+str(i)+' of '+str(len(img_paths))) 125 | sys.stdout.flush() 126 | 127 | bag.close() 128 | 129 | if __name__ == '__main__': 130 | main() 131 | -------------------------------------------------------------------------------- /rosbag_creator/LIO_bag .py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import sys, os, re 4 | from ros import rosbag 5 | import rospy 6 | import roslib 7 | roslib.load_manifest('sensor_msgs') 8 | from sensor_msgs.msg import PointCloud2, Imu, PointField 9 | from numpy.lib.recfunctions import unstructured_to_structured 10 | import std_msgs.msg as std_msgs 11 | import argparse 12 | from tqdm import tqdm 13 | import array 14 | import pickle 15 | 16 | def split_float_to_secs_nsecs(float_number): 17 | # Split the number into integer and fractional parts 18 | str_number = str(float_number) 19 | integer_part, decimal_part = str_number.split('.') 20 | decimal_as_nsecs = round(float(f"0.{decimal_part}") * 1_000_000_000) 21 | # Use the built-in `format` function to ensure we get 9 digits 22 | decimal_as_nsecs_str = format(decimal_as_nsecs, '09') 23 | # Return the parts as integers 24 | return int(integer_part), int(decimal_as_nsecs_str) 25 | 26 | def sort_files_in_folder(folder_path): 27 | # Get a list of all files in the folder 28 | file_list = os.listdir(folder_path) 29 | sorted_files = sorted(file_list, key=lambda x: int(re.findall(r'\d+', x)[0])) 30 | sorted_array = [] 31 | for file in sorted_files: 32 | file_path = os.path.join(folder_path, file) 33 | sorted_array.append(file_path) 34 | return sorted_array 35 | 36 | def read_pickle(file_path): 37 | with open(file_path, 'rb') as f: 38 | data = pickle.load(f) 39 | return data 40 | 41 | def process_pointcloud(points_in, label, label_to_delete): 42 | list_points = [] 43 | for i in range(len(label)): 44 | if label[i] not in label_to_delete: 45 | list_points.append(points_in[i].tolist()) 46 | return list_points 47 | 48 | def creat_pc2_msg( points, header, point_step = None): 49 | fields = [PointField(name='x', offset=0, 50 | datatype=PointField.FLOAT32, count=1), 51 | PointField(name='y', offset=4, 52 | datatype=PointField.FLOAT32, count=1), 53 | PointField(name='z', offset=8, 54 | datatype=PointField.FLOAT32, count=1), 55 | PointField(name='intensity', offset=12, 56 | datatype=PointField.FLOAT32, count=1)] 57 | # Check if input is numpy array 58 | if isinstance(points, np.ndarray): 59 | # Check if this is an unstructured array 60 | if points.dtype.names is None: 61 | assert all(fields[0].datatype == field.datatype for field in fields[1:]), \ 62 | 'All fields need to have the same datatype. Pass a structured NumPy array \ 63 | with multiple dtypes otherwise.' 64 | # Convert unstructured to structured array 65 | points = unstructured_to_structured( 66 | points, 67 | dtype=dtype_from_fields(fields, point_step)) 68 | else: 69 | assert points.dtype == dtype_from_fields(fields, point_step), \ 70 | 'PointFields and structured NumPy array dtype do not match for all fields! \ 71 | Check their field order, names and types.' 72 | else: 73 | # Cast python objects to structured NumPy array (slow) 74 | points = np.array( 75 | # Points need to be tuples in the structured array 76 | list(map(tuple, points)), 77 | dtype=dtype_from_fields(fields, point_step)) 78 | # Handle organized clouds 79 | assert len(points.shape) <= 2, \ 80 | 'Too many dimensions for organized cloud! \ 81 | Points can only be organized in max. two dimensional space' 82 | height = 1 83 | width = points.shape[0] 84 | # Check if input points are an organized cloud (2D array of points) 85 | if len(points.shape) == 2: 86 | height = points.shape[1] 87 | # Convert numpy points to array.array 88 | memory_view = memoryview(points) 89 | casted = memory_view.cast('B') 90 | array_array = array.array('B') 91 | array_array.frombytes(casted) 92 | list_array = list(array_array) 93 | 94 | cloud = PointCloud2( 95 | header=header, 96 | height=height, 97 | width=width, 98 | is_dense=False, 99 | is_bigendian=sys.byteorder != 'little', 100 | fields=fields, 101 | point_step=points.dtype.itemsize, 102 | row_step=(points.dtype.itemsize * width)) 103 | cloud.data = list_array 104 | return cloud 105 | 106 | def dtype_from_fields(fields, point_step = None): 107 | field_names = [] 108 | field_offsets = [] 109 | field_datatypes = [] 110 | _DATATYPES = {} 111 | _DATATYPES[PointField.INT8] = np.dtype(np.int8) 112 | _DATATYPES[PointField.UINT8] = np.dtype(np.uint8) 113 | _DATATYPES[PointField.INT16] = np.dtype(np.int16) 114 | _DATATYPES[PointField.UINT16] = np.dtype(np.uint16) 115 | _DATATYPES[PointField.INT32] = np.dtype(np.int32) 116 | _DATATYPES[PointField.UINT32] = np.dtype(np.uint32) 117 | _DATATYPES[PointField.FLOAT32] = np.dtype(np.float32) 118 | _DATATYPES[PointField.FLOAT64] = np.dtype(np.float64) 119 | 120 | DUMMY_FIELD_PREFIX = 'unnamed_field' 121 | for i, field in enumerate(fields): 122 | # Datatype as numpy datatype 123 | datatype = _DATATYPES[field.datatype] 124 | # Name field 125 | if field.name == '': 126 | name = f'{DUMMY_FIELD_PREFIX}_{i}' 127 | else: 128 | name = field.name 129 | # Handle fields with count > 1 by creating subfields with a suffix consiting 130 | # of "_" followed by the subfield counter [0 -> (count - 1)] 131 | assert field.count > 0, "Can't process fields with count = 0." 132 | for a in range(field.count): 133 | # Add suffix if we have multiple subfields 134 | if field.count > 1: 135 | subfield_name = f'{name}_{a}' 136 | else: 137 | subfield_name = name 138 | assert subfield_name not in field_names, 'Duplicate field names are not allowed!' 139 | field_names.append(subfield_name) 140 | # Create new offset that includes subfields 141 | field_offsets.append(field.offset + a * datatype.itemsize) 142 | field_datatypes.append(datatype.str) 143 | # Create dtype 144 | dtype_dict = { 145 | 'names': field_names, 146 | 'formats': field_datatypes, 147 | 'offsets': field_offsets 148 | } 149 | if point_step is not None: 150 | dtype_dict['itemsize'] = point_step 151 | return np.dtype(dtype_dict) 152 | 153 | 154 | def main(): 155 | 156 | argparser = argparse.ArgumentParser(description= 'for genarating raw data') 157 | argparser.add_argument('--root_path',type=str,default='/media/lde/yuhang/DynaCARLA',help='Data index') 158 | argparser.add_argument('--seg_lidar',action='store_true',default=False,help='If crop lidar point cloud belongs to certain label') 159 | argparser.add_argument('--seg_lidar_label',metavar='label1_label2_label3_...',type=str,default='4_10',help='lidar labels to crop') 160 | argparser.add_argument('--output_path',type=str,default='/media/lde/yuhang/DynaCARLA/rosbags',help='output path to save ros bag') 161 | args = argparser.parse_args() 162 | 163 | args.label_to_delete = [int(x) for x in args.seg_lidar_label.split('_')] 164 | map_names = ['Town01', 'Town02', 'Town03', 'Town04', 'Town05', 'Town06', 'Town07', 'Town10HD'] 165 | # [dynamic_type, if_use_segmentation] 166 | presets = [['dynamic', False], ['dynamic', True], ['static', False]] 167 | 168 | for map_name in map_names: 169 | seq_root = os.path.join(args.root_path, map_name) 170 | common_data_path = os.path.join(seq_root, 'common_data.pkl') 171 | for preset in presets: 172 | if preset[1]: 173 | bag_name = 'LIOseg_'+map_name+'_'+preset[0]+'_.bag' 174 | else: 175 | bag_name = 'LIO_'+map_name+'_'+preset[0]+'_.bag' 176 | bag_path = os.path.join(args.output_path,map_name,'lidar',bag_name) 177 | 178 | with open(common_data_path, 'rb') as f: 179 | common_data = pickle.load(f) 180 | 181 | with rosbag.Bag(bag_path, 'w') as bag: 182 | 183 | #----------------------imu data-------------------- 184 | print('converting imu data......') 185 | for i in tqdm(range(len(common_data['imu']['acc']))): 186 | secs, nano_secs = split_float_to_secs_nsecs(common_data['imu']['timestamp'][i]) 187 | timestamp = rospy.Time(secs=secs, nsecs=nano_secs) 188 | imu_msg = Imu() 189 | imu_msg.header.seq = i 190 | if i ==1: 191 | i = 2 192 | imu_msg.header.frame_id = "imu" 193 | imu_msg.header.stamp = timestamp 194 | imu_msg.linear_acceleration.x = float(common_data['imu']['acc'][i][0]) 195 | imu_msg.linear_acceleration.y = float(common_data['imu']['acc'][i][1]) 196 | imu_msg.linear_acceleration.z = float(common_data['imu']['acc'][i][2]) 197 | imu_msg.angular_velocity.x = float(common_data['imu']['gyro'][i][0]) 198 | imu_msg.angular_velocity.y = float(common_data['imu']['gyro'][i][1]) 199 | imu_msg.angular_velocity.z = float(common_data['imu']['gyro'][i][2]) 200 | bag.write("/imu", imu_msg, timestamp) 201 | 202 | print('converting lidar msg...') 203 | lidar_path = os.path.join(seq_root, 'lidar', preset[0]) 204 | seg_lidar_path = os.path.join(seq_root, 'seg_lidar', preset[0]) 205 | lidar_file_paths = sort_files_in_folder(lidar_path) 206 | seg_lidar_file_paths = sort_files_in_folder(seg_lidar_path) 207 | for index in tqdm(range(len(lidar_file_paths))): 208 | lidar_pkl = read_pickle(lidar_file_paths[index]) 209 | label_pkl = read_pickle(seg_lidar_file_paths[index]) 210 | if preset[1]: 211 | lidar_list = process_pointcloud(lidar_pkl, label_pkl, args.label_to_delete) 212 | else: 213 | lidar_list = lidar_pkl.tolist() 214 | secs, nano_secs = split_float_to_secs_nsecs(common_data['lidar']['timestamp'][index]) 215 | timestamp = rospy.Time(secs=secs, nsecs=nano_secs) 216 | header = std_msgs.Header() 217 | header.stamp = timestamp 218 | header.frame_id = 'laser' 219 | header.seq = index 220 | lidar_msg = creat_pc2_msg(lidar_list, header) 221 | bag.write('/velodyne', lidar_msg, timestamp) 222 | 223 | bag.close() 224 | 225 | if __name__ == '__main__': 226 | main() 227 | --------------------------------------------------------------------------------