diff --git a/PythonClient/carla/image_converter.py b/PythonClient/carla/image_converter.py index e06d586f8..98ef715fa 100644 --- a/PythonClient/carla/image_converter.py +++ b/PythonClient/carla/image_converter.py @@ -58,19 +58,19 @@ def labels_to_cityscapes_palette(image): Cityscapes palette. """ classes = { - 0: [0, 0, 0], # None - 1: [70, 70, 70], # Buildings - 2: [190, 153, 153], # Fences - 3: [72, 0, 90], # Other - 4: [220, 20, 60], # Pedestrians - 5: [153, 153, 153], # Poles - 6: [157, 234, 50], # RoadLines - 7: [128, 64, 128], # Roads - 8: [244, 35, 232], # Sidewalks - 9: [107, 142, 35], # Vegetation - 10: [0, 0, 255], # Vehicles - 11: [102, 102, 156], # Walls - 12: [220, 220, 0] # TrafficSigns + 0: [0, 0, 0], # None + 1: [70, 70, 70], # Buildings + 2: [190, 153, 153], # Fences + 3: [72, 0, 90], # Other + 4: [220, 20, 60], # Pedestrians + 5: [153, 153, 153], # Poles + 6: [157, 234, 50], # RoadLines + 7: [128, 64, 128], # Roads + 8: [244, 35, 232], # Sidewalks + 9: [107, 142, 35], # Vegetation + 10: [0, 0, 255], # Vehicles + 11: [102, 102, 156], # Walls + 12: [220, 220, 0] # TrafficSigns } array = labels_to_array(image) result = numpy.zeros((array.shape[0], array.shape[1], 3)) @@ -88,7 +88,7 @@ def depth_to_array(image): array = array.astype(numpy.float32) # Apply (R + G * 256 + B * 256 * 256) / (256 * 256 * 256 - 1). normalized_depth = numpy.dot(array[:, :, :3], [65536.0, 256.0, 1.0]) - normalized_depth /= 16777215.0 # (256.0 * 256.0 * 256.0 - 1.0) + normalized_depth /= 16777215.0 # (256.0 * 256.0 * 256.0 - 1.0) return normalized_depth @@ -112,19 +112,22 @@ def depth_to_local_point_cloud(image): Convert an image containing CARLA encoded depth-map to a 2D array containing the 3D position (relative to the camera) of each pixel. """ - far = 100000.0 # centimeters + far = 100000.0 # centimeters normalized_depth = depth_to_array(image) # (Intrinsic) K Matrix k = numpy.identity(3) k[0, 2] = image.width / 2.0 k[1, 2] = image.height / 2.0 - k[0, 0] = k[1, 1] = image.width / (2.0 * math.tan(image.fov * math.pi / 360.0)) + k[0, 0] = k[1, 1] = image.width / \ + (2.0 * math.tan(image.fov * math.pi / 360.0)) # 2d pixel coordinates pixel_length = image.width * image.height - u_coord = repmat(numpy.r_[image.width-1:-1:-1], image.height, 1).reshape(pixel_length) - v_coord = repmat(numpy.c_[image.height-1:-1:-1], 1, image.width).reshape(pixel_length) + u_coord = repmat(numpy.r_[image.width-1:-1:-1], + image.height, 1).reshape(pixel_length) + v_coord = repmat(numpy.c_[image.height-1:-1:-1], + 1, image.width).reshape(pixel_length) normalized_depth = numpy.reshape(normalized_depth, pixel_length) # Search for sky pixels (where the depth is 1.0) to delete them @@ -144,29 +147,36 @@ def depth_to_local_point_cloud(image): # [[X1,Y1,Z1],[X2,Y2,Z2], ... [Xn,Yn,Zn]] return numpy.transpose(p3d) -def depth_to_local_colored_point_cloud(image, color): + +def depth_to_local_colored_point_cloud(image, color, max_depth=0.9): """ Convert an image containing CARLA encoded depth-map to a 2D array containing - the 3D position (relative to the camera) of each pixel. + the 3D position (relative to the camera) of each pixel and its corresponding + RGB color of an array. + "max_depth" is used to omit the points that are far enough """ - far = 100000.0 # max depth in centimeters + far = 100000.0 # max depth in centimeters normalized_depth = depth_to_array(image) # (Intrinsic) K Matrix k = numpy.identity(3) k[0, 2] = image.width / 2.0 k[1, 2] = image.height / 2.0 - k[0, 0] = k[1, 1] = image.width / (2.0 * math.tan(image.fov * math.pi / 360.0)) + k[0, 0] = k[1, 1] = image.width / \ + (2.0 * math.tan(image.fov * math.pi / 360.0)) # 2d pixel coordinates pixel_length = image.width * image.height - u_coord = repmat(numpy.r_[image.width-1:-1:-1], image.height, 1).reshape(pixel_length) - v_coord = repmat(numpy.c_[image.height-1:-1:-1], 1, image.width).reshape(pixel_length) + u_coord = repmat(numpy.r_[image.width-1:-1:-1], + image.height, 1).reshape(pixel_length) + v_coord = repmat(numpy.c_[image.height-1:-1:-1], + 1, image.width).reshape(pixel_length) color = color.reshape(pixel_length, 3) normalized_depth = numpy.reshape(normalized_depth, pixel_length) - # Search for sky pixels (where the depth is 1.0) to delete them - max_depth_indexes = numpy.where(normalized_depth == 1.0) + # Search for pixels where the depth is greater than max_depth to + # delete them + max_depth_indexes = numpy.where(normalized_depth > max_depth) normalized_depth = numpy.delete(normalized_depth, max_depth_indexes) u_coord = numpy.delete(u_coord, max_depth_indexes) v_coord = numpy.delete(v_coord, max_depth_indexes) @@ -180,5 +190,5 @@ def depth_to_local_colored_point_cloud(image, color): p3d *= normalized_depth * far # Formating the output to: - # [[X1,Y1,Z1],[X2,Y2,Z2], ... [Xn,Yn,Zn]] + # [[X1,Y1,Z1,R1,G1,B1],[X2,Y2,Z2,R2,G2,B2], ... [Xn,Yn,Zn,Rn,Gn,Bn]] return numpy.concatenate((numpy.transpose(p3d), color), axis=1) diff --git a/PythonClient/point_cloud_example.py b/PythonClient/point_cloud_example.py index 79176a367..19969c6f0 100755 --- a/PythonClient/point_cloud_example.py +++ b/PythonClient/point_cloud_example.py @@ -60,22 +60,18 @@ def run_carla_client(host, port, save_images_to_disk, image_filename_format): camera1.set_image_size(*image_size) camera1.set_position(*camera_local_pos) camera1.set_rotation(*camera_local_rotation) + settings.add_sensor(camera1) camera2 = Camera( 'CameraRGB', PostProcessing='SceneFinal', CameraFOV=fov) camera2.set_image_size(*image_size) camera2.set_position(*camera_local_pos) camera2.set_rotation(*camera_local_rotation) - - settings.add_sensor(camera1) settings.add_sensor(camera2) scene = client.load_settings(settings) # Start at location index id '0' - # client.start_episode(0) - number_of_player_starts = len(scene.player_start_spots) - player_start = random.randint(0, max(0, number_of_player_starts - 1)) client.start_episode(0) # Compute the camera transform matrix @@ -108,12 +104,10 @@ def run_carla_client(host, port, save_images_to_disk, image_filename_format): image.save_to_disk( image_filename_format.format(episode, name, frame)) - # 2d to (camera) local 3d + # 2d to (camera) local 3d ########################################## t = StopWatch() - # RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]] image_RGB = to_rgb_array(sensor_data['CameraRGB']) - # [[X0,Y0,Z0],..[Xn,Yn,Zn]] # points_3d = depth_to_local_point_cloud(sensor_data['CameraDepth']) @@ -121,35 +115,41 @@ def run_carla_client(host, port, save_images_to_disk, image_filename_format): points_3d = depth_to_local_colored_point_cloud( sensor_data['CameraDepth'], image_RGB) t.stop() - print('Transformation took {:.0f} ms'.format(t.milliseconds())) + print('2D to 3D took {:.0f} ms'.format(t.milliseconds())) - # (camera) local 3d to world 3d + # (camera) local 3d to world 3d #################################### t = StopWatch() - # Separating the 3d point and its corresponding color color_list = points_3d[:, 3:6] points_3d = points_3d[:, 0:3] + # Get the player transformation car_to_world = measurements.player_measurements.transform + + # Compute the final transformation matrix car_to_world_matrix = compute_transform_matrix( car_to_world) * camera_to_car_matrix - # We need the foramt [[X0,..Xn],[Z0,..Zn],[Z0,..Zn]] - points_3d = points_3d.transpose() - - # Car to World transformation - points_3d = transform_points( - car_to_world_matrix, points_3d).transpose() + # Car to World transformation given the 3D local points + # and the transformation matrix + points_3d = transform_points(car_to_world_matrix, points_3d) + # Merging again the 3D points (now in world space) with + # the previous colors points_3d = numpy.concatenate((points_3d, color_list), axis=1) + # Now we have an array of 3D points and colors + # [ [X1,Y1,Z1,R1,G1,B1], + # [X2,Y2,Z2,R2,G2,B2], + # ..., + # [Xn,Yn,Zn,Rn,Gn,Bn] ] t.stop() - print('Local to world transformation took {:.0f} ms'.format( + print('Local 3D to world 3D took {:.0f} ms'.format( t.milliseconds())) t = StopWatch() - # Save PLY to disk + # Save PLY to disk ################################################# print('Generaing PLY ...') ply = '\n'.join(['{:.2f} {:.2f} {:.2f} {:.0f} {:.0f} {:.0f}'.format( *p) for p in points_3d.tolist()]) @@ -159,7 +159,8 @@ def run_carla_client(host, port, save_images_to_disk, image_filename_format): os.makedirs(output_folder) print('Saving PLY ...') - with open('{}/{:0>4}.ply'.format(output_folder, frame), 'w+') as f: + # Open the file and save with the specific PLY format + with open('{}/{:0>5}.ply'.format(output_folder, frame), 'w+') as f: f.write( '\n'.join([construct_ply_header(len(points_3d), True), ply])) t.stop() @@ -214,52 +215,24 @@ def compute_transform_matrix_raw(x=0, y=0, z=0, pitch=0, roll=0, yaw=0, sX=1, sY def transform_points(transform_matrix, points): """ - Expected points format [[X0..,Xn],[Y0..,Yn],[Z0..,Zn]] + Given a 4x4 transformation matrix, transform an array of 3D points. + Expected foramt: [[X0,Y0,Z0],..[Xn,Yn,Zn]] """ - # Add 0s row so [[X0..,Xn],[Y0..,Yn],[Z0..,Zn],[0,..0]] + # Needed foramt: [[X0,..Xn],[Z0,..Zn],[Z0,..Zn]] + points = points.transpose() + # Add 0s row: [[X0..,Xn],[Y0..,Yn],[Z0..,Zn],[0,..0]] points = numpy.append(points, numpy.ones((1, points.shape[1])), axis=0) # Point transformation points = transform_matrix * points # Return all but last row - return points[0:3] - - -def transform_points_old(transform, points): - yaw = numpy.radians(transform.rotation.yaw) - roll = numpy.radians(-transform.rotation.roll) - pitch = numpy.radians(-transform.rotation.pitch) - - yaw_matrix = numpy.array([ - [math.cos(yaw), -math.sin(yaw), 0], - [math.sin(yaw), math.cos(yaw), 0], - [0, 0, 1] - ]) - - pitch_matrix = numpy.array([ - [math.cos(pitch), 0, -math.sin(pitch)], - [0, 1, 0], - [math.sin(pitch), 0, math.cos(pitch)] - ]) - - roll_matrix = numpy.array([ - [1, 0, 0], - [0, math.cos(roll), math.sin(roll)], - [0, -math.sin(roll), math.cos(roll)] - ]) - - rotation_matrix = numpy.dot( - yaw_matrix, numpy.dot(pitch_matrix, roll_matrix)) - - translation = numpy.array([ - [transform.location.x], - [transform.location.y], - [transform.location.z] - ]) - - return numpy.dot(rotation_matrix, points) + translation + return points[0:3].transpose() def construct_ply_header(points_number, colors=False): + """ + Generates a PLY header given a total number of 3D points and + coloring property if specified + """ header = ['ply', 'format ascii 1.0', 'element vertex {}',