Transforming radar point cloud to global map frame

[jhoorneman]

I working on labeling individual point cloud points using the binary road map as ground truth. For this, I’m trying to transform the point cloud in such a way that their coordinates correspond to the ones on the map. I assumed that when I use the sensor and ego pose transforms to get the global coordinate frame, this would be enough. However this doesn’t seem to be the case: when I plot my results and compare it to the render_sample_data function, the point cloud is in a completely different location. I’m using the following code:

pc = RadarPointCloud.from_file(osp.join(self.nusc.dataroot, sample_data['filename']))

ego_pose = self.nusc.get('ego_pose', sample_data['ego_pose_token'])
cal_sensor = self.nusc.get('calibrated_sensor', sample_data['calibrated_sensor_token'])

car_from_senor = transform_matrix(cal_sensor['translation'], Quaternion(cal_sensor['rotation']), inverse=False)

global_from_car = transform_matrix(ego_pose['translation'], Quaternion(ego_pose['rotation']), inverse=False)

# Combine tranformation matrices
global_from_sensor = np.dot(global_from_car, car_from_senor)

pc.transform(global_from_sensor)

Somewhere else in the code I compensate for the map resolution.
Am I missing some steps or is the code wrong in some way? Thanks in advance!

[holger-motional]

Comparing to https://github.com/nutonomy/nuscenes-devkit/blob/master/python-sdk/nuscenes/nuscenes.py#L749 , I think your code is correct. What does the output look like? Since you know the global ego pose translation you can just check whether the points are close (within ~200m) to that.

[jhoorneman]

I’ve added two plots. The first one, with white background, is a modified version of render_sample that only shows radar points.The second one is all radar point clouds transformed with the code I showed above, plotted on the same map. I’ve also added the code I use for plotting, but I don’t think there is a lot that can go wrong there.

The point cloud in the second image is the same as in the first and the ego vehicle is in the same spot with respect to the point cloud. But the point cloud itself is rotated, mirrored and in the wrong location.
You can use the three sort of outlier points in an L-shape as a reference to compare the pointclouds. In the first image they are on top of the main point cloud, in the second image in the bottom right.

The render_sample function has multiple transformation steps: on the point cloud in from_file_multisweep and in the use_flat_vehicle_coordinates section and on the map itself in render_ego_centric_map. Since it’s so spread out in the code, I have a hard time understanding what exactly is transformed and how.

This is the code I use to make the second plot. The pc_to_global function uses the code of my first post.

# List of all radar modalities
all_radar_modalities = ('RADAR_FRONT', 'RADAR_FRONT_LEFT', 'RADAR_FRONT_RIGHT', 'RADAR_BACK_LEFT',
                        'RADAR_BACK_RIGHT')

# if radar modalities are not specified use all
if radar_modalities is None:
    radar_modalities = all_radar_modalities

for modality in radar_modalities:
    if all_radar_modalities.count(modality) != 1:
        raise ValueError("Error: Invalid radar sensor modality!")

# get nuscenes tokens
sample = self.nusc.get('sample', sample_token)
sample_data_token = sample['data'][radar_modalities[0]]
sample_data = self.nusc.get('sample_data', sample_data_token)
ego_pose = self.nusc.get('ego_pose', sample_data['ego_pose_token'])
scene_token = sample['scene_token']
scene = self.nusc.get('scene', scene_token)
log = self.nusc.get('log', scene['log_token'])

# get map
filename = self.nusc.get('map', log['map_token'])['filename']
map_ = cv.imread(osp.join(self.nusc.dataroot, filename), cv.IMREAD_GRAYSCALE)

# get point cloud of all specified radar sensor modalities
pc = np.zeros([2, 0])
for channel in radar_modalities:
    sample_data_token = sample['data'][channel]
    pc = np.hstack([pc, self.pc_to_global(sample_data_token).points[:2, :] / 0.1])

# plot parameters
ego_location = np.array(ego_pose['translation']) / 0.1
point_scale = 3.0

# plot road map and point cloud
plt.figure()
plt.imshow(map_, cmap='gray')
plt.scatter(pc[0], pc[1], s=point_scale)
plt.plot(ego_location[0], ego_location[1], 'x', color='red')

[jhoorneman]

I sort of fixed it by making a function that does all the transformations on point clouds and the map that normally get done in render_sample_data, RadarPointCloud.from_file_multisweep and render_egocentric_map. It returns a correct plot (shown below), but it’s far from an elegant solution.

def transform_pc_and_map(self, sample_token: str, axes_limit: int = 150, base_directory: str = '',
                         use_contour: bool = True, do_plot: bool = False) -> tuple:
    def crop_image(image: np.array,
                   x_px: int,
                   y_px: int,
                   axes_limit_px: int) -> np.array:
        x_min = int(x_px - axes_limit_px)
        x_max = int(x_px + axes_limit_px)
        y_min = int(y_px - axes_limit_px)
        y_max = int(y_px + axes_limit_px)

        cropped_image = image[y_min:y_max, x_min:x_max]

        return cropped_image

    # nuscenes stuff
    scene_token, log_token = self.__tokens_from_sample(sample_token)
    current_sample_record = self.nusc.get('sample', sample_token)

    ref_channel = 'LIDAR_TOP'
    ref_sd_token = current_sample_record['data'][ref_channel]
    ref_sd_record = self.nusc.get('sample_data', ref_sd_token)
    ref_pose_record = self.nusc.get('ego_pose', ref_sd_record['ego_pose_token'])
    ref_cs_record = self.nusc.get('calibrated_sensor', ref_sd_record['calibrated_sensor_token'])

    radar_modalities = ('RADAR_FRONT', 'RADAR_FRONT_LEFT', 'RADAR_FRONT_RIGHT', 'RADAR_BACK_LEFT',
                        'RADAR_BACK_RIGHT')

    # get map information
    basic_mask: MapMask = self.nusc.get('map', self.nusc.get('log', log_token)['map_token'])['mask']
    if use_contour:
        mask_raster = self.__contour_mask_from_sample(sample_token, base_directory=base_directory)
    else:
        mask_raster = basic_mask.mask()

    # get radar point cloud
    points_total = np.zeros((4, 0))
    for mod in radar_modalities:
        # get data
        current_sd_token = current_sample_record['data'][mod]
        current_sd_record = self.nusc.get('sample_data', current_sd_token)
        current_pose_record = self.nusc.get('ego_pose', current_sd_record['ego_pose_token'])
        current_cs_record = self.nusc.get('calibrated_sensor', current_sd_record['calibrated_sensor_token'])

        pc = RadarPointCloud.from_file(osp.join(self.nusc.dataroot, current_sd_record['filename']))
        points = np.vstack((pc.points[:3, :], np.ones(pc.points.shape[1])))

        # Transformations done in from_file_multisweep
        # Homogeneous transform from ego car frame to reference frame.
        ref_from_car = transform_matrix(ref_cs_record['translation'], Quaternion(ref_cs_record['rotation']),
                                        inverse=True)
        # Homogeneous transformation matrix from global to _current_ ego car frame.
        car_from_global = transform_matrix(ref_pose_record['translation'], Quaternion(ref_pose_record['rotation']),
                                           inverse=True)
        global_from_car = transform_matrix(current_pose_record['translation'],
                                           Quaternion(current_pose_record['rotation']), inverse=False)
        # Homogeneous transformation matrix from sensor coordinate frame to ego car frame.
        car_from_current = transform_matrix(current_cs_record['translation'], Quaternion(current_cs_record['rotation']),
                                            inverse=False)
        # Fuse four transformation matrices into one and perform transform.
        trans_matrix = reduce(np.dot, [ref_from_car, car_from_global, global_from_car, car_from_current])
        points = trans_matrix.dot(points)
        points_total = np.hstack((points_total, points))

    # Transformations done in render_sample_data
    # Compute rotation between 3D vehicle pose and "flat" vehicle pose (parallel to global z plane).
    ref_to_ego = transform_matrix(translation=ref_cs_record['translation'],
                                  rotation=Quaternion(ref_cs_record["rotation"]))
    ego_yaw = Quaternion(ref_pose_record['rotation']).yaw_pitch_roll[0]
    rotation_vehicle_flat_from_vehicle = np.dot(
        Quaternion(scalar=np.cos(ego_yaw / 2), vector=[0, 0, np.sin(ego_yaw / 2)]).rotation_matrix,
        Quaternion(ref_pose_record['rotation']).inverse.rotation_matrix)
    vehicle_flat_from_vehicle = np.eye(4)
    vehicle_flat_from_vehicle[:3, :3] = rotation_vehicle_flat_from_vehicle
    viewpoint = np.dot(vehicle_flat_from_vehicle, ref_to_ego)
    points_total = viewpoint.dot(points_total)

    # Transformations done in render_egocentric_map
    # Retrieve and crop mask.
    pixel_coords = basic_mask.to_pixel_coords(ref_pose_record['translation'][0],
                                              ref_pose_record['translation'][1])
    scaled_limit_px = int(axes_limit * (1.0 / basic_mask.resolution))
    # noinspection PyTypeChecker
    cropped = crop_image(mask_raster, pixel_coords[0], pixel_coords[1], int(scaled_limit_px * np.sqrt(2)))
    # Rotate image.
    ypr_rad = Quaternion(ref_pose_record['rotation']).yaw_pitch_roll
    yaw_deg = -np.degrees(ypr_rad[0])
    rotated_cropped = np.array(Image.fromarray(cropped).rotate(yaw_deg))
    # Crop image.
    ego_centric_map = crop_image(rotated_cropped, rotated_cropped.shape[1] / 2,
                                 rotated_cropped.shape[0] / 2,
                                 scaled_limit_px)

    # Transformation done to compensate for not using the matplotlib.imshow extent paramater
    points_total += np.array((axes_limit, axes_limit, 0, 0)).reshape((4, 1))
    points_total /= basic_mask.resolution
    ego_centric_map = np.flipud(ego_centric_map)

    # Plotting
    if do_plot:
        plt.figure()
        plt.imshow(ego_centric_map, cmap='gray', vmin=0, vmax=255)
        plt.scatter(points_total[0, :], points_total[1, :], s=3)
        plt.plot(axes_limit / basic_mask.resolution, axes_limit / basic_mask.resolution, 'x', color='red')

    return points_total[:3, :], ego_centric_map

[holger-motional]

I am glad you were able to fix it. In general I recommend reusing as much of our code as possible. If you have suggestions how to make our devkit more usable, let me know.

[jhoorneman]

I also figured out the more elegant solution now! The approach in my first post was correct, except that dividing the coordinates by 0.1 to compensate for the map resolution wasn’t enough. The origin of the map coordinates lies in the bottom left corner, instead of top left, so that has to get compensated as well. The function to_pixel_coords() does both those compensations.

If other people want to transform a point cloud to the map frame of reference:

def transform_pc(self, sample_data_token):
    _, _, log_token = self.__tokens_from_sample_data(sample_data_token)
    sample_data = self.nusc.get('sample_data', sample_data_token)
    ego_pose = self.nusc.get('ego_pose', sample_data['ego_pose_token'])
    cal_sensor = self.nusc.get('calibrated_sensor', sample_data['calibrated_sensor_token'])

    pc = RadarPointCloud.from_file(osp.join(self.nusc.dataroot, sample_data['filename']))
    basic_mask: MapMask = self.nusc.get('map', self.nusc.get('log', log_token)['map_token'])['mask']

    car_from_senor = transform_matrix(cal_sensor['translation'], Quaternion(cal_sensor['rotation']), inverse=False)
    global_from_car = transform_matrix(ego_pose['translation'], Quaternion(ego_pose['rotation']), inverse=False)

    # Combine tranformation matrices
    global_from_sensor = np.dot(global_from_car, car_from_senor)
    
    # Transform pointcloud
    pc.transform(global_from_sensor)

    # to map coordinates
    x, y = basic_mask.to_pixel_coords(pc.points[0], pc.points[1])
    points = np.vstack((x, y))
    return points

[holger-motional]

I’m glad you were able to resolve it.

[tomb]

This is really helpful. Thank you!