def _get_radar_data(self, sample_rec: dict, sample_data: dict,
nsweeps: int) -> RadarPointCloud:
"""
Returns Radar point cloud in Vehicle Coordinates
:param sample_rec: sample record dictionary from nuscenes
:param sample_data: sample data dictionary from nuscenes
:param nsweeps: number of sweeps to return for this radar
:return pc: RadarPointCloud containing this samnple and all sweeps
"""
radar_path = os.path.join(self.nusc_path, sample_data['filename'])
cs_record = self.nusc.get('calibrated_sensor',
sample_data['calibrated_sensor_token'])
if nsweeps > 1:
## Returns in vehicle coordinates
pc, _ = RadarPointCloud.from_file_multisweep(
self.nusc,
sample_rec,
sample_data['channel'],
sample_data['channel'],
nsweeps=nsweeps,
min_distance=self.radar_min_distance)
else:
## Returns in sensor coordinates
pc = RadarPointCloud.from_file(radar_path)
## Sensor to vehicle
rot_matrix = Quaternion(cs_record['rotation']).rotation_matrix
pc.rotate(rot_matrix)
pc.translate(np.array(cs_record['translation']))
return pc
def get_data(sensor_data):
sd_record = nusc.get('sample_data', sensor_data)
sensor_modality = sd_record['sensor_modality']
nsweeps = 1
sample_rec = nusc.get('sample', sd_record['sample_token'])
ref_chan = 'LIDAR_TOP'
chan = sd_record['channel']
if sensor_modality == 'lidar':
pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=nsweeps)
points = pc.points
return points # (4, 24962) max 30000
elif sensor_modality == 'radar':
pc, times = RadarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=nsweeps)
points = pc.points
return points # (18, -1) # max 100
elif sensor_modality == 'camera':
data_path, boxes, camera_intrinsic = nusc.get_sample_data(
sensor_data) #, box_vis_level=box_vis_level)
data = Image.open(data_path)
img = np.array(data)
return img # (900, 1600, 3)
def test_load_pointclouds(self):
"""
Loads up lidar and radar pointclouds.
"""
assert 'NUSCENES' in os.environ, 'Set NUSCENES env. variable to enable tests.'
dataroot = os.environ['NUSCENES']
nusc = NuScenes(version='v1.0-mini', dataroot=dataroot, verbose=False)
sample_rec = nusc.sample[0]
lidar_name = nusc.get('sample_data',
sample_rec['data']['LIDAR_TOP'])['filename']
radar_name = nusc.get('sample_data',
sample_rec['data']['RADAR_FRONT'])['filename']
lidar_path = os.path.join(dataroot, lidar_name)
radar_path = os.path.join(dataroot, radar_name)
pc1 = LidarPointCloud.from_file(lidar_path)
pc2 = RadarPointCloud.from_file(radar_path)
pc3, _ = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
'LIDAR_TOP',
'LIDAR_TOP',
nsweeps=2)
pc4, _ = RadarPointCloud.from_file_multisweep(nusc,
sample_rec,
'RADAR_FRONT',
'RADAR_FRONT',
nsweeps=2)
# Check for valid dimensions.
assert pc1.points.shape[0] == pc3.points.shape[
0] == 4, 'Error: Invalid dimension for lidar pointcloud!'
assert pc2.points.shape[0] == pc4.points.shape[
0] == 18, 'Error: Invalid dimension for radar pointcloud!'
assert pc1.points.dtype == pc3.points.dtype, 'Error: Invalid dtype for lidar pointcloud!'
assert pc2.points.dtype == pc4.points.dtype, 'Error: Invalid dtype for radar pointcloud!'
def get_sensor_sample_data(nusc,
sample,
sensor_channel,
dtype=np.float32,
size=None):
"""
This function takes the token of a sample and a sensor sensor_channel and returns the according data
:param sample: the nuscenes sample dict
:param sensor_channel: the target sensor channel of the given sample to load the data from
:param dtype: the target numpy type
:param size: for resizing the image
Radar Format:
- Shape: 19 x n
- Semantics:
[0]: x (1)
[1]: y (2)
[2]: z (3)
[3]: dyn_prop (4)
[4]: id (5)
[5]: rcs (6)
[6]: vx (7)
[7]: vy (8)
[8]: vx_comp (9)
[9]: vy_comp (10)
[10]: is_quality_valid (11)
[11]: ambig_state (12)
[12]: x_rms (13)
[13]: y_rms (14)
[14]: invalid_state (15)
[15]: pdh0 (16)
[16]: vx_rms (17)
[17]: vy_rms (18)
[18]: distance (19)
"""
# Get filepath
sd_rec = nusc.get('sample_data', sample['data'][sensor_channel])
file_name = osp.join(nusc.dataroot, sd_rec['filename'])
# Check conditions
if not osp.exists(file_name):
raise FileNotFoundError("nuscenes data must be located in %s" %
file_name)
# Read the data
if "RADAR" in sensor_channel:
pcs, times = RadarPointCloud.from_file_multisweep(nusc, sample, sensor_channel, \
sensor_channel, nsweeps=required_sweep_count, min_distance=0.0) # Load radar points
data = pcs.points.astype(dtype)
else:
raise Exception("\"%s\" is not supported" % sensor_channel)
return data
def get_radar_pointcloud(self, channel='RADAR_FRONT'):
"""
Extracting radar detection pointcloud with velocities for current timestep in current scene for specified radar channel.
:param channel: Radar channel selection. Front radar as default
:return (Point cloud [Position(x,y,z) X n_points], Point cloud [Velocity(x,y,z) X n_points])
"""
# Check for correct radar channel selection
assert channel in self.radar_sensor_channels, " [!] Radar channel \"{}\" not found.".format(
channel)
# Select sensor data record
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
lidar_token = self.sample['data']['LIDAR_TOP']
# The point cloud is transformed to the lidar frame for visualization purposes.
ref_chan = 'LIDAR_TOP'
pc, times = RadarPointCloud.from_file_multisweep(self.nusc,
self.sample,
channel,
ref_chan,
nsweeps=1)
# Transform radar velocities (x is front, y is left), as these are not transformed when loading the point
# cloud.
radar_cs_record = self.nusc.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
lidar_sd_record = self.nusc.get('sample_data', lidar_token)
lidar_cs_record = self.nusc.get(
'calibrated_sensor', lidar_sd_record['calibrated_sensor_token'])
velocities = pc.points[8:10, :] # Compensated velocity
velocities = np.vstack((velocities, np.zeros(pc.points.shape[1])))
velocities = np.dot(
Quaternion(radar_cs_record['rotation']).rotation_matrix,
velocities)
velocities = np.dot(
Quaternion(lidar_cs_record['rotation']).rotation_matrix.T,
velocities)
velocities[2, :] = np.zeros(pc.points.shape[1])
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
points_vel = view_points(pc.points[:3, :] + velocities,
np.eye(4),
normalize=False)
print(" [*] Radar point cloud extracted from channel \"" + channel +
"\". Shape: " + str(points.shape))
return points, points_vel
def render_sample_data(self,
sample_data_token: str,
with_anns: bool = True,
box_vis_level: BoxVisibility = BoxVisibility.ANY,
axes_limit: float = 40,
ax: Axes = None,
nsweeps: int = 1) -> None:
"""
Render sample data onto axis.
:param sample_data_token: Sample_data token.
:param with_anns: Whether to draw annotations.
:param box_vis_level: If sample_data is an image, this sets required visibility for boxes.
:param axes_limit: Axes limit for lidar and radar (measured in meters).
:param ax: Axes onto which to render.
:param nsweeps: Number of sweeps for lidar and radar.
"""
# Get sensor modality.
sd_record = self.nusc.get('sample_data', sample_data_token)
sensor_modality = sd_record['sensor_modality']
if sensor_modality == 'lidar':
# Get boxes in lidar frame.
_, boxes, _ = self.nusc.get_sample_data(sample_data_token, box_vis_level=box_vis_level)
# Get aggregated point cloud in lidar frame.
sample_rec = self.nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
ref_chan = 'LIDAR_TOP'
pc, times = LidarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps)
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists/axes_limit/np.sqrt(2))
ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=(c, c, c))
# Limit visible range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'radar':
# Get boxes in lidar frame.
sample_rec = self.nusc.get('sample', sd_record['sample_token'])
lidar_token = sample_rec['data']['LIDAR_TOP']
_, boxes, _ = self.nusc.get_sample_data(lidar_token, box_vis_level=box_vis_level)
# Get aggregated point cloud in lidar frame.
# The point cloud is transformed to the lidar frame for visualization purposes.
chan = sd_record['channel']
ref_chan = 'LIDAR_TOP'
pc, times = RadarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps)
# Transform radar velocities (x is front, y is left), as these are not transformed when loading the point
# cloud.
radar_cs_record = self.nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token'])
lidar_sd_record = self.nusc.get('sample_data', lidar_token)
lidar_cs_record = self.nusc.get('calibrated_sensor', lidar_sd_record['calibrated_sensor_token'])
velocities = pc.points[8:10, :] # Compensated velocity
velocities = np.vstack((velocities, np.zeros(pc.points.shape[1])))
velocities = np.dot(Quaternion(radar_cs_record['rotation']).rotation_matrix, velocities)
velocities = np.dot(Quaternion(lidar_cs_record['rotation']).rotation_matrix.T, velocities)
velocities[2, :] = np.zeros(pc.points.shape[1])
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists / axes_limit / np.sqrt(2))
sc = ax.scatter(points[0, :], points[1, :], c=colors, s=3)
# Show velocities.
points_vel = view_points(pc.points[:3, :] + velocities, np.eye(4), normalize=False)
max_delta = 10
deltas_vel = points_vel - points
deltas_vel = 3 * deltas_vel # Arbitrary scaling
deltas_vel = np.clip(deltas_vel, -max_delta, max_delta) # Arbitrary clipping
colors_rgba = sc.to_rgba(colors)
for i in range(points.shape[1]):
ax.arrow(points[0, i], points[1, i], deltas_vel[0, i], deltas_vel[1, i], color=colors_rgba[i])
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=(c, c, c))
# Limit visible range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'camera':
# Load boxes and image.
data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(sample_data_token,
box_vis_level=box_vis_level)
data = Image.open(data_path)
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 16))
# Show image.
ax.imshow(data)
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=camera_intrinsic, normalize=True, colors=(c, c, c))
# Limit visible range.
ax.set_xlim(0, data.size[0])
ax.set_ylim(data.size[1], 0)
else:
raise ValueError("Error: Unknown sensor modality!")
ax.axis('off')
ax.set_title(sd_record['channel'])
ax.set_aspect('equal')
def create_annotations(self, sample_token, sensor_channels):
"""
Create annotations for the the given sample token.
1 bounding box vector contains:
:param sample_token: the sample_token to get the annotation for
:param sensor_channels: list of channels for cropping the labels, e.g. ['CAM_FRONT', 'RADAR_FRONT']
This works only for CAMERA atm
:returns:
annotations dictionary:
{
'labels': [] # <list of n int>
'bboxes': [] # <list of n x 4 float> [xmin, ymin, xmax, ymax]
'distances': [] # <list of n float> Center of box given as x, y, z.
'visibilities': [] # <list of n float> Visibility of annotated object
}
"""
if any([s for s in sensor_channels if 'RADAR' in s]):
print("[WARNING] Cropping to RADAR is not supported atm")
sensor_channels = [
c for c in sensor_channels if 'CAM' in sensor_channels
]
sample = self.nusc.get('sample', sample_token)
annotations_count = 0
annotations = {
'labels': [], # <list of n int>
'bboxes': [], # <list of n x 4 float> [xmin, ymin, xmax, ymax]
'distances':
[], # <list of n float> Center of box given as x, y, z.
'visibilities': [],
'num_radar_pts': [
] #<list of n int> number of radar points that cover that annotation
}
# Camera parameters
for selected_sensor_channel in sensor_channels:
sd_rec = self.nusc.get('sample_data',
sample['data'][selected_sensor_channel])
# Create Boxes:
_, boxes, camera_intrinsic = self.nusc.get_sample_data(
sd_rec['token'], box_vis_level=BoxVisibility.ANY)
imsize_src = (sd_rec['width'], sd_rec['height']
) # nuscenes has (width, height) convention
bbox_resize = [1. / sd_rec['height'], 1. / sd_rec['width']]
if not self.normalize_bbox:
bbox_resize[0] *= float(self.image_min_side)
bbox_resize[1] *= float(self.image_max_side)
# Create labels for all boxes that are visible
for box in boxes:
# Add labels to boxes
if box.name in self.classes:
box.label = self.classes[box.name]
# Check if box is visible and transform box to 1D vector
if box_in_image(box=box,
intrinsic=camera_intrinsic,
imsize=imsize_src,
vis_level=BoxVisibility.ANY):
## Points in box method for annotation filterS
# check if bounding box has an according radar point
if self.only_radar_annotated == 2:
pcs, times = RadarPointCloud.from_file_multisweep(self.nusc, sample, self.radar_sensors[0], \
selected_sensor_channel, nsweeps=self.n_sweeps, min_distance=0.0, merge=False)
for pc in pcs:
pc.points = radar.enrich_radar_data(pc.points)
if len(pcs) > 0:
radar_sample = np.concatenate(
[pc.points for pc in pcs], axis=-1)
else:
print(
"[WARNING] only_radar_annotated=2 and sweeps=0 removes all annotations"
)
radar_sample = np.zeros(
shape=(len(radar.channel_map), 0))
radar_sample = radar_sample.astype(
dtype=np.float32)
mask = points_in_box(box, radar_sample[0:3, :])
if True not in mask:
continue
# If visible, we create the corresponding label
box2d = box.box2d(camera_intrinsic
) # returns [xmin, ymin, xmax, ymax]
box2d[0] *= bbox_resize[1]
box2d[1] *= bbox_resize[0]
box2d[2] *= bbox_resize[1]
box2d[3] *= bbox_resize[0]
annotations['bboxes'].insert(annotations_count, box2d)
annotations['labels'].insert(annotations_count,
box.label)
annotations['num_radar_pts'].insert(
annotations_count,
self.nusc.get('sample_annotation',
box.token)['num_radar_pts'])
distance = (box.center[0]**2 + box.center[1]**2 +
box.center[2]**2)**0.5
annotations['distances'].insert(
annotations_count, distance)
annotations['visibilities'].insert(
annotations_count,
int(
self.nusc.get('sample_annotation',
box.token)['visibility_token']))
annotations_count += 1
else:
# The current name has been ignored
pass
annotations['labels'] = np.array(annotations['labels'])
annotations['bboxes'] = np.array(annotations['bboxes'])
annotations['distances'] = np.array(annotations['distances'])
annotations['num_radar_pts'] = np.array(annotations['num_radar_pts'])
annotations['visibilities'] = np.array(annotations['visibilities'])
# num_radar_pts mathod for annotation filter
if self.only_radar_annotated == 1:
anns_to_keep = np.where(annotations['num_radar_pts'])[0]
for key in annotations:
annotations[key] = annotations[key][anns_to_keep]
return annotations
def load_image(self, image_index):
"""
Returns the image plus from given image and radar samples.
It takes the requested channels into account.
:param sample_token: [str] the token pointing to a certain sample
:returns: imageplus
"""
# Initialize local variables
radar_name = self.radar_sensors[0]
camera_name = self.camera_sensors[0]
# Gettign data from nuscenes database
sample_token = self.sample_tokens[image_index]
sample = self.nusc.get('sample', sample_token)
# Grab the front camera and the radar sensor.
radar_token = sample['data'][radar_name]
camera_token = sample['data'][camera_name]
image_target_shape = (self.image_min_side, self.image_max_side)
# Load the image
image_sample = self.load_sample_data(sample, camera_name)
# Add noise to the image if enabled
if self.noisy_image_method is not None and self.noise_factor > 0:
image_sample = noisy(self.noisy_image_method, image_sample,
self.noise_factor)
if self._is_image_plus_enabled() or self.camera_dropout > 0.0:
# Parameters
kwargs = {
'pointsensor_token': radar_token,
'camera_token': camera_token,
'height': (0, self.radar_projection_height),
'image_target_shape': image_target_shape,
'clear_radar': np.random.rand() < self.radar_dropout,
'clear_image': np.random.rand() < self.camera_dropout,
}
# Create image plus
# radar_sample = self.load_sample_data(sample, radar_name) # Load samples from disk
# Get filepath
if self.noise_filter:
required_sweep_count = self.n_sweeps + self.noise_filter.num_sweeps_required - 1
else:
required_sweep_count = self.n_sweeps
# sd_rec = self.nusc.get('sample_data', sample['data'][sensor_channel])
sensor_channel = radar_name
pcs, times = RadarPointCloud.from_file_multisweep(self.nusc, sample, sensor_channel, \
sensor_channel, nsweeps=required_sweep_count, min_distance=0.0, merge=False)
if self.noise_filter:
# fill up with zero sweeps
for _ in range(required_sweep_count - len(pcs)):
pcs.insert(
0,
RadarPointCloud(
np.zeros(shape=(RadarPointCloud.nbr_dims(), 0))))
radar_sample = [radar.enrich_radar_data(pc.points) for pc in pcs]
if self.noise_filter:
##### Filter the pcs #####
radar_sample = list(
self.noise_filter.denoise(radar_sample, self.n_sweeps))
if len(radar_sample) == 0:
radar_sample = np.zeros(shape=(len(radar.channel_map), 0))
else:
##### merge pcs into single radar samples array #####
radar_sample = np.concatenate(radar_sample, axis=-1)
radar_sample = radar_sample.astype(dtype=np.float32)
if self.perfect_noise_filter:
cartesian_uncertainty = 0.5 # meters
angular_uncertainty = math.radians(1.7) # degree
category_selection = self.noise_category_selection
nusc_sample_data = self.nusc.get('sample_data', radar_token)
radar_gt_mask = calc_mask(nusc=self.nusc, nusc_sample_data=nusc_sample_data, points3d=radar_sample[0:3,:], \
tolerance=cartesian_uncertainty, angle_tolerance=angular_uncertainty, \
category_selection=category_selection)
# radar_sample = radar_sample[:, radar_gt_mask.astype(np.bool)]
radar_sample = np.compress(radar_gt_mask,
radar_sample,
axis=-1)
if self.normalize_radar:
# we need to noramlize
# : use preprocess method analog to image preprocessing
sigma_factor = int(self.normalize_radar)
for ch in range(
3, radar_sample.shape[0]
): # neural fusion requires x y and z to be not normalized
norm_interval = (
-127.5, 127.5
) # caffee mode is default and has these norm interval for img
radar_sample[ch, :] = radar.normalize(
ch,
radar_sample[ch, :],
normalization_interval=norm_interval,
sigma_factor=sigma_factor)
img_p_full = self.image_plus_creation(self.nusc,
image_data=image_sample,
radar_data=radar_sample,
**kwargs)
# reduce to requested channels
#self.channels = [ch - 1 for ch in self.channels] # Shift channels by 1, cause we have a weird convetion starting at 1
input_data = img_p_full[:, :, self.channels]
else: # We are not in image_plus mode
# Only resize, because in the other case this is contained in image_plus_creation
input_data = cv2.resize(image_sample, image_target_shape[::-1])
return input_data
def load_image(self, image_index):
"""
Returns the image plus from given image and radar samples.
It takes the requested channels into account.
:param sample_token: [str] the token pointing to a certain sample
:returns: imageplus
"""
# Initialize local variables
radar_name = self.radar_sensors[0]
camera_name = self.camera_sensors[0]
# Gettign data from nuscenes database
sample_token = self.sample_tokens[image_index]
sample = self.nusc.get('sample', sample_token)
# Grab the front camera and the radar sensor.
radar_token = sample['data'][radar_name]
camera_token = sample['data'][camera_name]
image_target_shape = (self.image_min_side, self.image_max_side)
# Load the image
image_sample = self.load_sample_data(sample,
camera_name,
dtype=np.uint8)
# Add noise to the image if enabled
if self.noisy_image_method is not None and self.noise_factor > 0:
image_sample = noisy(self.noisy_image_method, image_sample,
self.noise_factor)
if self.image_radar_fusion or self.camera_dropout > 0.0:
# Parameters
kwargs = {
'pointsensor_token': radar_token,
'camera_token': camera_token,
'image_target_shape': image_target_shape,
'clear_radar': np.random.rand() < self.radar_dropout,
'clear_image': np.random.rand() < self.camera_dropout,
}
# Create image plus
# radar_sample = self.load_sample_data(sample, radar_name) # Load samples from disk
# Get filepath
if self.noise_filter: #TODO:现在的noise_filter还没有实现，等待以后实现，所以self.noise_filter是置0的
required_sweep_count = self.n_sweeps + self.noise_filter.num_sweeps_required - 1
else:
required_sweep_count = self.n_sweeps
# sd_rec = self.nusc.get('sample_data', sample['data'][sensor_channel])
sensor_channel = radar_name
pcs, times = RadarPointCloud.from_file_multisweep(self.nusc, sample, sensor_channel, \
sensor_channel, nsweeps=required_sweep_count,
min_distance=0.0)
# if self.noise_filter:
# # fill up with zero sweeps
# for _ in range(required_sweep_count - len(pcs)):
# pcs.insert(0, RadarPointCloud(np.zeros(shape=(RadarPointCloud.nbr_dims(), 0))))
radar_sample = pcs.points #[radar.enrich_radar_data(pc.points) for pc in pcs]
# if self.noise_filter:
# ##### Filter the pcs #####
# radar_sample = list(self.noise_filter.denoise(radar_sample, self.n_sweeps))
#如果self.noise_filter这个函数做好了，则下面注释的可以释放
# if len(radar_sample) == 0:
# radar_sample = np.zeros(shape=(len(radar.channel_map), 0))
# else:
# ##### merge pcs into single radar samples array #####
# radar_sample = np.concatenate(radar_sample, axis=-1)
radar_sample = radar_sample.astype(dtype=np.float32)
radar_sample = radar.enrich_radar_data(
radar_sample) #给多次扫描的radar增加测量数据
if self.perfect_noise_filter:
cartesian_uncertainty = 0.5 # meters
angular_uncertainty = math.radians(1.7) # degree
category_selection = self.noise_category_selection
nusc_sample_data = self.nusc.get('sample_data', radar_token)
radar_gt_mask = calc_mask(nusc=self.nusc, nusc_sample_data=nusc_sample_data,
points3d=radar_sample[0:3, :], \
tolerance=cartesian_uncertainty, angle_tolerance=angular_uncertainty, \
category_selection=category_selection)
# radar_sample = radar_sample[:, radar_gt_mask.astype(np.bool)]
radar_sample = np.compress(radar_gt_mask,
radar_sample,
axis=-1)
if self.normalize_radar:
# we need to noramlize
# : use preprocess method analog to image preprocessing
sigma_factor = int(self.normalize_radar)
for ch in range(
3, radar_sample.shape[0]
): # neural fusion requires x y and z to be not normalized
norm_interval = (
0, 255
) # caffee mode is default and has these norm interval for img
radar_sample[ch, :] = radar.normalize(
ch,
radar_sample[ch, :],
normalization_interval=norm_interval,
sigma_factor=sigma_factor)
#print(radar_sample.shape)
img_p_full, large_scale_p_full, middle_scale_p_full, little_scale_p_full = self.image_plus_creation(
self.nusc,
image_data=image_sample,
radar_data=radar_sample,
radar_cover_shape=self.radar_cover_shape,
**kwargs)
img_p_full = img_p_full.astype(np.uint8)
large_scale_p_full = large_scale_p_full.astype(np.uint8)
middle_scale_p_full = middle_scale_p_full.astype(np.uint8)
little_scale_p_full = little_scale_p_full.astype(np.uint8)
#如果image_plus_visual等于true时，则进行渲染
if self.image_plus_visual and self.isdebug: #在不debug时，程序不会进行渲染
#######################img_p_full####################################
input_data = self.create_input_data_plus_visual(img_p_full)
#######################large_scale_p_full############################
large_scale_input_data = self.create_input_data_plus_visual(
large_scale_p_full)
#######################middle_scale_p_full###########################
middle_scale_input_data = self.create_input_data_plus_visual(
middle_scale_p_full)
#######################little_scale_p_full###########################
little_scale_input_data = self.create_input_data_plus_visual(
little_scale_p_full)
else:
input_data = img_p_full[:, :, self.channels]
large_scale_input_data = large_scale_p_full[:, :,
self.channels]
middle_scale_input_data = middle_scale_p_full[:, :,
self.channels]
little_scale_input_data = little_scale_p_full[:, :,
self.channels]
else: # We are not in image_plus mode
# Only resize, because in the other case this is contained in image_plus_creation
input_data = cv2.resize(image_sample, image_target_shape[::-1])
return input_data, large_scale_input_data, middle_scale_input_data, little_scale_input_data
def dataset_mapper(self, frame):
"""
Add sensor data in vehicle or global coordinates to the frame
:param frame (dict): A frame dictionary from the db (no sensor data)
:return frame (dict): frame with all sensor data
"""
sample_record = self.get('sample', frame['sample_token'])
ref_sd_record = self.get(
'sample_data', sample_record['data'][self.cfg.REF_POSE_CHANNEL])
ref_pose_record = self.get('ego_pose', ref_sd_record['ego_pose_token'])
ref_cs_record = self.get('calibrated_sensor',
ref_sd_record['calibrated_sensor_token'])
frame['ref_pose_record'] = ref_pose_record
## Load camera data
cams = frame.get('camera', [])
for cam in cams:
sd_record = self.get('sample_data', cam['token'])
filename = osp.join(self.dataroot, sd_record['filename'])
image = self.get_camera_data(filename)
cam['image'] = image
cam['height'] = sd_record['height']
cam['width'] = sd_record['width']
cam['filename'] = filename
cam['cs_record'] = self.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
cam['pose_record'] = self.get('ego_pose',
sd_record['ego_pose_token'])
## Load annotations in vehicle coordinates
frame['anns'] = self._get_anns(frame['anns'], ref_pose_record)
## Filter anns outside image if in 'camera' mode:
if self.cfg.SAMPLE_MODE == 'camera':
filtered_anns = []
for ann in frame['anns']:
box_cam = nsutils.map_annotation_to_camera(
ann['box_3d'], cam['cs_record'], cam['pose_record'],
ref_pose_record, self.cfg.COORDINATES)
cam_intrinsic = np.array(cam['cs_record']['camera_intrinsic'])
if not box_in_image(box_cam, cam_intrinsic, (1600, 900)):
continue
if self.cfg.GEN_2D_BBOX:
ann['box_2d'] = nsutils.box_3d_to_2d_simple(
box_cam, cam_intrinsic, (1600, 900))
filtered_anns.append(ann)
frame['anns'] = filtered_anns
## Load LIDAR data in vehicle coordinates
if 'lidar' in frame:
lidar = frame['lidar']
sd_record = self.get('sample_data', lidar['token'])
lidar['cs_record'] = self.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
lidar['pose_record'] = self.get('ego_pose',
sd_record['ego_pose_token'])
lidar_pc, _ = LidarPointCloud.from_file_multisweep(
self,
sample_record,
lidar['channel'],
lidar['channel'],
nsweeps=self.cfg.LIDAR_SWEEPS,
min_distance=self.cfg.PC_MIN_DIST)
## Take from sensor to vehicle coordinates
lidar_pc = nsutils.sensor_to_vehicle(lidar_pc, lidar['cs_record'])
## filter points outside the image if in 'camera' mode
if self.cfg.SAMPLE_MODE == "camera":
cam = frame['camera'][0]
cam_intrinsic = np.array(cam['cs_record']['camera_intrinsic'])
lidar_pc_cam, _ = nsutils.map_pointcloud_to_camera(
lidar_pc,
cam_cs_record=cam['cs_record'],
cam_pose_record=cam['pose_record'],
pointsensor_pose_record=frame['lidar']['pose_record'],
coordinates=frame['coordinates'])
_, _, mask = nsutils.map_pointcloud_to_image(lidar_pc_cam,
cam_intrinsic,
img_shape=(1600,
900))
lidar_pc.points = lidar_pc.points[:, mask]
frame['lidar']['pointcloud'] = lidar_pc
## Load Radar data in vehicle coordinates
if 'radar' in frame:
all_radar_pcs = RadarPointCloud(np.zeros((18, 0)))
for radar in frame['radar']:
radar_pc, _ = RadarPointCloud.from_file_multisweep(
self,
sample_record,
radar['channel'],
self.cfg.REF_POSE_CHANNEL,
nsweeps=self.cfg.RADAR_SWEEPS,
min_distance=self.cfg.PC_MIN_DIST)
radar_pc = nsutils.sensor_to_vehicle(radar_pc, ref_cs_record)
## filter points outside the image if in 'camera' mode
if self.cfg.SAMPLE_MODE == "camera":
cam = frame['camera'][0]
cam_intrinsic = np.array(
cam['cs_record']['camera_intrinsic'])
radar_pc_cam, _ = nsutils.map_pointcloud_to_camera(
radar_pc,
cam_cs_record=cam['cs_record'],
cam_pose_record=cam['pose_record'],
pointsensor_pose_record=ref_pose_record,
coordinates=frame['coordinates'])
_, _, mask = nsutils.map_pointcloud_to_image(
radar_pc_cam, cam_intrinsic, img_shape=(1600, 900))
radar_pc.points = radar_pc.points[:, mask]
all_radar_pcs.points = np.hstack(
(all_radar_pcs.points, radar_pc.points))
frame['radar'] = {}
frame['radar']['pointcloud'] = all_radar_pcs
frame['radar']['pose_record'] = ref_pose_record
frame['radar']['cs_record'] = ref_cs_record
return frame
def nuscenes_gt_to_kitti(self) -> None:
"""
Converts nuScenes GT annotations to KITTI format.
"""
kitti_to_nu_lidar = Quaternion(axis=(0, 0, 1), angle=np.pi / 2)
kitti_to_nu_lidar_inv = kitti_to_nu_lidar.inverse
imsize = (1600, 900)
token_idx = 0 # Start tokens from 0.
# Create output folders.
label_folder = os.path.join(self.output_dir, self.split, 'label_2')
calib_folder = os.path.join(self.output_dir, self.split, 'calib')
image_folder = os.path.join(self.output_dir, self.split, 'image_2')
lidar_folder = os.path.join(self.output_dir, self.split, 'velodyne')
radar_folder = os.path.join(self.output_dir, self.split, 'radar')
for folder in [
label_folder, calib_folder, image_folder, lidar_folder,
radar_folder
]:
if not os.path.isdir(folder):
os.makedirs(folder)
id_to_token_file = os.path.join(self.output_dir, self.split,
'id2token.txt')
id2token = open(id_to_token_file, "w+")
# Use only the samples from the current split.
split_logs = create_splits_logs(self.split, self.nusc)
sample_tokens = self._split_to_samples(split_logs)
sample_tokens = sample_tokens[:self.image_count]
out_filenames = []
for sample_token in tqdm(sample_tokens):
# Get sample data.
sample = self.nusc.get('sample', sample_token)
sample_annotation_tokens = sample['anns']
cam_token = sample['data'][self.cam_name]
lidar_token = sample['data'][self.lidar_name]
radar_tokens = []
for radar_name in _C.RADARS.keys():
radar_tokens.append(sample['data'][radar_name])
# Retrieve sensor records.
sd_record_cam = self.nusc.get('sample_data', cam_token)
sd_record_lid = self.nusc.get('sample_data', lidar_token)
cs_record_cam = self.nusc.get(
'calibrated_sensor', sd_record_cam['calibrated_sensor_token'])
cs_record_lid = self.nusc.get(
'calibrated_sensor', sd_record_lid['calibrated_sensor_token'])
sd_record_rad = []
cs_record_rad = []
for i, radar_token in enumerate(radar_tokens):
sd_record_rad.append(self.nusc.get('sample_data', radar_token))
cs_record_rad.append(
self.nusc.get('calibrated_sensor',
sd_record_rad[i]['calibrated_sensor_token']))
# Combine transformations and convert to KITTI format.
# Note: cam uses same conventions in KITTI and nuScenes.
lid_to_ego = transform_matrix(cs_record_lid['translation'],
Quaternion(
cs_record_lid['rotation']),
inverse=False)
ego_to_cam = transform_matrix(cs_record_cam['translation'],
Quaternion(
cs_record_cam['rotation']),
inverse=True)
rad_to_ego = []
for cs_rec_rad in cs_record_rad:
rad_to_ego.append(
transform_matrix(cs_rec_rad['translation'],
Quaternion(cs_rec_rad['rotation']),
inverse=False))
velo_to_cam = np.dot(ego_to_cam, lid_to_ego)
# # TODO: Assuming Radar point are going to be in ego coordinates
# radar_to_cam = ego_to_cam
# Convert from KITTI to nuScenes LIDAR coordinates, where we apply velo_to_cam.
velo_to_cam_kitti = np.dot(velo_to_cam,
kitti_to_nu_lidar.transformation_matrix)
# # Nuscenes radars use same convention as KITTI lidar
# radar_to_cam_kitti = radar_to_cam
# Currently not used.
imu_to_velo_kitti = np.zeros((3, 4)) # Dummy values.
r0_rect = Quaternion(axis=[1, 0, 0], angle=0) # Dummy values.
# Projection matrix.
p_left_kitti = np.zeros((3, 4))
# Cameras are always rectified.
p_left_kitti[:3, :3] = cs_record_cam['camera_intrinsic']
# Create KITTI style transforms.
velo_to_cam_rot = velo_to_cam_kitti[:3, :3]
velo_to_cam_trans = velo_to_cam_kitti[:3, 3]
# radar_to_cam_rot = radar_to_cam_kitti[:3, :3]
# radar_to_cam_trans = radar_to_cam_kitti[:3, 3]
# Check that the lidar rotation has the same format as in KITTI.
assert (velo_to_cam_rot.round(0) == np.array([[0, -1,
0], [0, 0, -1],
[1, 0, 0]])).all()
assert (velo_to_cam_trans[1:3] < 0).all()
# Retrieve the token from the lidar.
# Note that this may be confusing as the filename of the camera will
# include the timestamp of the lidar, not the camera.
filename_cam_full = sd_record_cam['filename']
filename_lid_full = sd_record_lid['filename']
filename_rad_full = []
for sd_rec_rad in sd_record_rad:
filename_rad_full.append(sd_rec_rad['filename'])
out_filename = '%06d' % token_idx # Alternative to use KITTI names.
# out_filename = sample_token
# Write token to disk.
text = sample_token
id2token.write(text + '\n')
id2token.flush()
token_idx += 1
# Convert image (jpg to png).
src_im_path = os.path.join(self.nusc.dataroot, filename_cam_full)
dst_im_path = os.path.join(image_folder, out_filename + '.png')
if self.use_symlinks:
# Create symbolic links to nuscenes images
os.symlink(os.path.abspath(src_im_path), dst_im_path)
else:
im = Image.open(src_im_path)
im.save(dst_im_path, "PNG")
# Convert lidar.
src_lid_path = os.path.join(self.nusc.dataroot, filename_lid_full)
dst_lid_path = os.path.join(lidar_folder, out_filename + '.bin')
assert not dst_lid_path.endswith('.pcd.bin')
# pcl = LidarPointCloud.from_file(src_lid_path)
pcl, _ = LidarPointCloud.from_file_multisweep(
nusc=self.nusc,
sample_rec=sample,
chan=self.lidar_name,
ref_chan=self.lidar_name,
nsweeps=self.lidar_sweeps,
min_distance=1)
pcl.rotate(
kitti_to_nu_lidar_inv.rotation_matrix) # In KITTI lidar frame.
pcl.points = pcl.points.astype('float32')
with open(dst_lid_path, "w") as lid_file:
pcl.points.T.tofile(lid_file)
# # Visualize pointclouds
# _, ax = plt.subplots(1, 1, figsize=(9, 9))
# points = view_points(pcl.points[:3, :], np.eye(4), normalize=False)
# dists = np.sqrt(np.sum(pcl.points[:2, :] ** 2, axis=0))
# colors = np.minimum(1, dists / 50)
# ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# # plt.show(block=False)
# plt.show()
# Convert radar.
src_rad_path = []
for filename_radar in filename_rad_full:
src_rad_path.append(
os.path.join(self.nusc.dataroot, filename_radar))
dst_rad_path = os.path.join(radar_folder, out_filename + '.bin')
assert not dst_rad_path.endswith('.pcd.bin')
pcl = RadarPointCloud(np.zeros((18, 0)))
## Get Radar points in Lidar coordinate system
for i, rad_path in enumerate(src_rad_path):
pc, _ = RadarPointCloud.from_file_multisweep(
self.nusc,
sample_rec=sample,
chan=sd_record_rad[i]['channel'],
ref_chan=self.lidar_name,
nsweeps=self.radar_sweeps,
min_distance=0)
# rot_matrix = Quaternion(cs_record_rad[i]['rotation']).rotation_matrix
# pc.rotate(rot_matrix)
# pc.translate(np.array(cs_record_rad[i]['translation']))
pcl.points = np.hstack((pcl.points, pc.points))
pcl.rotate(
kitti_to_nu_lidar_inv.rotation_matrix) # In KITTI lidar frame.
## Visualize pointclouds
# _, ax = plt.subplots(1, 1, figsize=(9, 9))
# points = view_points(pcl.points[:3, :], np.eye(4), normalize=False)
# dists = np.sqrt(np.sum(pcl.points[:2, :] ** 2, axis=0))
# colors = np.minimum(1, dists / 50)
# ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# plt.show()
pcl.points = pcl.points.astype('float32')
with open(dst_rad_path, "w") as rad_file:
pcl.points.T.tofile(rad_file)
# Add to tokens.
out_filenames.append(out_filename)
# Create calibration file.
kitti_transforms = dict()
kitti_transforms['P0'] = np.zeros((3, 4)) # Dummy values.
kitti_transforms['P1'] = np.zeros((3, 4)) # Dummy values.
kitti_transforms['P2'] = p_left_kitti # Left camera transform.
kitti_transforms['P3'] = np.zeros((3, 4)) # Dummy values.
kitti_transforms[
'R0_rect'] = r0_rect.rotation_matrix # Cameras are already rectified.
kitti_transforms['Tr_velo_to_cam'] = np.hstack(
(velo_to_cam_rot, velo_to_cam_trans.reshape(3, 1)))
# kitti_transforms['Tr_radar_to_cam'] = np.hstack((radar_to_cam_rot, radar_to_cam_trans.reshape(3, 1)))
kitti_transforms['Tr_imu_to_velo'] = imu_to_velo_kitti
calib_path = os.path.join(calib_folder, out_filename + '.txt')
with open(calib_path, "w") as calib_file:
for (key, val) in kitti_transforms.items():
val = val.flatten()
val_str = '%.12e' % val[0]
for v in val[1:]:
val_str += ' %.12e' % v
calib_file.write('%s: %s\n' % (key, val_str))
# Write label file.
label_path = os.path.join(label_folder, out_filename + '.txt')
if os.path.exists(label_path):
print('Skipping existing file: %s' % label_path)
continue
with open(label_path, "w") as label_file:
for sample_annotation_token in sample_annotation_tokens:
sample_annotation = self.nusc.get('sample_annotation',
sample_annotation_token)
# Get box in LIDAR frame.
_, box_lidar_nusc, _ = self.nusc.get_sample_data(
lidar_token,
box_vis_level=BoxVisibility.NONE,
selected_anntokens=[sample_annotation_token])
box_lidar_nusc = box_lidar_nusc[0]
# Truncated: Set all objects to 0 which means untruncated.
truncated = 0.0
# Occluded: Set all objects to full visibility as this information is not available in nuScenes.
occluded = 0
# Convert nuScenes category to nuScenes detection challenge category.
detection_name = _C.KITTI_CLASSES.get(
sample_annotation['category_name'])
# Skip categories that are not in the KITTI classes.
if detection_name is None:
continue
# Convert from nuScenes to KITTI box format.
box_cam_kitti = KittiDB.box_nuscenes_to_kitti(
box_lidar_nusc, Quaternion(matrix=velo_to_cam_rot),
velo_to_cam_trans, r0_rect)
# Project 3d box to 2d box in image, ignore box if it does not fall inside.
bbox_2d = KittiDB.project_kitti_box_to_image(box_cam_kitti,
p_left_kitti,
imsize=imsize)
if bbox_2d is None:
# continue
## If box is not inside the image, 2D boxes are set to zero
bbox_2d = (0, 0, 0, 0)
# Set dummy score so we can use this file as result.
box_cam_kitti.score = 0
# Convert box to output string format.
output = KittiDB.box_to_string(name=detection_name,
box=box_cam_kitti,
bbox_2d=bbox_2d,
truncation=truncated,
occlusion=occluded)
# Write to disk.
label_file.write(output + '\n')
id2token.close()
