├── README.md ├── utils.py ├── constants.py ├── image_converter.py ├── datadescriptor.py ├── camera_utils.py ├── dataexport.py ├── bounding_box.py └── dolphins_generation.py /README.md: -------------------------------------------------------------------------------- 1 | # Dolphins 2 | 3 | This is the main code of [Dolphins](https://dolphins-dataset.net). 4 | 5 | Please modify dolphins_generation.py to set your own positions of sensors and the maps. 6 | 7 | Thanks for the work: https://github.com/liulingfeng123/Carla0.9.10-kitti-data-export. It helps to export data in KITTI format. 8 | 9 | The code has been tested with CARLA 0.9.12 10 | 11 | # Citation 12 | 13 | ``` 14 | @misc{2207.07609, 15 | Author = {Ruiqing Mao and Jingyu Guo and Yukuan Jia and Yuxuan Sun and Sheng Zhou and Zhisheng Niu}, 16 | Title = {DOLPHINS: Dataset for Collaborative Perception enabled Harmonious and Interconnected Self-driving}, 17 | Year = {2022}, 18 | Eprint = {arXiv:2207.07609}, 19 | } 20 | ``` 21 | -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | import random 2 | import time 3 | import colorsys 4 | import math 5 | import numpy as np 6 | 7 | 8 | class Timer(object): 9 | def __init__(self): 10 | self.step = 0 11 | self._lap_step = 0 12 | self._lap_time = time.time() 13 | 14 | def tick(self): 15 | self.step += 1 16 | 17 | def lap(self): 18 | self._lap_step = self.step 19 | self._lap_time = time.time() 20 | 21 | def ticks_per_second(self): 22 | return float(self.step - self._lap_step) / self.elapsed_seconds_since_lap() 23 | 24 | def elapsed_seconds_since_lap(self): 25 | return time.time() - self._lap_time 26 | 27 | 28 | def rand_color(seed): 29 | """Return random color based on a seed""" 30 | random.seed(seed) 31 | col = colorsys.hls_to_rgb(random.random(), random.uniform(.2, .8), 1.0) 32 | return (int(col[0]*255), int(col[1]*255), int(col[2]*255)) 33 | 34 | 35 | def vector3d_to_array(vec3d): 36 | return np.array([vec3d.x, vec3d.y, vec3d.z]) 37 | 38 | 39 | def degrees_to_radians(degrees): 40 | return degrees * math.pi / 180 41 | -------------------------------------------------------------------------------- /constants.py: -------------------------------------------------------------------------------- 1 | """ DATA GENERATION SETTINGS""" 2 | GEN_DATA = True # Whether or not to save training data 3 | VISUALIZE_LIDAR = False 4 | # How many frames to wait between each capture of screen, bounding boxes and lidar 5 | STEPS_BETWEEN_RECORDINGS = 10 6 | CLASSES_TO_LABEL = ["Vehicle", "Pedestrian"] 7 | # Lidar can be saved in bin to comply to kitti, or the standard .ply format 8 | LIDAR_DATA_FORMAT = "bin" 9 | assert LIDAR_DATA_FORMAT in [ 10 | "bin", "ply"], "Lidar data format must be either bin or ply" 11 | OCCLUDED_VERTEX_COLOR = (255, 0, 0) 12 | VISIBLE_VERTEX_COLOR = (0, 255, 0) 13 | # How many meters the car must drive before a new capture is triggered. 14 | DISTANCE_SINCE_LAST_RECORDING = 10 15 | # How many datapoints to record before resetting the scene. 16 | NUM_RECORDINGS_BEFORE_RESET = 20 17 | # How many frames to render before resetting the environment 18 | # For example, the agent may be stuck 19 | NUM_EMPTY_FRAMES_BEFORE_RESET = 100 20 | 21 | """ CARLA SETTINGS """ 22 | CAMERA_HEIGHT_POS = 2.0 23 | LIDAR_HEIGHT_POS = CAMERA_HEIGHT_POS 24 | MIN_BBOX_AREA_IN_PX = 100 25 | 26 | 27 | """ AGENT SETTINGS """ 28 | NUM_VEHICLES = 40 29 | NUM_PEDESTRIANS = 15 30 | 31 | """ RENDERING SETTINGS """ 32 | WINDOW_WIDTH = 1920 33 | WINDOW_HEIGHT = 1080 34 | MINI_WINDOW_WIDTH = 320 35 | MINI_WINDOW_HEIGHT = 180 36 | 37 | WINDOW_WIDTH_HALF = WINDOW_WIDTH / 2 38 | WINDOW_HEIGHT_HALF = WINDOW_HEIGHT / 2 39 | 40 | MAX_RENDER_DEPTH_IN_METERS = 100 # Meters 41 | MIN_VISIBLE_VERTICES_FOR_RENDER = 2 42 | 43 | NUM_OF_WALKERS = 10 44 | FILTERW = 'walker.pedestrian.*' 45 | FILTERV = 'vehicle.*' 46 | NUM_OF_VEHICLES = 18 47 | -------------------------------------------------------------------------------- /image_converter.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de 2 | # Barcelona (UAB). 3 | # 4 | # This work is licensed under the terms of the MIT license. 5 | # For a copy, see . 6 | 7 | """ 8 | Handy conversions for CARLA images. 9 | 10 | The functions here are provided for real-time display, if you want to save the 11 | converted images, save the images from Python without conversion and convert 12 | them afterwards with the C++ implementation at "Util/ImageConverter" as it 13 | provides considerably better performance. 14 | """ 15 | 16 | import glob 17 | import os 18 | import sys 19 | 20 | 21 | try: 22 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 23 | sys.version_info.major, 24 | sys.version_info.minor, 25 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 26 | except IndexError: 27 | pass 28 | 29 | import carla 30 | 31 | try: 32 | import numpy as np 33 | from numpy.matlib import repmat 34 | except ImportError: 35 | raise RuntimeError('cannot import numpy, make sure numpy package is installed') 36 | 37 | 38 | def to_bgra_array(image): 39 | """Convert a CARLA raw image to a BGRA numpy array.""" 40 | ''' 41 | if not isinstance(image, sensor.Image): 42 | raise ValueError("Argument must be a carla.sensor.Image") 43 | ''' 44 | array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) 45 | array = np.reshape(array, (image.height, image.width, 4)) 46 | return array 47 | 48 | 49 | def to_rgb_array(image): 50 | """Convert a CARLA raw image to a RGB numpy array.""" 51 | array = to_bgra_array(image) 52 | # Convert BGRA to RGB. 53 | array = array[:, :, :3] 54 | #array = array[:, :, ::-1] 55 | return array 56 | 57 | def labels_to_array(image): 58 | """ 59 | Convert an image containing CARLA semantic segmentation labels to a 2D array 60 | containing the label of each pixel. 61 | """ 62 | return to_bgra_array(image)[:, :, 2] 63 | 64 | 65 | def labels_to_cityscapes_palette(image): 66 | """ 67 | Convert an image containing CARLA semantic segmentation labels to 68 | Cityscapes palette. 69 | """ 70 | classes = { 71 | 0: [0, 0, 0], # None 72 | 1: [70, 70, 70], # Buildings 73 | 2: [190, 153, 153], # Fences 74 | 3: [72, 0, 90], # Other 75 | 4: [220, 20, 60], # Pedestrians 76 | 5: [153, 153, 153], # Poles 77 | 6: [157, 234, 50], # RoadLines 78 | 7: [128, 64, 128], # Roads 79 | 8: [244, 35, 232], # Sidewalks 80 | 9: [107, 142, 35], # Vegetation 81 | 10: [0, 0, 255], # Vehicles 82 | 11: [102, 102, 156], # Walls 83 | 12: [220, 220, 0] # TrafficSigns 84 | } 85 | array = labels_to_array(image) 86 | result = np.zeros((array.shape[0], array.shape[1], 3)) 87 | for key, value in classes.items(): 88 | result[np.where(array == key)] = value 89 | return result 90 | 91 | 92 | def depth_to_array(image): 93 | """ 94 | Convert an image containing CARLA encoded depth-map to a 2D array containing 95 | the depth value of each pixel normalized between [0.0, 1.0]. 96 | """ 97 | array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) 98 | array = np.reshape(array, (image.height, image.width, 4)) # RGBA format 99 | array = array[:, :, :3] # Take only RGB 100 | array = array[:, :, ::-1] # BGR 101 | array = array.astype(np.float32) # 2ms 102 | gray_depth = ((array[:, :, 0] + array[:, :, 1] * 256.0 + array[:, :, 2] * 256.0 * 256.0) / ( 103 | (256.0 * 256.0 * 256.0) - 1)) # 2.5ms 104 | gray_depth = 1000 * gray_depth 105 | return gray_depth 106 | 107 | 108 | def depth_to_logarithmic_grayscale(image): 109 | """ 110 | Convert an image containing CARLA encoded depth-map to a logarithmic 111 | grayscale image array. 112 | "max_depth" is used to omit the points that are far enough. 113 | """ 114 | normalized_depth = depth_to_array(image) 115 | # Convert to logarithmic depth. 116 | logdepth = np.ones(normalized_depth.shape) + \ 117 | (np.log(normalized_depth) / 5.70378) 118 | logdepth = np.clip(logdepth, 0.0, 1.0) 119 | logdepth *= 255.0 120 | # Expand to three colors. 121 | return np.repeat(logdepth[:, :, np.newaxis], 3, axis=2) -------------------------------------------------------------------------------- /datadescriptor.py: -------------------------------------------------------------------------------- 1 | # coding=utf-8 2 | """ 3 | #Values Name Description 4 | ---------------------------------------------------------------------------- 5 | 1 type Describes the type of object: 'Car', 'Van', 'Truck', 6 | 'Pedestrian', 'Person_sitting', 'Cyclist', 'Tram', 7 | 'Misc' or 'DontCare' 8 | 1 truncated Float from 0 (non-truncated) to 1 (truncated), where 9 | truncated refers to the object leaving image boundaries 10 | 1 occluded Integer (0,1,2,3) indicating occlusion state: 11 | 0 = fully visible, 1 = partly occluded 12 | 2 = largely occluded, 3 = unknown 13 | 1 alpha Observation angle of object, ranging [-pi..pi] 14 | 4 bbox 2D bounding box of object in the image (0-based index): 15 | contains left, top, right, bottom pixel coordinates 16 | 3 dimensions 3D object dimensions: height, width, length (in meters) 17 | 3 location 3D object location x,y,z in camera coordinates (in meters) 18 | 1 rotation_y Rotation ry around Y-axis in camera coordinates [-pi..pi] 19 | 1 score Only for results: Float, indicating confidence in 20 | detection, needed for p/r curves, higher is better. 21 | """ 22 | 23 | from typing import List 24 | from math import pi 25 | 26 | 27 | class KittiDescriptor: 28 | # This class is responsible for storing a single datapoint for the kitti 3d object detection task 29 | def __init__(self, type=None, bbox=None, dimensions=None, location=None, rotation_y=None, extent=None): 30 | self.type = type 31 | self.truncated = 0 32 | self.occluded = 0 33 | self.alpha = -10 34 | self.bbox = bbox 35 | self.dimensions = dimensions 36 | self.location = location 37 | self.rotation_y = rotation_y 38 | self.extent = extent 39 | self._valid_classes = ['Car', 'Van', 'Truck', 40 | 'Pedestrian', 'Person_sitting', 'Cyclist', 'Tram', 41 | 'Misc', 'DontCare'] 42 | 43 | def set_type(self, obj_type: str): 44 | assert obj_type in self._valid_classes, "Object must be of types {}".format( 45 | self._valid_classes) 46 | self.type = obj_type 47 | 48 | def set_truncated(self, truncated: float): 49 | assert 0 <= truncated <= 1, """Truncated must be Float from 0 (non-truncated) to 1 (truncated), where 50 | truncated refers to the object leaving image boundaries """ 51 | self.truncated = truncated 52 | 53 | def set_occlusion(self, occlusion: int): 54 | assert occlusion in range(0, 4), """Occlusion must be Integer (0,1,2,3) indicating occlusion state: 55 | 0 = fully visible, 1 = partly occluded 56 | 2 = largely occluded, 3 = unknown""" 57 | self._occluded = occlusion 58 | 59 | def set_alpha(self, alpha: float): 60 | assert -pi <= alpha <= pi, "Alpha must be in range [-pi..pi]" 61 | self.alpha = alpha 62 | 63 | def set_bbox(self, bbox: List[int]): 64 | assert len(bbox) == 4, """ Bbox must be 2D bounding box of object in the image (0-based index): 65 | contains left, top, right, bottom pixel coordinates (two points)""" 66 | self.bbox = bbox 67 | 68 | def set_3d_object_dimensions(self, bbox_extent): 69 | # Bbox extent consists of x,y and z. 70 | # The bbox extent is by Carla set as 71 | # x: length of vehicle (driving direction) 72 | # y: to the right of the vehicle 73 | # z: up (direction of car roof) 74 | # However, Kitti expects height, width and length (z, y, x): 75 | height, width, length = bbox_extent.z, bbox_extent.x, bbox_extent.y 76 | # Since Carla gives us bbox extent, which is a half-box, multiply all by two 77 | self.extent = (height, width, length) 78 | self.dimensions = "{} {} {}".format(2*height, 2*width, 2*length) 79 | 80 | def set_3d_object_location(self, obj_location): 81 | """ TODO: Change this to 82 | Converts the 3D object location from CARLA coordinates and saves them as KITTI coordinates in the object 83 | In Unreal, the coordinate system of the engine is defined as, which is the same as the lidar points 84 | z 85 | ▲ ▲ x 86 | | / 87 | | / 88 | |/____> y 89 | This is a left-handed coordinate system, with x being forward, y to the right and z up 90 | See also https://github.com/carla-simulator/carla/issues/498 91 | However, the camera coordinate system for KITTI is defined as 92 | ▲ z 93 | / 94 | / 95 | /____> x 96 | | 97 | | 98 | | 99 | ▼ 100 | y 101 | This is a right-handed coordinate system with z being forward, x to the right and y down 102 | Therefore, we have to make the following changes from Carla to Kitti 103 | Carla: X Y Z 104 | KITTI:-X -Y Z 105 | """ 106 | # Object location is four values (x, y, z, w). We only care about three of them (xyz) 107 | x, y, z = [float(x) for x in obj_location][0:3] 108 | assert None not in [ 109 | self.extent, self.type], "Extent and type must be set before location!" 110 | # Convert from Carla coordinate system to KITTI 111 | # This works for AVOD (image) 112 | self.location = " ".join(map(str, [y, -z, x])) 113 | 114 | def set_rotation_y(self, rotation_y: float): 115 | assert - \ 116 | pi <= rotation_y <= pi, "Rotation y must be in range [-pi..pi] - found {}".format( 117 | rotation_y) 118 | self.rotation_y = rotation_y 119 | 120 | def __str__(self): 121 | """ Returns the kitti formatted string of the datapoint if it is valid (all critical variables filled out), else it returns an error.""" 122 | if self.bbox is None: 123 | bbox_format = " " 124 | else: 125 | bbox_format = " ".join([str(x) for x in self.bbox]) 126 | 127 | return "{} {} {} {} {} {} {} {}".format(self.type, self.truncated, self.occluded, self.alpha, bbox_format, self.dimensions, self.location, self.rotation_y) 128 | -------------------------------------------------------------------------------- /camera_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy.linalg import pinv, inv 3 | from constants import WINDOW_HEIGHT, WINDOW_WIDTH 4 | 5 | 6 | 7 | 8 | def calc_projected_2d_bbox(vertices_pos2d): 9 | """ Takes in all vertices in pixel projection and calculates min and max of all x and y coordinates. 10 | Returns left top, right bottom pixel coordinates for the 2d bounding box as a list of four values. 11 | Note that vertices_pos2d contains a list of (y_pos2d, x_pos2d) tuples, or None 12 | """ 13 | 14 | legal_pos2d = list(filter(lambda x: x is not None, vertices_pos2d)) 15 | y_coords, x_coords = [int(x[0][0]) for x in legal_pos2d], [ 16 | int(x[1][0]) for x in legal_pos2d] 17 | min_x, max_x = min(x_coords), max(x_coords) 18 | min_y, max_y = min(y_coords), max(y_coords) 19 | return [min_x, min_y, max_x, max_y] 20 | 21 | 22 | def midpoint_from_agent_location(array, location, extrinsic_mat, intrinsic_mat): 23 | # Calculate the midpoint of the bottom chassis 24 | # This is used since kitti treats this point as the location of the car 25 | midpoint_vector = np.array([ 26 | [location.x], # [[X, 27 | [location.y], # Y, 28 | [location.z], # Z, 29 | [1.0] # 1.0]] 30 | ]) 31 | transformed_3d_midpoint = proj_to_camera(midpoint_vector, extrinsic_mat) 32 | return transformed_3d_midpoint 33 | 34 | 35 | def proj_to_camera(pos_vector, extrinsic_mat): 36 | # transform the points to camera 37 | transformed_3d_pos = np.dot(inv(extrinsic_mat), pos_vector) 38 | return transformed_3d_pos 39 | 40 | 41 | def proj_to_2d(camera_pos_vector, intrinsic_mat): 42 | # transform the points to 2D 43 | cords_x_y_z = camera_pos_vector[:3, :] 44 | cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) 45 | pos2d = np.dot(intrinsic_mat, cords_y_minus_z_x) 46 | # normalize the 2D points 47 | pos2d = np.array([ 48 | pos2d[0] / pos2d[2], 49 | pos2d[1] / pos2d[2], 50 | pos2d[2] 51 | ]) 52 | return pos2d 53 | 54 | 55 | def draw_3d_bounding_box(array, vertices_pos2d): 56 | """ Draws lines from each vertex to all connected vertices """ 57 | # Shows which verticies that are connected so that we can draw lines between them 58 | # The key of the dictionary is the index in the bbox array, and the corresponding value is a list of indices 59 | # referring to the same array. 60 | vertex_graph = {0: [1, 2, 4], 61 | 1: [0, 3, 5], 62 | 2: [0, 3, 6], 63 | 3: [1, 2, 7], 64 | 4: [0, 5, 6], 65 | 5: [1, 4, 7], 66 | 6: [2, 4, 7]} 67 | # Note that this can be sped up by not drawing duplicate lines 68 | for vertex_idx in vertex_graph: 69 | neighbour_idxs = vertex_graph[vertex_idx] 70 | from_pos2d = vertices_pos2d[vertex_idx] 71 | for neighbour_idx in neighbour_idxs: 72 | to_pos2d = vertices_pos2d[neighbour_idx] 73 | if from_pos2d is None or to_pos2d is None: 74 | continue 75 | y1, x1 = from_pos2d[0], from_pos2d[1] 76 | y2, x2 = to_pos2d[0], to_pos2d[1] 77 | # Only stop drawing lines if both are outside 78 | if not point_in_canvas((y1, x1)) and not point_in_canvas((y2, x2)): 79 | continue 80 | for x, y in get_line(x1, y1, x2, y2): 81 | if point_in_canvas((y, x)): 82 | array[int(y), int(x)] = (255, 0, 0) 83 | 84 | 85 | def draw_3d_invisible_bounding_box(array, vertices_pos2d): 86 | """ Draws lines from each vertex to all connected vertices """ 87 | # Shows which verticies that are connected so that we can draw lines between them 88 | # The key of the dictionary is the index in the bbox array, and the corresponding value is a list of indices 89 | # referring to the same array. 90 | vertex_graph = {0: [1, 2, 4], 91 | 1: [0, 3, 5], 92 | 2: [0, 3, 6], 93 | 3: [1, 2, 7], 94 | 4: [0, 5, 6], 95 | 5: [1, 4, 7], 96 | 6: [2, 4, 7]} 97 | # Note that this can be sped up by not drawing duplicate lines 98 | for vertex_idx in vertex_graph: 99 | neighbour_idxs = vertex_graph[vertex_idx] 100 | from_pos2d = vertices_pos2d[vertex_idx] 101 | for neighbour_idx in neighbour_idxs: 102 | to_pos2d = vertices_pos2d[neighbour_idx] 103 | if from_pos2d is None or to_pos2d is None: 104 | continue 105 | y1, x1 = from_pos2d[0], from_pos2d[1] 106 | y2, x2 = to_pos2d[0], to_pos2d[1] 107 | # Only stop drawing lines if both are outside 108 | if not point_in_canvas((y1, x1)) and not point_in_canvas((y2, x2)): 109 | continue 110 | for x, y in get_line(x1, y1, x2, y2): 111 | if point_in_canvas((y, x)): 112 | array[int(y), int(x)] = (0, 0, 255) 113 | 114 | 115 | def point_in_canvas(pos): 116 | """Return true if point is in canvas""" 117 | if (pos[0] >= 0) and (pos[0] < WINDOW_HEIGHT) and (pos[1] >= 0) and (pos[1] < WINDOW_WIDTH): 118 | return True 119 | return False 120 | 121 | 122 | def get_line(x1, y1, x2, y2): 123 | x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) 124 | #print("Calculating line from {},{} to {},{}".format(x1,y1,x2,y2)) 125 | points = [] 126 | issteep = abs(y2-y1) > abs(x2-x1) 127 | if issteep: 128 | x1, y1 = y1, x1 129 | x2, y2 = y2, x2 130 | rev = False 131 | if x1 > x2: 132 | x1, x2 = x2, x1 133 | y1, y2 = y2, y1 134 | rev = True 135 | deltax = x2 - x1 136 | deltay = abs(y2-y1) 137 | error = int(deltax / 2) 138 | y = y1 139 | ystep = None 140 | if y1 < y2: 141 | ystep = 1 142 | else: 143 | ystep = -1 144 | for x in range(x1, x2 + 1): 145 | if issteep: 146 | points.append((y, x)) 147 | else: 148 | points.append((x, y)) 149 | error -= deltay 150 | if error < 0: 151 | y += ystep 152 | error += deltax 153 | # Reverse the list if the coordinates were reversed 154 | if rev: 155 | points.reverse() 156 | return points 157 | 158 | 159 | def draw_rect(array, pos, size, color): 160 | """Draws a rect""" 161 | point_0 = (pos[0]-size/2, pos[1]-size/2) 162 | point_1 = (pos[0]+size/2, pos[1]+size/2) 163 | if point_in_canvas(point_0) and point_in_canvas(point_1): 164 | for i in range(size): 165 | for j in range(size): 166 | array[int(point_0[0]+i), int(point_0[1]+j)] = color 167 | 168 | 169 | def point_is_occluded(point, vertex_depth, depth_map): 170 | """ Checks whether or not the four pixels directly around the given point has less depth than the given vertex depth 171 | If True, this means that the point is occluded. 172 | """ 173 | y, x = map(int, point) 174 | from itertools import product 175 | neigbours = product((1, -1), repeat=2) 176 | is_occluded = [] 177 | for dy, dx in neigbours: 178 | if point_in_canvas((dy+y, dx+x)): 179 | # If the depth map says the pixel is closer to the camera than the actual vertex 180 | if depth_map[y+dy, x+dx] < vertex_depth: 181 | is_occluded.append(True) 182 | else: 183 | is_occluded.append(False) 184 | # Only say point is occluded if all four neighbours are closer to camera than vertex 185 | return all(is_occluded) 186 | -------------------------------------------------------------------------------- /dataexport.py: -------------------------------------------------------------------------------- 1 | """ 2 | This file contains all the methods responsible for saving the generated data in the correct output format. 3 | 4 | """ 5 | 6 | import numpy as np 7 | import os 8 | import sys 9 | import glob 10 | import logging 11 | from utils import degrees_to_radians 12 | import carla 13 | import math 14 | from numpy.linalg import pinv, inv 15 | from PIL import Image 16 | 17 | 18 | def save_groundplanes(planes_fname, player, lidar_height): 19 | from math import cos, sin 20 | """ Saves the groundplane vector of the current frame. 21 | The format of the ground plane file is first three lines describing the file (number of parameters). 22 | The next line is the three parameters of the normal vector, and the last is the height of the normal vector, 23 | which is the same as the distance to the camera in meters. 24 | """ 25 | rotation = player.get_transform().rotation 26 | pitch, roll = rotation.pitch, rotation.roll 27 | # Since measurements are in degrees, convert to radians 28 | pitch = degrees_to_radians(pitch) 29 | roll = degrees_to_radians(roll) 30 | # Rotate normal vector (y) wrt. pitch and yaw 31 | normal_vector = [cos(pitch)*sin(roll), 32 | -cos(pitch)*cos(roll), 33 | sin(pitch) 34 | ] 35 | normal_vector = map(str, normal_vector) 36 | with open(planes_fname, 'w') as f: 37 | f.write("# Plane\n") 38 | f.write("Width 4\n") 39 | f.write("Height 1\n") 40 | f.write("{} {}\n".format(" ".join(normal_vector), lidar_height)) 41 | logging.info("Wrote plane data to %s", planes_fname) 42 | 43 | 44 | def save_ref_files(OUTPUT_FOLDER, id): 45 | """ Appends the id of the given record to the files """ 46 | for name in ['train.txt', 'val.txt', 'trainval.txt']: 47 | path = os.path.join(OUTPUT_FOLDER, name) 48 | with open(path, 'a') as f: 49 | f.write("{0:06}".format(id) + '\n') 50 | logging.info("Wrote reference files to %s", path) 51 | 52 | 53 | def save_image_data(filename, image): 54 | logging.info("Wrote image data to %s", filename) 55 | image.save_to_disk(filename) 56 | 57 | 58 | def save_lidar_data(filename, point_cloud, format="bin"): 59 | """ Saves lidar data to given filename, according to the lidar data format. 60 | bin is used for KITTI-data format, while .ply is the regular point cloud format 61 | In Unreal, the coordinate system of the engine is defined as, which is the same as the lidar points 62 | z 63 | ^ ^ x 64 | | / 65 | | / 66 | |/____> y 67 | This is a left-handed coordinate system, with x being forward, y to the right and z up 68 | See also https://github.com/carla-simulator/carla/issues/498 69 | However, the lidar coordinate system from KITTI is defined as 70 | z 71 | ^ ^ x 72 | | / 73 | | / 74 | y<____|/ 75 | Which is a right handed coordinate sylstem 76 | Therefore, we need to flip the y axis of the lidar in order to get the correct lidar format for kitti. 77 | This corresponds to the following changes from Carla to Kitti 78 | Carla: X Y Z 79 | KITTI: X -Y Z 80 | NOTE: We do not flip the coordinate system when saving to .ply. 81 | """ 82 | logging.info("Wrote lidar data to %s", filename) 83 | 84 | if format == "bin": 85 | lidar_array = [[point[0], -point[1], point[2], 1.0] 86 | for point in point_cloud] 87 | lidar_array = np.array(lidar_array).astype(np.float32) 88 | 89 | logging.debug("Lidar min/max of x: {} {}".format( 90 | lidar_array[:, 0].min(), lidar_array[:, 0].max())) 91 | logging.debug("Lidar min/max of y: {} {}".format( 92 | lidar_array[:, 1].min(), lidar_array[:, 0].max())) 93 | logging.debug("Lidar min/max of z: {} {}".format( 94 | lidar_array[:, 2].min(), lidar_array[:, 0].max())) 95 | lidar_array.tofile(filename) 96 | 97 | # point_cloud.save_to_disk(filename) 98 | 99 | 100 | 101 | 102 | def save_kitti_data(filename, datapoints): 103 | with open(filename, 'w') as f: 104 | out_str = "\n".join([str(point) for point in datapoints if point]) 105 | f.write(out_str) 106 | logging.info("Wrote kitti data to %s", filename) 107 | 108 | def save_loc_data(filename, loc,loc_rc): 109 | loc=np.array(loc) 110 | loc_rc=np.array(loc_rc) 111 | ravel_mode = 'C' 112 | def write_flat(f, name, arr): 113 | f.write("{}: {}\n".format(name, ' '.join( 114 | map(str, arr.flatten(ravel_mode).squeeze())))) 115 | with open(filename, 'w') as f: 116 | write_flat(f, "ego_vehicle" , loc) 117 | write_flat(f, "other_vehicle" , loc_rc) 118 | logging.info("Wrote loc data to %s", filename) 119 | 120 | def save_loc_data_0(filename, loc): 121 | loc=np.array(loc) 122 | ravel_mode = 'C' 123 | def write_flat(f, name, arr): 124 | f.write("{}: {}\n".format(name, ' '.join( 125 | map(str, arr.flatten(ravel_mode).squeeze())))) 126 | with open(filename, 'w') as f: 127 | write_flat(f, "ego_vehicle" , loc) 128 | logging.info("Wrote loc data to %s", filename) 129 | 130 | def save_loc_data_2(filename, loc,loc_r,loc_rc): 131 | loc=np.array(loc) 132 | loc_r=np.array(loc_r) 133 | loc_rc=np.array(loc_rc) 134 | ravel_mode = 'C' 135 | def write_flat(f, name, arr): 136 | f.write("{}: {}\n".format(name, ' '.join( 137 | map(str, arr.flatten(ravel_mode).squeeze())))) 138 | with open(filename, 'w') as f: 139 | write_flat(f, "ego_vehicle" , loc) 140 | write_flat(f, "fore_vehicle" , loc_r) 141 | write_flat(f, "other_vehicle" , loc_rc) 142 | logging.info("Wrote loc data to %s", filename) 143 | 144 | def save_calibration_matrices(filename, intrinsic_mat, rt_2,rt_r,rt_rc): 145 | """ Saves the calibration matrices to a file. 146 | AVOD (and KITTI) refers to P as P=K*[R;t], so we will just store P. 147 | The resulting file will contain: 148 | 3x4 p0-p3 Camera P matrix. Contains extrinsic 149 | and intrinsic parameters. (P=K*[R;t]) 150 | 3x3 r0_rect Rectification matrix, required to transform points 151 | from velodyne to camera coordinate frame. 152 | 3x4 tr_velodyne_to_cam Used to transform from velodyne to cam 153 | coordinate frame according to: 154 | Point_Camera = P_cam * R0_rect * 155 | Tr_velo_to_cam * 156 | Point_Velodyne. 157 | 3x4 tr_imu_to_velo Used to transform from imu to velodyne coordinate frame. This is not needed since we do not export 158 | imu data. 159 | """ 160 | f=1000 161 | k=[[f,0,960,0],[0,f,540,0],[0,0,1,0]] 162 | it=np.array(k) 163 | 164 | R_r=np.dot(inv(rt_2),rt_r) 165 | L_r=R_r 166 | R_rc=np.dot(inv(rt_2),rt_rc) 167 | L_rc=R_rc 168 | kitti_to_carla=np.mat([[0,0,1,0],[1,0,0,0],[0,-1,0,0],[0,0,0,1]]) 169 | R_r=np.dot(R_r,kitti_to_carla) 170 | R_rc=np.dot(R_rc,kitti_to_carla) 171 | carla_to_kitti=np.mat([[0,1,0,0],[0,0,-1,0],[1,0,0,0],[0,0,0,1]]) 172 | R_r=np.dot(carla_to_kitti,R_r) 173 | R_rc=np.dot(carla_to_kitti,R_rc) 174 | 175 | 176 | ravel_mode = 'C' 177 | P0 = it 178 | P1 = np.dot(it,R_r) 179 | P2 = np.dot(it,R_rc) 180 | 181 | R_2_to_r=np.dot(inv(rt_r),rt_2) 182 | R_2_to_r=np.dot(R_2_to_r,kitti_to_carla) 183 | R_2_to_r=np.dot(carla_to_kitti,R_2_to_r) 184 | P_2_to_r = np.dot(it,R_2_to_r) 185 | 186 | R_2_to_rc=np.dot(inv(rt_rc),rt_2) 187 | R_2_to_rc=np.dot(R_2_to_rc,kitti_to_carla) 188 | R_2_to_rc=np.dot(carla_to_kitti,R_2_to_rc) 189 | P_2_to_rc = np.dot(it,R_2_to_rc) 190 | 191 | 192 | R0 = np.identity(3) 193 | vel_to_world = np.mat([[1,0,0,0],[0,-1,0,0],[0,0,1,0],[0,0,0,1]]) 194 | vel_ego_to_cam = np.mat([[0,-1,0,0],[0,0,-1,0],[1,0,0,0]]) 195 | vel_to_kitti = np.mat([[0,1,0,0],[0,0,-1,0],[1,0,0,0]]) 196 | vel_r_to_cam = np.dot(L_r,vel_to_world) 197 | vel_r_to_cam = np.dot(vel_to_kitti,vel_r_to_cam) 198 | vel_rc_to_cam = np.dot(L_rc,vel_to_world) 199 | vel_rc_to_cam = np.dot(vel_to_kitti,vel_rc_to_cam) 200 | 201 | TR_imu_to_velo = np.identity(3) 202 | TR_imu_to_velo = np.column_stack((TR_imu_to_velo, np.array([0, 0, 0]))) 203 | 204 | def write_flat(f, name, arr): 205 | f.write("{}: {}\n".format(name, ' '.join( 206 | map(str, arr.flatten(ravel_mode).squeeze())))) 207 | 208 | # All matrices are written on a line with spacing 209 | with open(filename, 'w') as f: 210 | # Avod expects all 4 P-matrices even though we only use the first 211 | write_flat(f, "P0" , P0) 212 | write_flat(f, "P1" , np.array(P1)) 213 | write_flat(f, "P2" , np.array(P2)) 214 | write_flat(f, "P3" , P0) 215 | write_flat(f, "Pc-r" , np.array(P_2_to_r)) 216 | write_flat(f, "Pc-rc" , np.array(P_2_to_rc)) 217 | write_flat(f, "R0_rect", R0) 218 | write_flat(f, "Tr_velo_to_cam", np.array(vel_ego_to_cam)) 219 | write_flat(f, "Tr_velo_r_to_cam", np.array(vel_r_to_cam)) 220 | write_flat(f, "Tr_velo_rc_to_cam", np.array(vel_rc_to_cam)) 221 | write_flat(f, "TR_imu_to_velo", TR_imu_to_velo) 222 | logging.info("Wrote all calibration matrices to %s", filename) 223 | -------------------------------------------------------------------------------- /bounding_box.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | import sys 4 | 5 | try: 6 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 7 | sys.version_info.major, 8 | sys.version_info.minor, 9 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 10 | except IndexError: 11 | pass 12 | 13 | import carla 14 | 15 | 16 | 17 | from datadescriptor import KittiDescriptor 18 | from camera_utils import * 19 | from constants import WINDOW_HEIGHT, WINDOW_WIDTH, MAX_RENDER_DEPTH_IN_METERS, MIN_VISIBLE_VERTICES_FOR_RENDER, VISIBLE_VERTEX_COLOR, OCCLUDED_VERTEX_COLOR, MIN_BBOX_AREA_IN_PX 20 | from utils import degrees_to_radians 21 | import logging 22 | from image_converter import depth_to_array 23 | import numpy as np 24 | 25 | 26 | def transform_points(transform, points): 27 | """ 28 | Given a 4x4 transformation matrix, transform an array of 3D points. 29 | Expected point foramt: [[X0,Y0,Z0],..[Xn,Yn,Zn]] 30 | """ 31 | # Needed foramt: [[X0,..Xn],[Z0,..Zn],[Z0,..Zn]]. So let's transpose 32 | # the point matrix. 33 | 34 | points = points.transpose() 35 | # Add 0s row: [[X0..,Xn],[Y0..,Yn],[Z0..,Zn],[0,..0]] 36 | points = np.append(points, np.ones((1, points.shape[1])), axis=0) 37 | # Point transformation 38 | points = np.mat(transform.get_matrix()) * points 39 | # Return all but last row 40 | return points[0:3].transpose() 41 | 42 | 43 | def bbox_2d_from_agent(agent, intrinsic_mat, extrinsic_mat, ext, bbox_transform, agent_transform, rotRP): # rotRP expects point to be in Kitti lidar format 44 | """ Creates bounding boxes for a given agent and camera/world calibration matrices. 45 | Returns the modified image that contains the screen rendering with drawn on vertices from the agent """ 46 | bbox = vertices_from_extension(ext) 47 | # transform the vertices respect to the bounding box transform 48 | bbox = transform_points(bbox_transform, bbox) 49 | # the bounding box transform is respect to the agents transform 50 | # so let's transform the points relative to it's transform 51 | bbox = transform_points(agent_transform, bbox) 52 | # agents's transform is relative to the world, so now, 53 | # bbox contains the 3D bounding box vertices relative to the world 54 | # Additionally, you can logging.info these vertices to check that is working 55 | # Store each vertex 2d points for drawing bounding boxes later 56 | vertices_pos2d = vertices_to_2d_coords(bbox, intrinsic_mat, extrinsic_mat) 57 | return vertices_pos2d 58 | 59 | 60 | def calculate_occlusion_stats(image, vertices_pos2d, depth_map, draw_vertices=True): 61 | """ Draws each vertex in vertices_pos2d if it is in front of the camera 62 | The color is based on whether the object is occluded or not. 63 | Returns the number of visible vertices and the number of vertices outside the camera. 64 | """ 65 | num_visible_vertices = 0 66 | num_vertices_outside_camera = 0 67 | 68 | for y_2d, x_2d, vertex_depth in vertices_pos2d: 69 | # if the point is in front of the camera but not too far away 70 | if MAX_RENDER_DEPTH_IN_METERS > vertex_depth > 0 and point_in_canvas((y_2d, x_2d)): 71 | is_occluded = point_is_occluded( 72 | (y_2d, x_2d), vertex_depth, depth_map) 73 | if is_occluded: 74 | vertex_color = OCCLUDED_VERTEX_COLOR 75 | else: 76 | num_visible_vertices += 1 77 | vertex_color = VISIBLE_VERTEX_COLOR 78 | if draw_vertices: 79 | draw_rect(image, (y_2d, x_2d), 4, vertex_color) 80 | else: 81 | num_vertices_outside_camera += 1 82 | return num_visible_vertices, num_vertices_outside_camera 83 | 84 | 85 | def create_kitti_datapoint_ego(agent, intrinsic_mat, extrinsic_mat, image, depth_image, player, rotRP, draw_3D_bbox=True): 86 | """ Calculates the bounding box of the given agent, and returns a KittiDescriptor which describes the object to 87 | be labeled """ 88 | obj_type, agent_transform, bbox_transform, ext, location = transforms_from_agent( 89 | agent) 90 | 91 | if obj_type is None: 92 | logging.warning( 93 | "Could not get bounding box for agent. Object type is None") 94 | return image, None 95 | vertices_pos2d = bbox_2d_from_agent( 96 | agent, intrinsic_mat, extrinsic_mat, ext, bbox_transform, agent_transform, rotRP) 97 | depth_map = depth_to_array(depth_image) 98 | num_visible_vertices, num_vertices_outside_camera = calculate_occlusion_stats( 99 | image, vertices_pos2d, depth_map, draw_vertices=draw_3D_bbox) 100 | 101 | midpoint = midpoint_from_agent_location( 102 | image, location, extrinsic_mat, intrinsic_mat) 103 | x, y, z = [float(x) for x in midpoint][0:3] 104 | 105 | flag=0 106 | from math import pi 107 | datapoint_c = KittiDescriptor() 108 | if x*x <= 10000 and y*y < 1600: 109 | flag=1 110 | 111 | datapoint_c.set_bbox([0,0,0,0]) 112 | rotation_y = get_relative_rotation_y(agent, player) % pi # 取余数 113 | datapoint_c.set_3d_object_dimensions(ext) 114 | datapoint_c.set_type(obj_type) 115 | datapoint_c.set_3d_object_location(midpoint) 116 | datapoint_c.set_rotation_y(rotation_y) 117 | if num_visible_vertices >= MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 118 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 119 | datapoint_c.set_bbox(bbox_2d) 120 | 121 | # At least N vertices has to be visible in order to draw bbox 122 | if num_visible_vertices >= MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 123 | flag=2 124 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 125 | area = calc_bbox2d_area(bbox_2d) 126 | if area < MIN_BBOX_AREA_IN_PX: 127 | logging.info("Filtered out bbox with too low area {}".format(area)) 128 | return image, None 129 | if draw_3D_bbox: 130 | draw_3d_bounding_box(image, vertices_pos2d) 131 | 132 | rotation_y = get_relative_rotation_y(agent, player) % pi # 取余数 133 | 134 | datapoint = KittiDescriptor() 135 | datapoint.set_bbox(bbox_2d) 136 | datapoint.set_3d_object_dimensions(ext) 137 | datapoint.set_type(obj_type) 138 | datapoint.set_3d_object_location(midpoint) 139 | datapoint.set_rotation_y(rotation_y) 140 | 141 | if num_visible_vertices < MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 142 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 143 | area = calc_bbox2d_area(bbox_2d) 144 | if area < MIN_BBOX_AREA_IN_PX: 145 | logging.info("Filtered out bbox with too low area {}".format(area)) 146 | return image, None 147 | if draw_3D_bbox: 148 | draw_3d_invisible_bounding_box(image, vertices_pos2d) 149 | 150 | if flag==2: 151 | return image, datapoint_c,datapoint 152 | elif flag==1: 153 | return image,datapoint_c, None 154 | else: 155 | return image, None, None 156 | 157 | 158 | def create_kitti_datapoint_ego_invi(agent, intrinsic_mat, extrinsic_mat, image, depth_image, player, rotRP, draw_3D_bbox=True): 159 | """ Calculates the bounding box of the given agent, and returns a KittiDescriptor which describes the object to 160 | be labeled """ 161 | obj_type, agent_transform, bbox_transform, ext, location = transforms_from_agent( 162 | agent) 163 | 164 | if obj_type is None: 165 | logging.warning( 166 | "Could not get bounding box for agent. Object type is None") 167 | return image, None 168 | vertices_pos2d = bbox_2d_from_agent( 169 | agent, intrinsic_mat, extrinsic_mat, ext, bbox_transform, agent_transform, rotRP) 170 | depth_map = depth_to_array(depth_image) 171 | num_visible_vertices, num_vertices_outside_camera = calculate_occlusion_stats( 172 | image, vertices_pos2d, depth_map, draw_vertices=False) 173 | 174 | midpoint = midpoint_from_agent_location( 175 | image, location, extrinsic_mat, intrinsic_mat) 176 | x, y, z = [float(x) for x in midpoint][0:3] 177 | 178 | flag=0 179 | from math import pi 180 | datapoint_c = KittiDescriptor() 181 | 182 | if num_visible_vertices < MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 183 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 184 | area = calc_bbox2d_area(bbox_2d) 185 | if area < MIN_BBOX_AREA_IN_PX: 186 | logging.info("Filtered out bbox with too low area {}".format(area)) 187 | return image, None 188 | if draw_3D_bbox: 189 | draw_3d_invisible_bounding_box(image, vertices_pos2d) 190 | return image 191 | 192 | def create_kitti_datapoint(agent, intrinsic_mat, extrinsic_mat, image, depth_image, player, rotRP, draw_3D_bbox=True): 193 | """ Calculates the bounding box of the given agent, and returns a KittiDescriptor which describes the object to 194 | be labeled """ 195 | obj_type, agent_transform, bbox_transform, ext, location = transforms_from_agent( 196 | agent) 197 | 198 | if obj_type is None: 199 | logging.warning( 200 | "Could not get bounding box for agent. Object type is None") 201 | return image, None 202 | vertices_pos2d = bbox_2d_from_agent( 203 | agent, intrinsic_mat, extrinsic_mat, ext, bbox_transform, agent_transform, rotRP) 204 | depth_map = depth_to_array(depth_image) 205 | num_visible_vertices, num_vertices_outside_camera = calculate_occlusion_stats( 206 | image, vertices_pos2d, depth_map, draw_vertices=draw_3D_bbox) 207 | 208 | midpoint = midpoint_from_agent_location( 209 | image, location, extrinsic_mat, intrinsic_mat) 210 | x, y, z = [float(x) for x in midpoint][0:3] 211 | 212 | flag=0 213 | from math import pi 214 | datapoint_c = KittiDescriptor() 215 | if x*x <= 10000 and y*y < 1600: 216 | flag=1 217 | 218 | datapoint_c.set_bbox([0,0,0,0]) 219 | rotation_y = get_relative_rotation_y(agent, player) % pi # 取余数 220 | datapoint_c.set_3d_object_dimensions(ext) 221 | datapoint_c.set_type(obj_type) 222 | datapoint_c.set_3d_object_location(midpoint) 223 | datapoint_c.set_rotation_y(rotation_y) 224 | if num_visible_vertices >= MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 225 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 226 | datapoint_c.set_bbox(bbox_2d) 227 | 228 | # At least N vertices has to be visible in order to draw bbox 229 | if num_visible_vertices >= MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 230 | flag=2 231 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 232 | area = calc_bbox2d_area(bbox_2d) 233 | if area < MIN_BBOX_AREA_IN_PX: 234 | logging.info("Filtered out bbox with too low area {}".format(area)) 235 | return image, None 236 | if draw_3D_bbox: 237 | draw_3d_bounding_box(image, vertices_pos2d) 238 | 239 | rotation_y = get_relative_rotation_y(agent, player) % pi # 取余数 240 | 241 | datapoint = KittiDescriptor() 242 | datapoint.set_bbox(bbox_2d) 243 | datapoint.set_3d_object_dimensions(ext) 244 | datapoint.set_type(obj_type) 245 | datapoint.set_3d_object_location(midpoint) 246 | datapoint.set_rotation_y(rotation_y) 247 | 248 | 249 | if flag==2: 250 | return image, datapoint_c,datapoint 251 | elif flag==1: 252 | return image,datapoint_c, None 253 | else: 254 | return image, None, None 255 | 256 | def create_kitti_datapoint_ori(agent, intrinsic_mat, extrinsic_mat, image, depth_image, player, rotRP, draw_3D_bbox=True): 257 | """ Calculates the bounding box of the given agent, and returns a KittiDescriptor which describes the object to 258 | be labeled """ 259 | obj_type, agent_transform, bbox_transform, ext, location = transforms_from_agent( 260 | agent) 261 | 262 | if obj_type is None: 263 | logging.warning( 264 | "Could not get bounding box for agent. Object type is None") 265 | return image, None 266 | vertices_pos2d = bbox_2d_from_agent( 267 | agent, intrinsic_mat, extrinsic_mat, ext, bbox_transform, agent_transform, rotRP) 268 | depth_map = depth_to_array(depth_image) 269 | num_visible_vertices, num_vertices_outside_camera = calculate_occlusion_stats( 270 | image, vertices_pos2d, depth_map, draw_vertices=draw_3D_bbox) 271 | 272 | midpoint = midpoint_from_agent_location( 273 | image, location, extrinsic_mat, intrinsic_mat) 274 | #x, y, z = [float(x) for x in midpoint][0:3] 275 | 276 | from math import pi 277 | 278 | # At least N vertices has to be visible in order to draw bbox 279 | if num_visible_vertices >= MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 280 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 281 | area = calc_bbox2d_area(bbox_2d) 282 | if area < MIN_BBOX_AREA_IN_PX: 283 | logging.info("Filtered out bbox with too low area {}".format(area)) 284 | return image, None 285 | if draw_3D_bbox: 286 | draw_3d_bounding_box(image, vertices_pos2d) 287 | 288 | rotation_y = get_relative_rotation_y(agent, player) % pi # 取余数 289 | 290 | datapoint = KittiDescriptor() 291 | datapoint.set_bbox(bbox_2d) 292 | datapoint.set_3d_object_dimensions(ext) 293 | datapoint.set_type(obj_type) 294 | datapoint.set_3d_object_location(midpoint) 295 | datapoint.set_rotation_y(rotation_y) 296 | 297 | return image,datapoint 298 | else: 299 | return image, None 300 | 301 | def create_kitti_datapoint_cloud(agent, intrinsic_mat, extrinsic_mat, image, depth_image, player, rotRP, draw_3D_bbox=True): 302 | """ Calculates the bounding box of the given agent, and returns a KittiDescriptor which describes the object to 303 | be labeled """ 304 | obj_type, agent_transform, bbox_transform, ext, location = transforms_from_agent( 305 | agent) 306 | 307 | if obj_type is None: 308 | logging.warning( 309 | "Could not get bounding box for agent. Object type is None") 310 | return image, None 311 | vertices_pos2d = bbox_2d_from_agent( 312 | agent, intrinsic_mat, extrinsic_mat, ext, bbox_transform, agent_transform, rotRP) 313 | depth_map = depth_to_array(depth_image) 314 | num_visible_vertices, num_vertices_outside_camera = calculate_occlusion_stats( 315 | image, vertices_pos2d, depth_map, draw_vertices=draw_3D_bbox) 316 | 317 | midpoint = midpoint_from_agent_location( 318 | image, location, extrinsic_mat, intrinsic_mat) 319 | # At least N vertices has to be visible in order to draw bbox 320 | x, y, z = [float(x) for x in midpoint][0:3] 321 | 322 | if x*x <= 10000 and y*y < 1600: 323 | datapoint = KittiDescriptor() 324 | datapoint.set_bbox([0,0,0,0]) 325 | if num_visible_vertices >= MIN_VISIBLE_VERTICES_FOR_RENDER and num_vertices_outside_camera < 4: 326 | bbox_2d = calc_projected_2d_bbox(vertices_pos2d) 327 | area = calc_bbox2d_area(bbox_2d) 328 | if area < MIN_BBOX_AREA_IN_PX: 329 | logging.info("Filtered out bbox with too low area {}".format(area)) 330 | return None 331 | 332 | datapoint.set_bbox(bbox_2d) 333 | from math import pi 334 | # xiu gai 335 | rotation_y = get_relative_rotation_y(agent, player) % pi # 取余数 336 | 337 | 338 | 339 | datapoint.set_3d_object_dimensions(ext) 340 | datapoint.set_type(obj_type) 341 | datapoint.set_3d_object_location(midpoint) 342 | datapoint.set_rotation_y(rotation_y) 343 | return datapoint 344 | else: 345 | return None 346 | 347 | 348 | def get_relative_rotation_y(agent, player): 349 | """ Returns the relative rotation of the agent to the camera in yaw 350 | The relative rotation is the difference between the camera rotation (on car) and the agent rotation""" 351 | # We only car about the rotation for the classes we do detection on 352 | if agent.get_transform(): 353 | rot_agent = agent.get_transform().rotation.yaw 354 | rot_car = player.get_transform().rotation.yaw 355 | return degrees_to_radians(rot_agent - rot_car) 356 | 357 | 358 | def vertices_to_2d_coords(bbox, intrinsic_mat, extrinsic_mat): 359 | """ Accepts a bbox which is a list of 3d world coordinates and returns a list 360 | of the 2d pixel coordinates of each vertex. 361 | This is represented as a tuple (y, x, d) where y and x are the 2d pixel coordinates 362 | while d is the depth. The depth can be used for filtering visible vertices. 363 | """ 364 | vertices_pos2d = [] 365 | 366 | for vertex in bbox: 367 | pos_vector = vertex_to_world_vector(vertex) 368 | # Camera coordinates 369 | transformed_3d_pos = proj_to_camera(pos_vector, extrinsic_mat) 370 | # 2d pixel coordinates 371 | 372 | pos2d = proj_to_2d(transformed_3d_pos, intrinsic_mat) 373 | # The actual rendered depth (may be wall or other object instead of vertex) 374 | vertex_depth = pos2d[2] 375 | # x_2d, y_2d = WINDOW_WIDTH - pos2d[0], WINDOW_HEIGHT - pos2d[1] 376 | x_2d, y_2d = pos2d[0], pos2d[1] 377 | vertices_pos2d.append((y_2d, x_2d, vertex_depth)) 378 | 379 | 380 | return vertices_pos2d 381 | 382 | 383 | def vertex_to_world_vector(vertex): 384 | """ Returns the coordinates of the vector in correct carla world format (X,Y,Z,1) """ 385 | return np.array([ 386 | [vertex[0, 0]], # [[X, 387 | [vertex[0, 1]], # Y, 388 | [vertex[0, 2]], # Z, 389 | [1.0] # 1.0]] 390 | ]) 391 | 392 | 393 | def vertices_from_extension(ext): 394 | """ Extraxts the 8 bounding box vertices relative to (0,0,0) 395 | https://github.com/carla-simulator/carla/commits/master/Docs/img/vehicle_bounding_box.png 396 | 8 bounding box vertices relative to (0,0,0) 397 | """ 398 | return np.array([ 399 | [ext.x, ext.y, ext.z], # Top left front 400 | [- ext.x, ext.y, ext.z], # Top left back 401 | [ext.x, - ext.y, ext.z], # Top right front 402 | [- ext.x, - ext.y, ext.z], # Top right back 403 | [ext.x, ext.y, - ext.z], # Bottom left front 404 | [- ext.x, ext.y, - ext.z], # Bottom left back 405 | [ext.x, - ext.y, - ext.z], # Bottom right front 406 | [- ext.x, - ext.y, - ext.z] # Bottom right back 407 | ]) 408 | 409 | 410 | def transforms_from_agent(agent): 411 | """ Returns the KITTI object type and transforms, locations and extension of the given agent """ 412 | if agent.type_id.find('walker') != -1: 413 | obj_type = 'Pedestrian' 414 | agent_transform = agent.get_transform() 415 | bbox_transform = carla.Transform(agent.bounding_box.location, agent.bounding_box.rotation) 416 | ext = agent.bounding_box.extent 417 | agent_transform.location.z-=ext.z 418 | bbox_transform.location.z+=ext.z 419 | location = agent.get_transform().location 420 | location.z-=ext.z 421 | #print("walker") 422 | #print(bbox_transform) 423 | elif agent.type_id.find('vehicle') != -1: 424 | obj_type = 'Car' 425 | agent_transform = agent.get_transform() 426 | bbox_transform = carla.Transform(agent.bounding_box.location, agent.bounding_box.rotation) 427 | ext = agent.bounding_box.extent 428 | location = agent.get_transform().location 429 | #print("car") 430 | #print(bbox_transform) 431 | else: 432 | return (None, None, None, None, None) 433 | return obj_type, agent_transform, bbox_transform, ext, location 434 | 435 | 436 | def calc_bbox2d_area(bbox_2d): 437 | """ Calculate the area of the given 2d bbox 438 | Input is assumed to be xmin, ymin, xmax, ymax tuple 439 | """ 440 | xmin, ymin, xmax, ymax = bbox_2d 441 | return (ymax - ymin) * (xmax - xmin) 442 | -------------------------------------------------------------------------------- /dolphins_generation.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | import sys 4 | 5 | try: 6 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 7 | sys.version_info.major, 8 | sys.version_info.minor, 9 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 10 | except IndexError: 11 | pass 12 | 13 | import carla 14 | import logging 15 | import math 16 | import pygame 17 | import random 18 | import queue 19 | import numpy as np 20 | from bounding_box import create_kitti_datapoint 21 | from constants import * 22 | import image_converter 23 | from dataexport import * 24 | 25 | 26 | """ OUTPUT FOLDER GENERATION """ 27 | PHASE = "Town00" 28 | OUTPUT_FOLDER = os.path.join("_out", PHASE) 29 | folders = ['calib', 'image_2', 'label_2', 'velodyne', 'planes','velodyne_r','image_r','label_r','velodyne_rc','image_rc','label_rc','loc','label_C_2','label_C_r','label_C_rc']#,'velodyne_rc2','image_rc2','label_rc2'] 30 | 31 | 32 | def maybe_create_dir(path): 33 | if not os.path.exists(directory): 34 | os.makedirs(directory) 35 | 36 | 37 | for folder in folders: 38 | directory = os.path.join(OUTPUT_FOLDER, folder) 39 | maybe_create_dir(directory) 40 | 41 | """ DATA SAVE PATHS """ 42 | GROUNDPLANE_PATH = os.path.join(OUTPUT_FOLDER, 'planes/{0:06}.txt') 43 | LIDAR_PATH = os.path.join(OUTPUT_FOLDER, 'velodyne/{0:06}.bin') 44 | LABEL_PATH = os.path.join(OUTPUT_FOLDER, 'label_2/{0:06}.txt') 45 | IMAGE_PATH = os.path.join(OUTPUT_FOLDER, 'image_2/{0:06}.png') 46 | CALIBRATION_PATH = os.path.join(OUTPUT_FOLDER, 'calib/{0:06}.txt') 47 | LIDAR_R_PATH = os.path.join(OUTPUT_FOLDER, 'velodyne_r/{0:06}.bin') 48 | LABEL_R_PATH = os.path.join(OUTPUT_FOLDER, 'label_r/{0:06}.txt') 49 | IMAGE_R_PATH = os.path.join(OUTPUT_FOLDER, 'image_r/{0:06}.png') 50 | LIDAR_RC_PATH = os.path.join(OUTPUT_FOLDER, 'velodyne_rc/{0:06}.bin') 51 | LABEL_RC_PATH = os.path.join(OUTPUT_FOLDER, 'label_rc/{0:06}.txt') 52 | IMAGE_RC_PATH = os.path.join(OUTPUT_FOLDER, 'image_rc/{0:06}.png') 53 | LOC_PATH = os.path.join(OUTPUT_FOLDER, 'loc/{0:06}.txt') 54 | LABEL_C_PATH = os.path.join(OUTPUT_FOLDER, 'label_C_2/{0:06}.txt') 55 | LABEL_C_R_PATH = os.path.join(OUTPUT_FOLDER, 'label_C_r/{0:06}.txt') 56 | LABEL_C_RC_PATH = os.path.join(OUTPUT_FOLDER, 'label_C_rc/{0:06}.txt') 57 | 58 | 59 | class SynchronyModel(object): 60 | def __init__(self): 61 | self.world, self.init_setting, self.client, self.traffic_manager = self._make_setting() 62 | self.blueprint_library = self.world.get_blueprint_library() 63 | self.non_player = [] 64 | self.actor_list = [] 65 | self.frame = None 66 | self.player = None 67 | self.road = None 68 | self.roadc = None 69 | self.captured_frame_no = self.current_captured_frame_num() 70 | self.sensors = [] 71 | self._queues = [] 72 | self.main_image = None 73 | self.depth_image = None 74 | self.point_cloud = None 75 | self.main_image_r = None 76 | self.depth_image_r = None 77 | self.point_cloud_r = None 78 | self.main_image_rc = None 79 | self.depth_image_rc = None 80 | self.point_cloud_rc = None 81 | self.loc_rot = None 82 | self.extrinsic = None 83 | self.extrinsic_r = None 84 | self.extrinsic_rc = None 85 | self.intrinsic, self.my_camera, self.rsu_camera, self.rsuc_camera = self._span_player() 86 | self._span_non_player() 87 | 88 | def __enter__(self): 89 | # set the sensor listener function 90 | def make_queue(register_event): 91 | q = queue.Queue() 92 | register_event(q.put) 93 | self._queues.append(q) 94 | 95 | make_queue(self.world.on_tick) 96 | for sensor in self.sensors: 97 | make_queue(sensor.listen) 98 | return self 99 | 100 | def current_captured_frame_num(self): 101 | # Figures out which frame number we currently are on 102 | # This is run once, when we start the simulator in case we already have a dataset. 103 | # The user can then choose to overwrite or append to the dataset. 104 | label_path = os.path.join(OUTPUT_FOLDER, 'label_2/') 105 | num_existing_data_files = len( 106 | [name for name in os.listdir(label_path) if name.endswith('.txt')]) 107 | print(num_existing_data_files) 108 | if num_existing_data_files == 0: 109 | return 0 110 | answer = "A" 111 | if answer.upper() == "O": 112 | logging.info( 113 | "Resetting frame number to 0 and overwriting existing") 114 | # Overwrite the data 115 | return 0 116 | logging.info("Continuing recording data on frame number {}".format( 117 | num_existing_data_files)) 118 | return num_existing_data_files 119 | 120 | def tick(self, timeout): 121 | # Drive the simulator to the next frame and get the data of the current frame 122 | self.frame = self.world.tick() 123 | data = [self._retrieve_data(q, timeout) for q in self._queues] 124 | assert all(x.frame == self.frame for x in data) 125 | 126 | return data 127 | 128 | def _retrieve_data(self, sensor_queue, timeout): 129 | while True: 130 | data = sensor_queue.get(timeout=timeout) 131 | if data.frame == self.frame: 132 | 133 | return data 134 | 135 | def __exit__(self, *args, **kwargs): 136 | # cover the world settings 137 | self.world.apply_settings(self.init_setting) 138 | 139 | def _make_setting(self): 140 | client = carla.Client('localhost', 2000) 141 | client.set_timeout(6.0) 142 | world = client.get_world() 143 | traffic_manager = client.get_trafficmanager(8000) 144 | traffic_manager.set_global_distance_to_leading_vehicle(1.0) 145 | traffic_manager.set_synchronous_mode(True) 146 | traffic_manager.set_hybrid_physics_mode(True) 147 | traffic_manager.set_hybrid_physics_radius(100) 148 | traffic_manager.set_respawn_dormant_vehicles(True) 149 | traffic_manager.set_boundaries_respawn_dormant_vehicles(21,70) 150 | # synchrony model and fixed time step 151 | init_setting = world.get_settings() 152 | settings = world.get_settings() 153 | settings.synchronous_mode = True 154 | settings.fixed_delta_seconds = 0.05 # 20 steps per second 155 | settings.actor_active_distance = 100 156 | world.apply_settings(settings) 157 | return world, init_setting, client, traffic_manager 158 | 159 | def _span_player(self): 160 | """create our target vehicle""" 161 | my_vehicle_bp = self.blueprint_library.filter("model3")[0] 162 | my_vehicle_bp.set_attribute('role_name', 'hero' ) 163 | location = carla.Location(190, 10, 0.5) 164 | rotation = carla.Rotation(0, 0, 0) 165 | transform_vehicle = carla.Transform(carla.Location(x=-48.642700, y=-19.353889, z=0.600000), carla.Rotation(pitch=0.000000, yaw=90.432304, roll=0.000000)) 166 | my_vehicle = self.world.spawn_actor(my_vehicle_bp, transform_vehicle) 167 | my_vehicle.set_autopilot(True) 168 | k, my_camera, rsu_camera, rsuc_camera = self._span_sensor(my_vehicle) 169 | self.actor_list.append(my_vehicle) 170 | self.player = my_vehicle 171 | return k, my_camera, rsu_camera, rsuc_camera#, rsuc2_camera 172 | 173 | def _span_sensor(self, player): 174 | """create camera, depth camera and lidar and attach to the target vehicle""" 175 | camera_bp = self.blueprint_library.find('sensor.camera.rgb') 176 | camera_d_bp = self.blueprint_library.find('sensor.camera.depth') 177 | lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast') 178 | 179 | camera_bp.set_attribute('image_size_x', str(WINDOW_WIDTH)) 180 | camera_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT)) 181 | 182 | camera_d_bp.set_attribute('image_size_x', str(WINDOW_WIDTH)) 183 | camera_d_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT)) 184 | 185 | lidar_bp.set_attribute('range', '200') 186 | lidar_bp.set_attribute('rotation_frequency', '20') 187 | lidar_bp.set_attribute('upper_fov', '2') 188 | lidar_bp.set_attribute('lower_fov', '-24.8') 189 | lidar_bp.set_attribute('points_per_second', '2560000') 190 | lidar_bp.set_attribute('channels', '64') 191 | lidar_bp.set_attribute('horizontal_fov', '360') 192 | lidar_bp.set_attribute('dropoff_general_rate', '0.1') 193 | lidar_location = carla.Location(0, 0, 1.8) 194 | lidar_rotation = carla.Rotation(0, 0, 0) 195 | lidar_transform = carla.Transform(lidar_location, lidar_rotation) 196 | 197 | transform_sensor = carla.Transform(carla.Location(x=0, z=1.8)) 198 | 199 | my_camera = self.world.spawn_actor(camera_bp, transform_sensor, attach_to=player,attachment_type=carla.AttachmentType.Rigid) 200 | my_camera_d = self.world.spawn_actor(camera_d_bp, transform_sensor, attach_to=player,attachment_type=carla.AttachmentType.Rigid) 201 | my_lidar = self.world.spawn_actor(lidar_bp, lidar_transform, attach_to=player) 202 | 203 | rsu_camera_bp = self.blueprint_library.find('sensor.camera.rgb') 204 | rsu_camera_bp.set_attribute("image_size_x",str(WINDOW_WIDTH)) 205 | rsu_camera_bp.set_attribute("image_size_y",str(WINDOW_HEIGHT)) 206 | # camera relative position related to the vehicle 207 | rsu_camera_transform = carla.Transform(carla.Location(x=-65.000000, y=5.000000, z=4.000000), carla.Rotation(pitch=0.000000, yaw=0.000000, roll=0.000000)) 208 | rsu_camera = self.world.spawn_actor(rsu_camera_bp, rsu_camera_transform) 209 | self.road = rsu_camera 210 | 211 | rsu_camera_d_bp = self.blueprint_library.find('sensor.camera.depth') 212 | rsu_camera_d_bp.set_attribute('image_size_x', str(WINDOW_WIDTH)) 213 | rsu_camera_d_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT)) 214 | rsu_camera_d_transform = carla.Transform(carla.Location(x=-65.000000, y=5.000000, z=4.000000), carla.Rotation(pitch=0.000000, yaw=0.000000, roll=0.000000)) 215 | rsu_camera_d = self.world.spawn_actor(rsu_camera_d_bp, rsu_camera_d_transform) 216 | 217 | rsu_lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast') 218 | rsu_lidar_bp.set_attribute('channels', str(64)) 219 | rsu_lidar_bp.set_attribute('points_per_second', str(2560000)) 220 | rsu_lidar_bp.set_attribute('rotation_frequency', str(20)) 221 | rsu_lidar_bp.set_attribute('range', str(200)) 222 | rsu_lidar_bp.set_attribute('dropoff_general_rate', '0.1') 223 | rsu_lidar_bp.set_attribute('horizontal_fov', '360') 224 | rsu_lidar_bp.set_attribute('upper_fov', str(0)) 225 | rsu_lidar_bp.set_attribute('lower_fov', str(-40)) 226 | rsu_lidar_location = carla.Location(-65, 5, 4) 227 | rsu_lidar_rotation = carla.Rotation(0, 0, 0) 228 | rsu_lidar_transform = carla.Transform(rsu_lidar_location, rsu_lidar_rotation) 229 | # spawn the lidar 230 | rsu_lidar = self.world.spawn_actor(rsu_lidar_bp, rsu_lidar_transform) 231 | 232 | rsuc_camera_bp = self.blueprint_library.find('sensor.camera.rgb') 233 | rsuc_camera_bp.set_attribute("image_size_x",str(WINDOW_WIDTH)) 234 | rsuc_camera_bp.set_attribute("image_size_y",str(WINDOW_HEIGHT)) 235 | # camera relative position related to the vehicle 236 | rsuc_camera_transform = carla.Transform(carla.Location(x=-36.000000, y=10.000000, z=4.000000), carla.Rotation(pitch=0.000000, yaw=135.000000, roll=0.000000)) 237 | rsuc_camera = self.world.spawn_actor(rsuc_camera_bp, rsuc_camera_transform) 238 | self.roadc = rsuc_camera 239 | 240 | rsuc_camera_d_bp = self.blueprint_library.find('sensor.camera.depth') 241 | rsuc_camera_d_bp.set_attribute('image_size_x', str(WINDOW_WIDTH)) 242 | rsuc_camera_d_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT)) 243 | rsuc_camera_d_transform = carla.Transform(carla.Location(x=-36.000000, y=10.000000, z=4.000000), carla.Rotation(pitch=0.000000, yaw=135.000000, roll=0.000000)) 244 | rsuc_camera_d = self.world.spawn_actor(rsuc_camera_d_bp, rsuc_camera_d_transform) 245 | 246 | rsuc_lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast') 247 | rsuc_lidar_bp.set_attribute('channels', str(64)) 248 | rsuc_lidar_bp.set_attribute('points_per_second', str(2560000)) 249 | rsuc_lidar_bp.set_attribute('rotation_frequency', str(20)) 250 | rsuc_lidar_bp.set_attribute('range', str(200)) 251 | rsuc_lidar_bp.set_attribute('dropoff_general_rate', '0.1') 252 | rsuc_lidar_bp.set_attribute('horizontal_fov', '360') 253 | rsuc_lidar_bp.set_attribute('upper_fov', str(0)) 254 | rsuc_lidar_bp.set_attribute('lower_fov', str(-40)) 255 | rsuc_lidar_location = carla.Location(-36, 10, 4) 256 | rsuc_lidar_rotation = carla.Rotation(0, 135, 0) 257 | rsuc_lidar_transform = carla.Transform(rsuc_lidar_location, rsuc_lidar_rotation) 258 | # spawn the lidar 259 | rsuc_lidar = self.world.spawn_actor(rsuc_lidar_bp, rsuc_lidar_transform) 260 | 261 | self.actor_list.append(my_camera) 262 | self.actor_list.append(my_camera_d) 263 | self.actor_list.append(my_lidar) 264 | self.actor_list.append(rsu_camera) 265 | self.actor_list.append(rsu_camera_d) 266 | self.actor_list.append(rsu_lidar) 267 | self.actor_list.append(rsuc_camera) 268 | self.actor_list.append(rsuc_camera_d) 269 | self.actor_list.append(rsuc_lidar) 270 | self.sensors.append(my_camera) 271 | self.sensors.append(my_camera_d) 272 | self.sensors.append(my_lidar) 273 | self.sensors.append(rsu_camera) 274 | self.sensors.append(rsu_camera_d) 275 | self.sensors.append(rsu_lidar) 276 | self.sensors.append(rsuc_camera) 277 | self.sensors.append(rsuc_camera_d) 278 | self.sensors.append(rsuc_lidar) 279 | 280 | # camera intrinsic 281 | k = np.identity(3) 282 | k[0, 2] = WINDOW_WIDTH_HALF 283 | k[1, 2] = WINDOW_HEIGHT_HALF 284 | f = WINDOW_WIDTH / (2.0 * math.tan(90.0 * math.pi / 360.0)) 285 | k[0, 0] = k[1, 1] = f 286 | return k, my_camera,rsu_camera,rsuc_camera 287 | 288 | def _span_non_player(self): 289 | """create autonomous vehicles and people""" 290 | 291 | spawn_point_1 = carla.Transform(carla.Location(x=-48.642700, y=-8, z=0.600000), carla.Rotation(pitch=0.000000, yaw=90.432304, roll=0.000000)) 292 | blueprint_1 = random.choice(self.blueprint_library.filter(FILTERV)) 293 | if blueprint_1.has_attribute('color'): 294 | color = random.choice(blueprint_1.get_attribute('color').recommended_values) 295 | blueprint_1.set_attribute('color', color) 296 | if blueprint_1.has_attribute('driver_id'): 297 | driver_id = random.choice(blueprint_1.get_attribute('driver_id').recommended_values) 298 | blueprint_1.set_attribute('driver_id', driver_id) 299 | blueprint_1.set_attribute('role_name', 'autopilot') 300 | 301 | 302 | spawn_point_2=carla.Transform(carla.Location(x=-47.842700, y=0, z=0.600000), carla.Rotation(pitch=0.000000, yaw=90.432304, roll=0.000000)) 303 | blueprint_2 = random.choice(self.blueprint_library.filter(FILTERV)) 304 | if blueprint_2.has_attribute('color'): 305 | color = random.choice(blueprint_2.get_attribute('color').recommended_values) 306 | blueprint_2.set_attribute('color', color) 307 | if blueprint_2.has_attribute('driver_id'): 308 | driver_id = random.choice(blueprint_2.get_attribute('driver_id').recommended_values) 309 | blueprint_2.set_attribute('driver_id', driver_id) 310 | blueprint_2.set_attribute('role_name', 'autopilot') 311 | 312 | blueprints = self.world.get_blueprint_library().filter(FILTERV) 313 | blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] 314 | blueprints = [x for x in blueprints if not x.id.endswith('isetta')] 315 | blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] 316 | blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] 317 | blueprints = [x for x in blueprints if not x.id.endswith('t2')] 318 | blueprints = sorted(blueprints, key=lambda bp: bp.id) 319 | 320 | spawn_points_all = self.world.get_map().get_spawn_points() 321 | #number_of_spawn_points_all = len(spawn_points_all) 322 | spawn_points=[] 323 | for n, transform in enumerate(spawn_points_all): 324 | if (transform.location.x>-90) and (transform.location.x<-10) and (transform.location.y<60) and (transform.location.y>-30): 325 | spawn_points.append(spawn_points_all[n]) 326 | number_of_spawn_points = len(spawn_points) 327 | if NUM_OF_VEHICLES <= number_of_spawn_points: 328 | random.shuffle(spawn_points) 329 | number_of_vehicles = NUM_OF_VEHICLES 330 | elif NUM_OF_VEHICLES > number_of_spawn_points: 331 | msg = 'requested %d vehicles, but could only find %d spawn points' 332 | logging.warning(msg, NUM_OF_VEHICLES, number_of_spawn_points) 333 | number_of_vehicles = number_of_spawn_points 334 | 335 | # @todo cannot import these directly. 336 | SpawnActor = carla.command.SpawnActor 337 | 338 | batch = [] 339 | batch.append(SpawnActor(blueprint_1, spawn_point_1)) 340 | batch.append(SpawnActor(blueprint_2, spawn_point_2)) 341 | for n, transform in enumerate(spawn_points): 342 | if n >= number_of_vehicles: 343 | break 344 | blueprint = random.choice(blueprints) 345 | if blueprint.has_attribute('color'): 346 | color = random.choice(blueprint.get_attribute('color').recommended_values) 347 | blueprint.set_attribute('color', color) 348 | if blueprint.has_attribute('driver_id'): 349 | driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) 350 | blueprint.set_attribute('driver_id', driver_id) 351 | blueprint.set_attribute('role_name', 'autopilot') 352 | 353 | # spawn the cars and set their autopilot 354 | batch.append(SpawnActor(blueprint, transform)) 355 | 356 | vehicles_id = [] 357 | for response in self.client.apply_batch_sync(batch): 358 | if response.error: 359 | logging.error(response.error) 360 | else: 361 | vehicles_id.append(response.actor_id) 362 | vehicle_actors = self.world.get_actors(vehicles_id) 363 | self.non_player.extend(vehicle_actors) 364 | self.actor_list.extend(vehicle_actors) 365 | 366 | for i in vehicle_actors: 367 | i.set_autopilot(True, self.traffic_manager.get_port()) 368 | 369 | blueprintsWalkers = self.world.get_blueprint_library().filter(FILTERW) 370 | percentagePedestriansRunning = 0.0 # how many pedestrians will run 371 | percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road 372 | # 1. take all the random locations to spawn 373 | spawn_points = [] 374 | for i in range(NUM_OF_WALKERS): 375 | spawn_point = carla.Transform() 376 | loc = self.world.get_random_location_from_navigation() 377 | while (loc.x>-30) or (loc.x<-80) or (loc.y<-20) or (loc.y>20): 378 | loc = self.world.get_random_location_from_navigation() 379 | if (loc != None): 380 | spawn_point.location = loc 381 | spawn_points.append(spawn_point) 382 | # 2. we spawn the walker object 383 | batch = [] 384 | walker_speed = [] 385 | for spawn_point in spawn_points: 386 | walker_bp = random.choice(blueprintsWalkers) 387 | # set as not invincible 388 | if walker_bp.has_attribute('is_invincible'): 389 | walker_bp.set_attribute('is_invincible', 'false') 390 | # set the max speed 391 | if walker_bp.has_attribute('speed'): 392 | if (random.random() > percentagePedestriansRunning): 393 | # walking 394 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) 395 | else: 396 | # running 397 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) 398 | else: 399 | print("Walker has no speed") 400 | walker_speed.append(0.0) 401 | batch.append(SpawnActor(walker_bp, spawn_point)) 402 | results = self.client.apply_batch_sync(batch, True) 403 | walker_speed2 = [] 404 | walkers_list = [] 405 | all_id = [] 406 | walkers_id = [] 407 | for i in range(len(results)): 408 | if results[i].error: 409 | logging.error(results[i].error) 410 | else: 411 | walkers_list.append({"id": results[i].actor_id}) 412 | walker_speed2.append(walker_speed[i]) 413 | walker_speed = walker_speed2 414 | # 3. we spawn the walker controller 415 | batch = [] 416 | walker_controller_bp = self.world.get_blueprint_library().find('controller.ai.walker') 417 | for i in range(len(walkers_list)): 418 | batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) 419 | results = self.client.apply_batch_sync(batch, True) 420 | for i in range(len(results)): 421 | if results[i].error: 422 | logging.error(results[i].error) 423 | else: 424 | walkers_list[i]["con"] = results[i].actor_id 425 | # 4. we put altogether the walkers and controllers id to get the objects from their id 426 | for i in range(len(walkers_list)): 427 | all_id.append(walkers_list[i]["con"]) 428 | all_id.append(walkers_list[i]["id"]) 429 | all_actors = self.world.get_actors(all_id) 430 | 431 | for i in range(len(walkers_list)): 432 | walkers_id.append(walkers_list[i]["id"]) 433 | walker_actors = self.world.get_actors(walkers_id) 434 | self.non_player.extend(walker_actors) 435 | self.actor_list.extend(all_actors) 436 | self.world.tick() 437 | 438 | # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) 439 | # set how many pedestrians can cross the road 440 | self.world.set_pedestrians_cross_factor(percentagePedestriansCrossing) 441 | for i in range(0, len(all_id), 2): 442 | # start walker 443 | all_actors[i].start() 444 | # set walk to random point 445 | all_actors[i].go_to_location(self.world.get_random_location_from_navigation()) 446 | # max speed 447 | all_actors[i].set_max_speed(float(walker_speed[int(i / 2)])) 448 | 449 | print('spawned %d walkers and %d vehicles, press Ctrl+C to exit.' % (len(walkers_id), len(vehicles_id))) 450 | 451 | def _save_training_files(self, datapoints, point_cloud, datapoints_r, point_cloud_r, datapoints_rc, point_cloud_rc,loc,datapoints_c,datapoints_c_r,datapoints_c_rc):#datapoints_rc2, point_cloud_rc2): 452 | """ Save data in Kitti dataset format """ 453 | logging.info("Attempting to save at frame no {}, frame no: {}".format(self.frame, self.captured_frame_no)) 454 | groundplane_fname = GROUNDPLANE_PATH.format(self.captured_frame_no) 455 | lidar_fname = LIDAR_PATH.format(self.captured_frame_no) 456 | kitti_fname = LABEL_PATH.format(self.captured_frame_no) 457 | img_fname = IMAGE_PATH.format(self.captured_frame_no) 458 | calib_filename = CALIBRATION_PATH.format(self.captured_frame_no) 459 | img_r_fname = IMAGE_R_PATH.format(self.captured_frame_no) 460 | lidar_r_fname = LIDAR_R_PATH.format(self.captured_frame_no) 461 | kitti_r_fname = LABEL_R_PATH.format(self.captured_frame_no) 462 | img_rc_fname = IMAGE_RC_PATH.format(self.captured_frame_no) 463 | lidar_rc_fname = LIDAR_RC_PATH.format(self.captured_frame_no) 464 | kitti_rc_fname = LABEL_RC_PATH.format(self.captured_frame_no) 465 | loc_fname = LOC_PATH.format(self.captured_frame_no) 466 | kitti_c_fname = LABEL_C_PATH.format(self.captured_frame_no) 467 | kitti_c_r_fname = LABEL_C_R_PATH.format(self.captured_frame_no) 468 | kitti_c_rc_fname = LABEL_C_RC_PATH.format(self.captured_frame_no) 469 | 470 | save_loc_data_0(loc_fname, loc) 471 | 472 | save_groundplanes( 473 | groundplane_fname, self.player, LIDAR_HEIGHT_POS) 474 | save_ref_files(OUTPUT_FOLDER, self.captured_frame_no) 475 | save_image_data( 476 | img_fname, self.main_image) 477 | save_image_data( 478 | img_r_fname, self.main_image_r) 479 | save_image_data( 480 | img_rc_fname, self.main_image_rc) 481 | 482 | save_kitti_data(kitti_fname, datapoints) 483 | save_kitti_data(kitti_r_fname, datapoints_r) 484 | save_kitti_data(kitti_rc_fname, datapoints_rc) 485 | save_kitti_data(kitti_c_fname, datapoints_c) 486 | save_kitti_data(kitti_c_r_fname, datapoints_c_r) 487 | save_kitti_data(kitti_c_rc_fname, datapoints_c_rc) 488 | 489 | save_calibration_matrices( 490 | calib_filename, self.intrinsic, self.extrinsic,self.extrinsic_r,self.extrinsic_rc) 491 | save_lidar_data(lidar_fname, point_cloud) 492 | save_lidar_data(lidar_r_fname, point_cloud_r) 493 | save_lidar_data(lidar_rc_fname, point_cloud_rc) 494 | 495 | def generate_datapoints(self, image): 496 | """ Returns a list of datapoints (labels and such) that are generated this frame together with the main image 497 | image """ 498 | datapoints = [] 499 | datapoints_c = [] 500 | image = image.copy() 501 | # Remove this 502 | rotRP = np.identity(3) 503 | # Stores all datapoints for the current frames 504 | for agent in self.non_player: 505 | if GEN_DATA: 506 | image, kitti_datapoint_c, kitti_datapoint = create_kitti_datapoint( 507 | agent, self.intrinsic, self.extrinsic, image, self.depth_image, self.player, rotRP) 508 | if kitti_datapoint: 509 | datapoints.append(kitti_datapoint) 510 | if kitti_datapoint_c: 511 | datapoints_c.append(kitti_datapoint_c) 512 | 513 | return image, datapoints, datapoints_c 514 | 515 | def generate_datapoints_r(self, image): 516 | """ Returns a list of datapoints (labels and such) that are generated this frame together with the main image 517 | image """ 518 | datapoints = [] 519 | datapoints_c = [] 520 | image = image.copy() 521 | # Remove this 522 | rotRP = np.identity(3) 523 | # Stores all datapoints for the current frames 524 | for agent in self.non_player: 525 | if GEN_DATA: 526 | image, kitti_datapoint_c, kitti_datapoint = create_kitti_datapoint( 527 | agent, self.intrinsic, self.extrinsic_r, image, self.depth_image_r, self.road, rotRP) 528 | if kitti_datapoint: 529 | datapoints.append(kitti_datapoint) 530 | if kitti_datapoint_c: 531 | datapoints_c.append(kitti_datapoint_c) 532 | 533 | return image, datapoints, datapoints_c 534 | 535 | def generate_datapoints_rc(self, image): 536 | """ Returns a list of datapoints (labels and such) that are generated this frame together with the main image 537 | image """ 538 | datapoints = [] 539 | datapoints_c = [] 540 | image = image.copy() 541 | # Remove this 542 | rotRP = np.identity(3) 543 | # Stores all datapoints for the current frames 544 | for agent in self.non_player: 545 | if GEN_DATA: 546 | image, kitti_datapoint_c, kitti_datapoint = create_kitti_datapoint( 547 | agent, self.intrinsic, self.extrinsic_rc, image, self.depth_image_rc, self.roadc, rotRP) 548 | if kitti_datapoint: 549 | datapoints.append(kitti_datapoint) 550 | if kitti_datapoint_c: 551 | datapoints_c.append(kitti_datapoint_c) 552 | 553 | return image, datapoints, datapoints_c 554 | 555 | 556 | 557 | def draw_image(surface, image, blend=False): 558 | array = image[:, :, ::-1] 559 | image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) 560 | if blend: 561 | image_surface.set_alpha(100) 562 | surface.blit(image_surface, (0, 0)) 563 | 564 | 565 | def should_quit(): 566 | for event in pygame.event.get(): 567 | if event.type == pygame.QUIT: 568 | return True 569 | elif event.type == pygame.KEYUP: 570 | if event.key == pygame.K_ESCAPE: 571 | return True 572 | return False 573 | 574 | 575 | def get_font(): 576 | fonts = [x for x in pygame.font.get_fonts()] 577 | default_font = 'ubuntumono' 578 | font = default_font if default_font in fonts else fonts[0] 579 | font = pygame.font.match_font(font) 580 | return pygame.font.Font(font, 14) 581 | 582 | 583 | def main(): 584 | 585 | pygame.init() 586 | display = pygame.display.set_mode( 587 | (WINDOW_WIDTH, WINDOW_HEIGHT), 588 | pygame.HWSURFACE | pygame.DOUBLEBUF) 589 | font = get_font() 590 | clock = pygame.time.Clock() 591 | 592 | with SynchronyModel() as sync_mode: 593 | try: 594 | step = 1 595 | stop_flag = 1 596 | while True: 597 | if should_quit(): 598 | break 599 | if stop_flag == 60: 600 | break 601 | clock.tick() 602 | snapshot, sync_mode.main_image, sync_mode.depth_image, sync_mode.point_cloud, sync_mode.main_image_r, sync_mode.depth_image_r, sync_mode.point_cloud_r, sync_mode.main_image_rc, sync_mode.depth_image_rc, sync_mode.point_cloud_rc = sync_mode.tick( 603 | timeout=2.0) 604 | 605 | sync_mode.loc_rot=sync_mode.player.get_transform() 606 | z=sync_mode.loc_rot.location.z#+1.8 607 | loc=[sync_mode.loc_rot.location.x,sync_mode.loc_rot.location.y,z,sync_mode.loc_rot.rotation.pitch,sync_mode.loc_rot.rotation.yaw,sync_mode.loc_rot.rotation.roll] 608 | 609 | image = image_converter.to_rgb_array(sync_mode.main_image) 610 | image_r = image_converter.to_rgb_array(sync_mode.main_image_r) 611 | image_rc = image_converter.to_rgb_array(sync_mode.main_image_rc) 612 | sync_mode.extrinsic = np.mat(sync_mode.my_camera.get_transform().get_matrix()) 613 | sync_mode.extrinsic_r = np.mat(sync_mode.rsu_camera.get_transform().get_matrix()) 614 | sync_mode.extrinsic_rc = np.mat(sync_mode.rsuc_camera.get_transform().get_matrix()) 615 | image, datapoints, datapoints_c = sync_mode.generate_datapoints(image) 616 | image_r, datapoints_r, datapoints_c_r = sync_mode.generate_datapoints_r(image_r) 617 | image_rc, datapoints_rc, datapoints_c_rc = sync_mode.generate_datapoints_rc(image_rc) 618 | 619 | if (datapoints or datapoints_r or datapoints_rc or datapoints_c or datapoints_c_r or datapoints_c_rc) and step % 10 == 0: 620 | data = np.copy(np.frombuffer(sync_mode.point_cloud.raw_data, dtype=np.dtype('f4'))) 621 | data_r = np.copy(np.frombuffer(sync_mode.point_cloud_r.raw_data, dtype=np.dtype('f4'))) 622 | data_rc = np.copy(np.frombuffer(sync_mode.point_cloud_rc.raw_data, dtype=np.dtype('f4'))) 623 | data = np.reshape(data, (int(data.shape[0] / 4), 4)) 624 | data_r = np.reshape(data_r, (int(data_r.shape[0] / 4), 4)) 625 | data_rc = np.reshape(data_rc, (int(data_rc.shape[0] / 4), 4)) 626 | # Isolate the 3D data 627 | points = data[:, :-1] 628 | points_r = data_r[:, :-1] 629 | points_rc = data_rc[:, :-1] 630 | sync_mode._save_training_files(datapoints, points, datapoints_r, points_r, datapoints_rc, points_rc,loc,datapoints_c,datapoints_c_r,datapoints_c_rc) 631 | sync_mode.captured_frame_no += 1 632 | stop_flag += 1 633 | 634 | step = step+1 635 | fps = round(1.0 / snapshot.timestamp.delta_seconds) 636 | 637 | draw_image(display, image) 638 | display.blit( 639 | font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), 640 | (8, 10)) 641 | display.blit( 642 | font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), 643 | (8, 28)) 644 | pygame.display.flip() 645 | finally: 646 | print('destroying actors.') 647 | for actor in sync_mode.actor_list: 648 | actor.destroy() 649 | pygame.quit() 650 | print('done.') 651 | 652 | 653 | if __name__ == '__main__': 654 | main() 655 | --------------------------------------------------------------------------------