Restructured and commented few things

PythonClient/carla/image_converter.py

@@ -58,19 +58,19 @@ def labels_to_cityscapes_palette(image):
     Cityscapes palette.
     """
     classes = {
-        0: [0, 0, 0],        # None
+        0: [0, 0, 0],         # None
-        1: [70, 70, 70],     # Buildings
+        1: [70, 70, 70],      # Buildings
-        2: [190, 153, 153],  # Fences
+        2: [190, 153, 153],   # Fences
-        3: [72, 0, 90],      # Other
+        3: [72, 0, 90],       # Other
-        4: [220, 20, 60],    # Pedestrians
+        4: [220, 20, 60],     # Pedestrians
-        5: [153, 153, 153],  # Poles
+        5: [153, 153, 153],   # Poles
-        6: [157, 234, 50],   # RoadLines
+        6: [157, 234, 50],    # RoadLines
-        7: [128, 64, 128],   # Roads
+        7: [128, 64, 128],    # Roads
-        8: [244, 35, 232],   # Sidewalks
+        8: [244, 35, 232],    # Sidewalks
-        9: [107, 142, 35],   # Vegetation
+        9: [107, 142, 35],    # Vegetation
-        10: [0, 0, 255],     # Vehicles
+        10: [0, 0, 255],      # Vehicles
-        11: [102, 102, 156], # Walls
+        11: [102, 102, 156],  # Walls
-        12: [220, 220, 0]    # TrafficSigns
+        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)
+    u_coord = repmat(numpy.r_[image.width-1:-1:-1],
-    v_coord = repmat(numpy.c_[image.height-1:-1:-1], 1, image.width).reshape(pixel_length)
+                     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)
+    u_coord = repmat(numpy.r_[image.width-1:-1:-1],
-    v_coord = repmat(numpy.c_[image.height-1:-1:-1], 1, image.width).reshape(pixel_length)
+                     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
+    # Search for pixels where the depth is greater than max_depth to
-    max_depth_indexes = numpy.where(normalized_depth == 1.0)
+    # 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)

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]]
+            # Car to World transformation given the 3D local points
-            points_3d = points_3d.transpose()
+            # and the transformation matrix
-
+            points_3d = transform_points(car_to_world_matrix, points_3d)
-            # Car to World transformation
-            points_3d = transform_points(
-                car_to_world_matrix, points_3d).transpose()
 
+            # 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]
+    return points[0:3].transpose()
-
-
-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
 
 
 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 {}',