NuScenes的radar数据包含18个属性,每个属性在编解码过程中使用不同的字符大小作为分割,故不能单纯的使用二进制读取方式。只能使用NuScenes自带的RadarPointCloud类进行解析得到人们熟悉的十进制。
import numpy as np
from abc import ABC,abstractmethod
from typing import Tuple,List,Dict
from matplotlib.axes import Axes
import struct
bin_url = './velodyne/n008-2018-05-21-11-06-59-0400__RADAR_BACK_LEFT__1526915248384382.pcd'
class PointCloud(ABC):
"""
Abstract class for manipulating and viewing point clouds.
Every point cloud (lidar and radar) consists of points where:
- Dimensions 0, 1, 2 represent x, y, z coordinates.
These are modified when the point cloud is rotated or translated.
- All other dimensions are optional. Hence these have to be manually modified if the reference frame changes.
"""
def __init__(self, points: np.ndarray):
"""
Initialize a point cloud and check it has the correct dimensions.
:param points: <np.float: d, n>. d-dimensional input point cloud matrix.
"""
assert points.shape[0] == self.nbr_dims(), 'Error: Pointcloud points must have format: %d x n' % self.nbr_dims()
self.points = points
@staticmethod
@abstractmethod
def nbr_dims() -> int:
"""
Returns the number of dimensions.
:return: Number of dimensions.
"""
pass
@classmethod
@abstractmethod
def from_file(cls, file_name: str) -> 'PointCloud':
"""
Loads point cloud from disk.
:param file_name: Path of the pointcloud file on disk.
:return: PointCloud instance.
"""
pass
@classmethod
def from_file_multisweep(cls,
nusc: 'NuScenes',
sample_rec: Dict,
chan: str,
ref_chan: str,
nsweeps: int = 5,
min_distance: float = 1.0) -> Tuple['PointCloud', np.ndarray]:
"""
Return a point cloud that aggregates multiple sweeps.
As every sweep is in a different coordinate frame, we need to map the coordinates to a single reference frame.
As every sweep has a different timestamp, we need to account for that in the transformations and timestamps.
:param nusc: A NuScenes instance.
:param sample_rec: The current sample.
:param chan: The lidar/radar channel from which we track back n sweeps to aggregate the point cloud.
:param ref_chan: The reference channel of the current sample_rec that the point clouds are mapped to.
:param nsweeps: Number of sweeps to aggregated.
:param min_distance: Distance below which points are discarded.
:return: (all_pc, all_times). The aggregated point cloud and timestamps.
"""
# Init.
points = np.zeros((cls.nbr_dims(), 0))
all_pc = cls(points)
all_times = np.zeros((1, 0))
# Get reference pose and timestamp.
ref_sd_token = sample_rec['data'][ref_chan]
ref_sd_rec = nusc.get('sample_data', ref_sd_token)
ref_pose_rec = nusc.get('ego_pose', ref_sd_rec['ego_pose_token'])
ref_cs_rec = nusc.get('calibrated_sensor', ref_sd_rec['calibrated_sensor_token'])
ref_time = 1e-6 * ref_sd_rec['timestamp']
# Homogeneous transform from ego car frame to reference frame.
ref_from_car = transform_matrix(ref_cs_rec['translation'], Quaternion(ref_cs_rec['rotation']), inverse=True)
# Homogeneous transformation matrix from global to _current_ ego car frame.
car_from_global = transform_matrix(ref_pose_rec['translation'], Quaternion(ref_pose_rec['rotation']),
inverse=True)
# Aggregate current and previous sweeps.
sample_data_token = sample_rec['data'][chan]
current_sd_rec = nusc.get('sample_data', sample_data_token)
for _ in range(nsweeps):
# Load up the pointcloud and remove points close to the sensor.
current_pc = cls.from_file(osp.join(nusc.dataroot, current_sd_rec['filename']))
current_pc.remove_close(min_distance)
# Get past pose.
current_pose_rec = nusc.get('ego_pose', current_sd_rec['ego_pose_token'])
global_from_car = transform_matrix(current_pose_rec['translation'],
Quaternion(current_pose_rec['rotation']), inverse=False)
# Homogeneous transformation matrix from sensor coordinate frame to ego car frame.
current_cs_rec = nusc.get('calibrated_sensor', current_sd_rec['calibrated_sensor_token'])
car_from_current = transform_matrix(current_cs_rec['translation'], Quaternion(current_cs_rec['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])
current_pc.transform(trans_matrix)
# Add time vector which can be used as a temporal feature.
time_lag = ref_time - 1e-6 * current_sd_rec['timestamp'] # Positive difference.
times = time_lag * np.ones((1, current_pc.nbr_points()))
all_times = np.hstack((all_times, times))
# Merge with key pc.
all_pc.points = np.hstack((all_pc.points, current_pc.points))
# Abort if there are no previous sweeps.
if current_sd_rec['prev'] == '':
break
else:
current_sd_rec = nusc.get('sample_data', current_sd_rec['prev'])
return all_pc, all_times
def nbr_points(self) -> int:
"""
Returns the number of points.
:return: Number of points.
"""
return self.points.shape[1]
def subsample(self, ratio: float) -> None:
"""
Sub-samples the pointcloud.
:param ratio: Fraction to keep.
"""
selected_ind = np.random.choice(np.arange(0, self.nbr_points()), size=int(self.nbr_points() * ratio))
self.points = self.points[:, selected_ind]
def remove_close(self, radius: float) -> None:
"""
Removes point too close within a certain radius from origin.
:param radius: Radius below which points are removed.
"""
x_filt = np.abs(self.points[0, :]) < radius
y_filt = np.abs(self.points[1, :]) < radius
not_close = np.logical_not(np.logical_and(x_filt, y_filt))
self.points = self.points[:, not_close]
def translate(self, x: np.ndarray) -> None:
"""
Applies a translation to the point cloud.
:param x: <np.float: 3, 1>. Translation in x, y, z.
"""
for i in range(3):
self.points[i, :] = self.points[i, :] + x[i]
def rotate(self, rot_matrix: np.ndarray) -> None:
"""
Applies a rotation.
:param rot_matrix: <np.float: 3, 3>. Rotation matrix.
"""
self.points[:3, :] = np.dot(rot_matrix, self.points[:3, :])
def transform(self, transf_matrix: np.ndarray) -> None:
"""
Applies a homogeneous transform.
:param transf_matrix: <np.float: 4, 4>. Homogenous transformation matrix.
"""
self.points[:3, :] = transf_matrix.dot(np.vstack((self.points[:3, :], np.ones(self.nbr_points()))))[:3, :]
def render_height(self,
ax: Axes,
view: np.ndarray = np.eye(4),
x_lim: Tuple[float, float] = (-20, 20),
y_lim: Tuple[float, float] = (-20, 20),
marker_size: float = 1) -> None:
"""
Very simple method that applies a transformation and then scatter plots the points colored by height (z-value).
:param ax: Axes on which to render the points.
:param view: <np.float: n, n>. Defines an arbitrary projection (n <= 4).
:param x_lim: (min, max). x range for plotting.
:param y_lim: (min, max). y range for plotting.
:param marker_size: Marker size.
"""
self._render_helper(2, ax, view, x_lim, y_lim, marker_size)
def render_intensity(self,
ax: Axes,
view: np.ndarray = np.eye(4),
x_lim: Tuple[float, float] = (-20, 20),
y_lim: Tuple[float, float] = (-20, 20),
marker_size: float = 1) -> None:
"""
Very simple method that applies a transformation and then scatter plots the points colored by intensity.
:param ax: Axes on which to render the points.
:param view: <np.float: n, n>. Defines an arbitrary projection (n <= 4).
:param x_lim: (min, max).
:param y_lim: (min, max).
:param marker_size: Marker size.
"""
self._render_helper(3, ax, view, x_lim, y_lim, marker_size)
def _render_helper(self,
color_channel: int,
ax: Axes,
view: np.ndarray,
x_lim: Tuple[float, float],
y_lim: Tuple[float, float],
marker_size: float) -> None:
"""
Helper function for rendering.
:param color_channel: Point channel to use as color.
:param ax: Axes on which to render the points.
:param view: <np.float: n, n>. Defines an arbitrary projection (n <= 4).
:param x_lim: (min, max).
:param y_lim: (min, max).
:param marker_size: Marker size.
"""
points = view_points(self.points[:3, :], view, normalize=False)
ax.scatter(points[0, :], points[1, :], c=self.points[color_channel, :], s=marker_size)
ax.set_xlim(x_lim[0], x_lim[1])
ax.set_ylim(y_lim[0], y_lim[1])
class RadarPointCloud(PointCloud):
# Class-level settings for radar pointclouds, see from_file().
invalid_states = [0] # type: List[int]
dynprop_states = range(7) # type: List[int] # Use [0, 2, 6] for moving objects only.
ambig_states = [3] # type: List[int]
@classmethod
def disable_filters(cls) -> None:
"""
Disable all radar filter settings.
Use this method to plot all radar returns.
Note that this method affects the global settings.
"""
cls.invalid_states = list(range(18))
cls.dynprop_states = list(range(8))
cls.ambig_states = list(range(5))
@classmethod
def default_filters(cls) -> None:
"""
Set the defaults for all radar filter settings.
Note that this method affects the global settings.
"""
cls.invalid_states = [0]
cls.dynprop_states = range(7)
cls.ambig_states = [3]
@staticmethod
def nbr_dims() -> int:
"""
Returns the number of dimensions.
:return: Number of dimensions.
"""
return 18
@classmethod
def from_file(cls,
file_name: str,
invalid_states: List[int] = None,
dynprop_states: List[int] = None,
ambig_states: List[int] = None) -> 'RadarPointCloud':
"""
Loads RADAR data from a Point Cloud Data file. See details below.
:param file_name: The path of the pointcloud file.
:param invalid_states: Radar states to be kept. See details below.
:param dynprop_states: Radar states to be kept. Use [0, 2, 6] for moving objects only. See details below.
:param ambig_states: Radar states to be kept. See details below.
To keep all radar returns, set each state filter to range(18).
:return: <np.float: d, n>. Point cloud matrix with d dimensions and n points.
Example of the header fields:
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z dyn_prop id rcs vx vy vx_comp vy_comp is_quality_valid ambig_state x_rms y_rms invalid_state pdh0 vx_rms vy_rms
SIZE 4 4 4 1 2 4 4 4 4 4 1 1 1 1 1 1 1 1
TYPE F F F I I F F F F F I I I I I I I I
COUNT 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
WIDTH 125
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 125
DATA binary
Below some of the fields are explained in more detail:
x is front, y is left
vx, vy are the velocities in m/s.
vx_comp, vy_comp are the velocities in m/s compensated by the ego motion.
We recommend using the compensated velocities.
invalid_state: state of Cluster validity state.
(Invalid states)
0x01 invalid due to low RCS
0x02 invalid due to near-field artefact
0x03 invalid far range cluster because not confirmed in near range
0x05 reserved
0x06 invalid cluster due to high mirror probability
0x07 Invalid cluster because outside sensor field of view
0x0d reserved
0x0e invalid cluster because it is a harmonics
(Valid states)
0x00 valid
0x04 valid cluster with low RCS
0x08 valid cluster with azimuth correction due to elevation
0x09 valid cluster with high child probability
0x0a valid cluster with high probability of being a 50 deg artefact
0x0b valid cluster but no local maximum
0x0c valid cluster with high artefact probability
0x0f valid cluster with above 95m in near range
0x10 valid cluster with high multi-target probability
0x11 valid cluster with suspicious angle
dynProp: Dynamic property of cluster to indicate if is moving or not.
0: moving
1: stationary
2: oncoming
3: stationary candidate
4: unknown
5: crossing stationary
6: crossing moving
7: stopped
ambig_state: State of Doppler (radial velocity) ambiguity solution.
0: invalid
1: ambiguous
2: staggered ramp
3: unambiguous
4: stationary candidates
pdh0: False alarm probability of cluster (i.e. probability of being an artefact caused by multipath or similar).
0: invalid
1: <25%
2: 50%
3: 75%
4: 90%
5: 99%
6: 99.9%
7: <=100%
"""
assert file_name.endswith('.pcd'), 'Unsupported filetype {}'.format(file_name)
meta = []
with open(file_name, 'rb') as f:
for line in f:
line = line.strip().decode('utf-8')
meta.append(line)
if line.startswith('DATA'):
break
data_binary = f.read()
# Get the header rows and check if they appear as expected.
assert meta[0].startswith('#'), 'First line must be comment'
assert meta[1].startswith('VERSION'), 'Second line must be VERSION'
sizes = meta[3].split(' ')[1:]#['4', '4', '4', '1', '2', '4', '4', '4', '4', '4', '1', '1', '1', '1', '1', '1', '1', '1']
types = meta[4].split(' ')[1:]#['F', 'F', 'F', 'I', 'I', 'F', 'F', 'F', 'F', 'F', 'I', 'I', 'I', 'I', 'I', 'I', 'I', 'I']
counts = meta[5].split(' ')[1:]
width = int(meta[6].split(' ')[1])
height = int(meta[7].split(' ')[1])
data = meta[10].split(' ')[1]
feature_count = len(types)
# print(types)
assert width > 0
assert len([c for c in counts if c != c]) == 0, 'Error: COUNT not supported!'
assert height == 1, 'Error: height != 0 not supported!'
assert data == 'binary'
# Lookup table for how to decode the binaries.
unpacking_lut = {'F': {2: 'e', 4: 'f', 8: 'd'},
'I': {1: 'b', 2: 'h', 4: 'i', 8: 'q'},
'U': {1: 'B', 2: 'H', 4: 'I', 8: 'Q'}}
types_str = ''.join([unpacking_lut[t][int(s)] for t, s in zip(types, sizes)])#fffbhfffffbbbbbbbb
# Decode each point.
offset = 0
point_count = width
points = []
# print(point_count,feature_count)
# print(sizes)
for i in range(point_count):
point = []
for p in range(feature_count):
start_p = offset
end_p = start_p + int(sizes[p])
assert end_p < len(data_binary)
point_p = struct.unpack(types_str[p], data_binary[start_p:end_p])[0]
point.append(point_p)
offset = end_p
points.append(point)
# A NaN in the first point indicates an empty pointcloud.
point = np.array(points[0])
if np.any(np.isnan(point)):
return cls(np.zeros((feature_count, 0)))
# Convert to numpy matrix.
points = np.array(points).transpose()
# If no parameters are provided, use default settings.
invalid_states = cls.invalid_states if invalid_states is None else invalid_states
dynprop_states = cls.dynprop_states if dynprop_states is None else dynprop_states
ambig_states = cls.ambig_states if ambig_states is None else ambig_states
# Filter points with an invalid state.
valid = [p in invalid_states for p in points[-4, :]]
points = points[:, valid]
# Filter by dynProp.
valid = [p in dynprop_states for p in points[3, :]]
points = points[:, valid]
# Filter by ambig_state.
valid = [p in ambig_states for p in points[11, :]]
points = points[:, valid]
return cls(points)
pc=RadarPointCloud.from_file(bin_url)
# print(len(pc.points))
最终输出pc即可得到所有的毫米波点云。