def __call__(self, results):
"""Call function to load points data from file.
Args:
results (dict): Result dict containing point clouds data.
Returns:
dict: The result dict containing the point clouds data. \
Added key and value are described below.
- points (np.ndarray): Point clouds data.
"""
pts_filename = results["pts_filename"]
points = self._load_points(pts_filename)
points = points.reshape(-1, self.load_dim)
points = points[:, self.use_dim]
attribute_dims = None
if self.shift_height:
floor_height = np.percentile(points[:, 2], 0.99)
height = points[:, 2] - floor_height
points = np.concatenate([points, np.expand_dims(height, 1)], 1)
attribute_dims = dict(height=3)
points_class = get_points_type(self.coord_type)
points = points_class(points,
points_dim=points.shape[-1],
attribute_dims=attribute_dims)
results["points"] = points
return results
def __call__(self, results):
"""Call function to load points data from file.
Args:
results (dict): Result dict containing point clouds data.
Returns:
dict: The result dict containing the point clouds data. \
Added key and value are described below.
- points (:obj:`BasePoints`): Point clouds data.
"""
pts_filename = results['pts_filename']
points = self._load_points(pts_filename)
points = points.reshape(-1, self.load_dim)
points = points[:, self.use_dim]
attribute_dims = None
if self.shift_height:
floor_height = np.percentile(points[:, 2], 0.99)
height = points[:, 2] - floor_height
points = np.concatenate(
[points[:, :3],
np.expand_dims(height, 1), points[:, 3:]], 1)
attribute_dims = dict(height=3)
if self.use_color:
assert len(self.use_dim) >= 6
if attribute_dims is None:
attribute_dims = dict()
attribute_dims.update(
dict(color=[
points.shape[1] - 3,
points.shape[1] - 2,
points.shape[1] - 1,
]))
points_class = get_points_type(self.coord_type)
points = points_class(points,
points_dim=points.shape[-1],
attribute_dims=attribute_dims)
results['points'] = points
return results
def _load_points(self, pts_filename):
"""Private function to load point clouds data.
Args:
pts_filename (str): Filename of point clouds data.
Returns:
np.ndarray: An array containing point clouds data.
"""
if self._file_client is None:
self._file_client = mmcv.FileClient(**self._file_client_args)
try:
pts_bytes = self._file_client.get(pts_filename)
points = np.frombuffer(pts_bytes, dtype=np.float32)
except ConnectionError:
mmcv.check_file_exist(pts_filename)
if pts_filename.endswith(".npy"):
points = np.load(pts_filename)
else:
points = np.fromfile(pts_filename, dtype=np.float32)
points = points.reshape(-1, self._load_dim)
points = points[:, self._use_dim]
attribute_dims = None
# TODO attributes
if self.shift_height:
floor_height = np.percentile(points[:, 2], 0.99)
height = points[:, 2] - floor_height
points = np.concatenate([points, np.expand_dims(height, 1)], 1)
attribute_dims = dict(height=3)
points_class = get_points_type(self._coord_type)
points = points_class(points,
points_dim=points.shape[-1],
attribute_dims=attribute_dims)
return points
def apply_3d_transformation(pcd, coords_type, img_meta, reverse=False):
"""Apply transformation to input point cloud.
Args:
pcd (torch.Tensor): The point cloud to be transformed.
coords_type (str): 'DEPTH' or 'CAMERA' or 'LIDAR'
img_meta(dict): Meta info regarding data transformation.
reverse (bool): Reversed transformation or not.
Note:
The elements in img_meta['transformation_3d_flow']:
"T" stands for translation;
"S" stands for scale;
"R" stands for rotation;
"HF" stands for horizontal flip;
"VF" stands for vertical flip.
Returns:
torch.Tensor: The transformed point cloud.
"""
dtype = pcd.dtype
device = pcd.device
pcd_rotate_mat = (
torch.tensor(img_meta['pcd_rotation'], dtype=dtype, device=device)
if 'pcd_rotation' in img_meta else torch.eye(
3, dtype=dtype, device=device))
pcd_scale_factor = (
img_meta['pcd_scale_factor'] if 'pcd_scale_factor' in img_meta else 1.)
pcd_trans_factor = (
torch.tensor(img_meta['pcd_trans'], dtype=dtype, device=device)
if 'pcd_trans' in img_meta else torch.zeros(
(3), dtype=dtype, device=device))
pcd_horizontal_flip = img_meta[
'pcd_horizontal_flip'] if 'pcd_horizontal_flip' in \
img_meta else False
pcd_vertical_flip = img_meta[
'pcd_vertical_flip'] if 'pcd_vertical_flip' in \
img_meta else False
flow = img_meta['transformation_3d_flow'] \
if 'transformation_3d_flow' in img_meta else []
pcd = pcd.clone() # prevent inplace modification
pcd = get_points_type(coords_type)(pcd)
horizontal_flip_func = partial(pcd.flip, bev_direction='horizontal') \
if pcd_horizontal_flip else lambda: None
vertical_flip_func = partial(pcd.flip, bev_direction='vertical') \
if pcd_vertical_flip else lambda: None
if reverse:
scale_func = partial(pcd.scale, scale_factor=1.0 / pcd_scale_factor)
translate_func = partial(pcd.translate, trans_vector=-pcd_trans_factor)
# pcd_rotate_mat @ pcd_rotate_mat.inverse() is not
# exactly an identity matrix
# use angle to create the inverse rot matrix neither.
rotate_func = partial(pcd.rotate, rotation=pcd_rotate_mat.inverse())
# reverse the pipeline
flow = flow[::-1]
else:
scale_func = partial(pcd.scale, scale_factor=pcd_scale_factor)
translate_func = partial(pcd.translate, trans_vector=pcd_trans_factor)
rotate_func = partial(pcd.rotate, rotation=pcd_rotate_mat)
flow_mapping = {
'T': translate_func,
'S': scale_func,
'R': rotate_func,
'HF': horizontal_flip_func,
'VF': vertical_flip_func
}
for op in flow:
assert op in flow_mapping, f'This 3D data '\
f'transformation op ({op}) is not supported'
func = flow_mapping[op]
func()
return pcd.coord
def __call__(self, results):
points = results["points"].tensor
device = points.device
lidar2imgs = (torch.tensor(
results["img_T_lidar"], ).to(device).float())
# get x y z only
points = points[:, 0:3]
# bring the points to homogenous coords
points = torch.cat((points, torch.ones((len(points), 1))), dim=1)
# list cameras[h x w x channels]
imgs = results["img"]
# create the result array to store the colored points in
# n x color channels
colors_shape = (len(points), imgs[0].shape[-1])
points_colors = torch.zeros(colors_shape)
# marks points that have a valid color value
colored_points_mask = torch.zeros((len(points), ), dtype=torch.bool)
for img_idx in range(len(lidar2imgs)):
img_mat = lidar2imgs[img_idx]
img = imgs[img_idx]
# cameras x h x w x color channels
img = torch.from_numpy(img).to(device)
# transform all points on the img plane of the currently selected img
# use the theorem:
# if points would have been 4 x 1 (single point)
# (Img_mat . Points)^T = Points^T . Img_mat^T
# (4 x 4 . 4 x 1)^T = (1 x 4) . (4 x 4) -> 1 x 4
# this can be generalized for n points
# points is n x 4 already (no need to transpose)
projected_points = points @ img_mat.T
# normalize the projected coordinates
depth = projected_points[:, 2]
# add an epsilon to prevent division by zero
depth[depth == 0.0] = torch.finfo(torch.float32).eps
projected_points[:, 0] = torch.div(projected_points[:, 0], depth)
projected_points[:, 1] = torch.div(projected_points[:, 1], depth)
# create a mask of valid points
# valid means that the points lie inside the image x y borders, z is not filtered here
mask_x = torch.logical_and(projected_points[:, 0] > 0,
projected_points[:, 0] < img.shape[1])
mask_y = torch.logical_and(projected_points[:, 1] > 0,
projected_points[:, 1] < img.shape[0])
if self._filter_close:
valid_points_mask = torch.logical_and(mask_x, mask_y)
mask_z = projected_points[:, 2] > self._filter_close
valid_points_mask = torch.logical_and(valid_points_mask,
mask_z)
else:
valid_points_mask = torch.logical_and(mask_x, mask_y)
# use only the points inside the image
projected_points = projected_points[valid_points_mask]
# get x y as pixel indices
img_row_idxs = projected_points[:, 1].long()
img_col_idxs = projected_points[:, 0].long()
# self._debug_visualize(
# img,
# projected_points[:, 0].long(),
# projected_points[:, 1].long(),
# projected_points[:, 2],
# img_idx,
# )
projected_points_colors = img[img_row_idxs, img_col_idxs]
# TODO how to handle overlapping images?
# atm we override with the last camera image in case of conflict
points_colors[valid_points_mask] = projected_points_colors.float()
colored_points_mask[valid_points_mask] = True
# augment the points with the colors
valid_point_colors = points_colors[colored_points_mask]
valid_points = results["points"].tensor[colored_points_mask]
valid_points = valid_points[:, self._use_dim]
valid_points = torch.cat((valid_points, valid_point_colors), dim=1)
# collect all points that lie outside the images
invalid_points = results["points"].tensor[~colored_points_mask]
invalid_points = invalid_points[:, self._use_dim]
# zero pad the missing color channels
invalid_points = torch.cat(
(invalid_points,
torch.zeros((len(invalid_points), valid_point_colors.size(1)))),
dim=1)
points = torch.cat([valid_points, invalid_points], dim=0)
points_class = get_points_type(self._coord_type)
# TODO attributes
points = points_class(points,
points_dim=points.shape[-1],
attribute_dims=None)
# print("new dim =", points.points_dim)
results["points"] = points
return results
def __call__(self, results):
points = results["points"].tensor
device = points.device
img_T_lidar_tfs = (torch.tensor(
results["img_T_lidar"], ).to(device).float())
# list cameras [h x w x 1]
depth_maps = results["depth_maps"]
points = []
for img_idx in self._img_idxs:
img_T_lidar = img_T_lidar_tfs[img_idx]
depth_map = depth_maps[img_idx]
# remove extra depth channel
depth_map = depth_map.squeeze()
# create a point cloud from the pixels
# get the valid pixel coordinates
# all pixels with depth != 0 are valid
depth_map = torch.from_numpy(depth_map)
valid_idxs = torch.nonzero(depth_map)
points_img_plane = torch.ones((len(valid_idxs), 3))
valid_y_idxs = valid_idxs[:, 0]
valid_x_idxs = valid_idxs[:, 1]
valid_depth_values = depth_map[valid_y_idxs, valid_x_idxs]
# create the points for back projection
points_img_plane = torch.ones((len(valid_depth_values), 4))
points_img_plane[:, 0] = valid_x_idxs
points_img_plane[:, 1] = valid_y_idxs
# for back projection we need points (u,v,1, 1/z)
# because a point (x,y,z,1) is projected to (u,v,1,1/z)
# store the inverted depth (valid because depth was forced to non zero values beforehand)
points_img_plane[:, 3] = 1.0 / valid_depth_values
lidar_T_img = torch.inverse(img_T_lidar)
# transform all points on the img plane of the currently selected img
# use the theorem:
# if points would have been 4 x 1 (single point)
# (Img_mat . Points)^T = Points^T . Img_mat^T
# (4 x 4 . 4 x 1)^T = (1 x 4) . (4 x 4) -> 1 x 4
# this can be generalized for n points
# points is n x 4 already (no need to transpose)
points_lidar = points_img_plane @ lidar_T_img.T
points_lidar *= torch.unsqueeze(valid_depth_values, dim=-1)
# TODO how to hanle overlapping images?
points.append(points_lidar[:, 0:3])
points = torch.cat(points, dim=0)
points_class = get_points_type(self._coord_type)
points = points_class(points,
points_dim=points.shape[-1],
attribute_dims=None)
results['points'] = points
return results