Add working lidar and camera export to kitti format

camera_utils.py

@@ -2,6 +2,8 @@
 from numpy.linalg import pinv, inv
 from datageneration import WINDOW_HEIGHT, WINDOW_WIDTH
 
+from carla import image_converter
+
 def calc_projected_2d_bbox(vertices_pos2d):
     """ Takes in all vertices in pixel projection and calculates min and max of all x and y coordinates.
         Returns left top, right bottom pixel coordinates for the 2d bounding box as a list of four values.
@@ -19,9 +21,9 @@ def midpoint_from_agent_location(array, location, extrinsic_mat, intrinsic_mat):
     # This is used since kitti treats this point as the location of the car
     midpoint_vector = np.array([
         [location.x],  # [[X,
-            [location.y],  #   Y,
-            [location.z],  #   Z,
-            [1.0]           #   1.0]]
+        [location.y],  #   Y,
+        [location.z],  #   Z,
+        [1.0]          #   1.0]]
     ])
     transformed_3d_midpoint = proj_to_camera(midpoint_vector, extrinsic_mat)
     return transformed_3d_midpoint
@@ -35,14 +37,14 @@ def proj_to_camera(pos_vector, extrinsic_mat):
 
 def proj_to_2d(camera_pos_vector, intrinsic_mat):
     # transform the points to 2D
-        pos2d = np.dot(intrinsic_mat, camera_pos_vector[:3])
-        # normalize the 2D points
-        pos2d = np.array([
-            pos2d[0] / pos2d[2],
-            pos2d[1] / pos2d[2],
-            pos2d[2]
-        ])
-        return pos2d
+    pos2d = np.dot(intrinsic_mat, camera_pos_vector[:3])
+    # normalize the 2D points
+    pos2d = np.array([
+        pos2d[0] / pos2d[2],
+        pos2d[1] / pos2d[2],
+        pos2d[2]
+    ])
+    return pos2d
 
 def draw_3d_bounding_box(array, vertices_pos2d, vertex_graph):
     """ Draws lines from each vertex to all connected vertices """
@@ -134,4 +136,23 @@ def point_is_occluded(point, vertex_depth, depth_map):
             else:
                 is_occluded.append(False)
     # Only say point is occluded if all four neighbours are closer to camera than vertex 
-    return all(is_occluded)
+    return all(is_occluded)
+
+def to_depth_array(depth_image, k):
+    """ Converts a raw depth image from Camera depth sensor to an array where each index 
+        is the depth value. 
+        This conversion is needed because the depth camera encodes depth in the RGB-values
+        as d = (R + G*256 + B*256*256)/(256*256*256 - 1) * FAR_DISTANCE
+        K is the intrinsic matrix
+    """
+    from numpy.matlib import repmat
+    far_distance_in_meters = 1000
+    # RGB image will have shape (WINDOW_HEIGHT, WINDOW_WIDTH, 3)
+    array = image_converter.to_bgra_array(depth_image)
+    array = array.astype(np.float32)
+    # Apply (R + G * 256 + B * 256 * 256) / (256 * 256 * 256 - 1).
+    normalized_depth = np.dot(array[:, :, :3], [65536.0, 256.0, 1.0])
+    normalized_depth /= 16777215.0  # (256.0 * 256.0 * 256.0 - 1.0)
+    depth = normalized_depth * far_distance_in_meters
+    return depth
+

carla_utils.py

@@ -0,0 +1,78 @@
+"""
+This file includes a number of helper functions for the main loop of the carla simulator.
+This includes printing measurements and steering.0
+"""
+
+from carla.client import VehicleControl
+from carla.util import print_over_same_line
+
+try:
+    from pygame.locals import K_DOWN
+    from pygame.locals import K_LEFT
+    from pygame.locals import K_RIGHT
+    from pygame.locals import K_SPACE
+    from pygame.locals import K_UP
+    from pygame.locals import K_a
+    from pygame.locals import K_d
+    from pygame.locals import K_p
+    from pygame.locals import K_q
+    from pygame.locals import K_r
+    from pygame.locals import K_s
+    from pygame.locals import K_w
+except ImportError:
+    raise RuntimeError('cannot import pygame, make sure pygame package is installed')
+
+class KeyboardHelper:
+
+    def get_keyboard_control(keys, is_on_reverse, enable_autopilot):
+        if keys[K_r]:
+            return None
+        control = VehicleControl()
+        if keys[K_LEFT] or keys[K_a]:
+            control.steer = -1.0
+        if keys[K_RIGHT] or keys[K_d]:
+            control.steer = 1.0
+        if keys[K_UP] or keys[K_w]:
+            control.throttle = 1.0
+        if keys[K_DOWN] or keys[K_s]:
+            control.brake = 1.0
+        if keys[K_SPACE]:
+            control.hand_brake = True
+        if keys[K_q]:
+            is_on_reverse = is_on_reverse
+        if keys[K_p]:
+            enable_autopilot = enable_autopilot
+        control.reverse = is_on_reverse
+        return control, is_on_reverse, enable_autopilot
+
+
+class MeasurementsDisplayHelper:
+    def print_player_measurements_map(player_measurements, map_position, lane_orientation, timer):
+        message = 'Step {step} ({fps:.1f} FPS): '
+        message += 'Map Position ({map_x:.1f},{map_y:.1f}) '
+        message += 'Lane Orientation ({ori_x:.1f},{ori_y:.1f}) '
+        message += '{speed:.2f} km/h, '
+        message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road'
+        message = message.format(
+            map_x = map_position[0],
+            map_y = map_position[1],
+            ori_x = lane_orientation[0],
+            ori_y = lane_orientation[1],
+            step =  timer.step,
+            fps =   timer.ticks_per_second(),
+            speed= player_measurements.forward_speed * 3.6,
+            other_lane = 100 * player_measurements.intersection_otherlane,
+            offroad = 100 * player_measurements.intersection_offroad)
+        print_over_same_line(message)
+
+    def print_player_measurements(player_measurements, timer):
+        message = 'Step {step} ({fps:.1f} FPS): '
+        message += '{speed:.2f} km/h, '
+        message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road'
+        message = message.format(
+            step=timer.step,
+            fps=timer.ticks_per_second(),
+            speed=player_measurements.forward_speed * 3.6,
+            other_lane=100 * player_measurements.intersection_otherlane,
+            offroad=100 * player_measurements.intersection_offroad)
+        print_over_same_line(message)

datadescriptor.py

@@ -74,17 +74,48 @@ def set_3d_object_dimensions(self, bbox_extent):
         self.dimensions = "{} {} {}".format(2*height, 2*width, 2*length)
 
     def set_3b_object_location(self, obj_location):
-        # x and y should be inverted since kitti expects positive x and y to be right and up, respectively
+        """ TODO: Change this to 
+            Converts the 3D object location from CARLA coordinates and saves them as KITTI coordinates in the object
+            In Unreal, the coordinate system of the engine is defined as, which is the same as the lidar points
+            z
+            ▲   ▲ x
+            |  /
+            | /
+            |/____> y
+            This is a left-handed coordinate system, with x being forward, y to the right and z up 
+            See also https://github.com/carla-simulator/carla/issues/498
+            However, the camera coordinate system for KITTI is defined as
+                ▲ z
+               /
+              /
+             /____> x
+            |
+            |
+            |
+            ▼
+            y 
+            This is a right-handed coordinate system with z being forward, x to the right and y down
+            Therefore, we have to make the following changes from Carla to Kitti
+            Carla: X   Y   Z
+            KITTI:-X  -Y   Z
+        """
         # Object location is four values (x, y, z, w). We only care about three of them (xyz)
-        xyz = [float(x) for x in obj_location][0:3]
+        x, y, z = [float(x) for x in obj_location][0:3]
         assert None not in [self.extent, self.type], "Extent and type must be set before location!"
         if self.type == "Pedestrian": 
             # Since the midpoint/location of the pedestrian is in the middle of the agent, while for car it is at the bottom
             # we need to subtract the bbox extent in the height direction when adding location of pedestrian.
-            xyz[1] -= self.extent[0]
-        xyz[0] *= -1
-        xyz[1] *= -1
-        self.location = " ".join(map(str, xyz))
+            y -= self.extent[0]
+        # Convert from Carla coordinate system to KITTI
+        # This works for AVOD (image)
+        #x *= -1
+        #y *= -1
+        #self.location = " ".join(map(str, [y, -z, x]))
+        self.location = " ".join(map(str, [-x, -y, z]))
+        # This works for SECOND (lidar)
+        #self.location = " ".join(map(str, [z, x, y]))
+        #self.location = " ".join(map(str, [z, x, -y]))
+
 
     def set_rotation_y(self, rotation_y: float):
         assert -pi <= rotation_y <= pi, "Rotation y must be in range [-pi..pi] - found {}".format(rotation_y) 
@@ -98,3 +129,4 @@ def __str__(self):
             bbox_format = " ".join([str(x) for x in self.bbox])
 
         return "{} {} {} {} {} {} {} {}".format(self.type, self.truncated, self.occluded, self.alpha, bbox_format, self.dimensions, self.location, self.rotation_y)    
+

dataexport.py

@@ -31,7 +31,7 @@ def save_groundplanes(planes_fname, player_measurements, lidar_height):
     with open(planes_fname, 'w') as f:
         f.write("# Plane\n")
         f.write("Width 4\n")
-        f.write("Plane 1\n")
+        f.write("Height 1\n")
         f.write("{} {}\n".format(" ".join(normal_vector), lidar_height))
     logging.info("Wrote plane data to %s", planes_fname)
 
@@ -41,23 +41,51 @@ def save_ref_files(OUTPUT_FOLDER, id):
     for name in ['train.txt', 'val.txt', 'trainval.txt']:
         path = os.path.join(OUTPUT_FOLDER, name)
         with open(path, 'a') as f:
-            f.write(id + '\n')
+            f.write(str(id) + '\n')
         logging.info("Wrote reference files to %s", path)
 
+
 def save_image_data(filename, image):
     logging.info("Wrote image data to %s", filename)
     # Convert to correct color format
     color_fmt = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
     cv2.imwrite(filename, color_fmt)
 
-def save_lidar_data(filename, lidar_measurement, format="bin"):
+
+def save_lidar_data(filename, lidar_measurement, lidar_to_car_transform, LIDAR_HEIGHT, format="bin"):
     """ Saves lidar data to given filename, according to the lidar data format.
         bin is used for KITTI-data format, while .ply is the regular point cloud format
+        In Unreal, the coordinate system of the engine is defined as, which is the same as the lidar points
+        z
+        ^   ^ x
+        |  /
+        | /
+        |/____> y
+        This is a left-handed coordinate system, with x being forward, y to the right and z up 
+        See also https://github.com/carla-simulator/carla/issues/498
+        However, the lidar coordinate system from KITTI is defined as
+              z
+              ^   ^ x
+              |  /
+              | /
+        y<____|/
+        Which is a right handed coordinate sylstem
+        Therefore, we need to flip the y axis of the lidar in order to get the correct lidar format for kitti.
+        
+        This corresponds to the following changes from Carla to Kitti
+            Carla: X   Y   Z
+            KITTI: X  -Y   Z
+        NOTE: We do not flip the coordinate system when saving to .ply.
     """
     logging.info("Wrote lidar data to %s", filename)
+    point_cloud = np.array(lidar_to_car_transform.transform_points(lidar_measurement.data))  # originally returns a matrix
+
     if format == "bin":
-        lidar_array = [[point.x, -point.z, -point.y, 1.0] for point in lidar_measurement.point_cloud]  # Hopefully correct format
+        lidar_array = [[point[0], -point[1], point[2] - LIDAR_HEIGHT, 1.0] for point in point_cloud]
         lidar_array = np.array(lidar_array).astype(np.float32)
+        print("Lidar min/max of x: ", lidar_array[:, 0].min(), lidar_array[:, 0].max())
+        print("Lidar min/max of y: ", lidar_array[:, 1].min(), lidar_array[:, 0].max())
+        print("Lidar min/max of z: ", lidar_array[:, 2].min(), lidar_array[:, 0].max())
         lidar_array.tofile(filename)
     else:
         lidar_measurement.point_cloud.save_to_disk(filename)
@@ -81,15 +109,23 @@ def save_calibration_matrices(filename, intrinsic_mat, extrinsic_mat):
                                      Point_Camera = P_cam * R0_rect *
                                                     Tr_velo_to_cam *
                                                     Point_Velodyne.
+        3x4    tr_imu_to_velo        Used to transform from imu to velodyne coordinate frame. This is not needed since we do not export
+                                     imu data.
     """
+    # KITTI format demands that we flatten in row-major order
     ravel_mode = 'C'
     P0 = intrinsic_mat
     P0 = np.column_stack((P0, np.array([0, 0, 0])))
     P0 = np.ravel(P0, order=ravel_mode)
-    R0 = np.identity(3) # NOTE! This assumes that the camera and lidar occupy the same position on the car!!
-    TR_velodyne = np.identity(3)
-    TR_velodyne= np.column_stack((TR_velodyne, np.array([0, 0, 1])))
-    Tr_imu_to_velo = np.identity(3)
+    R0 = np.identity(3)
+    TR_velodyne = np.array([[0, -1, 0], 
+                            [0, 0, -1],
+                            [1, 0, 0]])
+    # Add translation vector from velo to camera. This is 0 because the position of camera and lidar is equal in our configuration.
+    TR_velodyne= np.column_stack((TR_velodyne, np.array([0, 0, 0])))
+    TR_imu_to_velo = np.identity(3)
+    TR_imu_to_velo = np.column_stack((TR_imu_to_velo, np.array([0, 0, 0])))
+
     def write_flat(f, name, arr):
         f.write("{}: {}\n".format(name, ' '.join(map(str, arr.flatten(ravel_mode).squeeze()))))
 
@@ -99,5 +135,5 @@ def write_flat(f, name, arr):
             write_flat(f, "P" + str(i), P0)
         write_flat(f, "R0_rect", R0)
         write_flat(f, "Tr_velo_to_cam", TR_velodyne)
-        write_flat(f, "Tr_imu_to_velo", Tr_imu_to_velo)
-    logging.info("Wrote all calibration matrices to %s", filename)
+        write_flat(f, "TR_imu_to_velo", TR_imu_to_velo)
+    logging.info("Wrote all calibration matrices to %s", filename)