p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Restructured and commented few things

This commit is contained in:
Marc Garcia Puig 2017-12-14 20:20:35 +01:00
parent 5121cfad0d
commit c81f4a8dc6
2 changed files with 68 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
PythonClient/carla/image_converter.py
View File

@ -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)

89
PythonClient/point_cloud_example.py
View File

@ -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 {}',