def getModel(PCs, boxes, offset=0, scale=1.0, normalize=False):
if len(PCs) == 0:
return PointCloud(np.ones((3, 0)))
points = np.ones((PCs[0].points.shape[0], 0))
for PC, box in zip(PCs, boxes):
cropped_PC = cropAndCenterPC(PC,
box,
offset=offset,
scale=scale,
normalize=normalize)
# try:
if cropped_PC.points.shape[1] > 0:
points = np.concatenate([points, cropped_PC.points], axis=1)
PC = PointCloud(points)
return PC
def getPCandBBfromPandas(self, box, calib):
center = [box["x"], box["y"] - box["height"] / 2, box["z"]]
size = [box["width"], box["length"], box["height"]]
orientation = Quaternion(
axis=[0, 1, 0], radians=box["rotation_y"]) * Quaternion(
axis=[1, 0, 0], radians=np.pi / 2) # 这波啊，这波是X-Y平面的旋转
# 绕Y正半轴逆时针旋转，弧度为box["rotation_y"]
# 再绕X正半轴逆时针旋转，角度为90°
BB = Box(center, size, orientation)
try:
# VELODYNE PointCloud
velodyne_path = os.path.join(self.KITTI_velo, box["scene"],
'{:06}.bin'.format(box["frame"]))
# ./data/traning/velodyne/box["scene"]/box["frame"].bin
# box["scene"]，例如0020文件夹，对应第二十一个场景序列
# box["frame"].bin，例如000001.bin
PC = PointCloud(
np.fromfile(velodyne_path, dtype=np.float32).reshape(-1, 4).T)
PC.transform(calib) # 校准
except:
# in case the Point cloud is missing
# (0001/[000177-000180].bin) 果然没了
PC = PointCloud(np.array([[0, 0, 0]]).T)
return PC, BB
def getPCandBBfromPandas(self, box, calib):
#求出车辆的中心点 从此处的中心点是根据KITTI中相机坐标系下的中心点
# 减去一半的高度移到地面上
center = [box["x"], box["y"] - box["height"] / 2, box["z"]]
size = [box["width"], box["length"], box["height"]]
#下面这个函数没有完全看明白 应该是将roy角转换成四元数吧
orientation = Quaternion(axis=[0, 1, 0],
radians=box["rotation_y"]) * Quaternion(
axis=[1, 0, 0], radians=np.pi / 2)
BB = Box(center, size, orientation) #用中心点坐标和w,h,l以及旋转角来初始化BOX这个类
State = True
try:
# VELODYNE PointCloud
velodyne_path = os.path.join(
self.KITTI_velo, box["scene"],
'%06d.bin' % (box["frame"])) #f'{box["frame"]:06}.bin')
#从点云的.bin文件中读取点云数据并且转换为4*x的矩阵，且去掉最后的一行的点云的密度表示数据
PC = PointCloud(
np.fromfile(velodyne_path, dtype=np.float32).reshape(-1, 4).T)
#将点云转换到相机坐标系下 因为label中的坐标和h,w,l在相机坐标系下的
PC.transform(calib)
except FileNotFoundError:
# logging.error("No such file found\n%s\n", velodyne_path)
PC = PointCloud(np.array([[0, 0, 0]]).T)
State = False
return PC, BB, State
def getlabelPC(PC, box, offset=0, scale=1.0):
box_tmp = copy.deepcopy(box)
new_PC = PointCloud(PC.points.copy())
rot_mat = np.transpose(box_tmp.rotation_matrix)
trans = -box_tmp.center
# align data
new_PC.translate(trans)
box_tmp.translate(trans)
new_PC.rotate((rot_mat))
box_tmp.rotate(Quaternion(matrix=(rot_mat)))
box_tmp.wlh = box_tmp.wlh * scale
maxi = np.max(box_tmp.corners(), 1) + offset
mini = np.min(box_tmp.corners(), 1) - offset
x_filt_max = new_PC.points[0, :] < maxi[0]
x_filt_min = new_PC.points[0, :] > mini[0]
y_filt_max = new_PC.points[1, :] < maxi[1]
y_filt_min = new_PC.points[1, :] > mini[1]
z_filt_max = new_PC.points[2, :] < maxi[2]
z_filt_min = new_PC.points[2, :] > mini[2]
close = np.logical_and(x_filt_min, x_filt_max)
close = np.logical_and(close, y_filt_min)
close = np.logical_and(close, y_filt_max)
close = np.logical_and(close, z_filt_min)
close = np.logical_and(close, z_filt_max)
new_label = np.zeros(new_PC.points.shape[1])
new_label[close] = 1
return new_label
def cropPC(PC, box, offset=0, scale=1.0):
box_tmp = copy.deepcopy(box)
box_tmp.wlh = box_tmp.wlh * scale
maxi = np.max(box_tmp.corners(), 1) + offset
mini = np.min(box_tmp.corners(), 1) - offset
x_filt_max = PC.points[0, :] < maxi[0]
x_filt_min = PC.points[0, :] > mini[0]
y_filt_max = PC.points[1, :] < maxi[1]
y_filt_min = PC.points[1, :] > mini[1]
z_filt_max = PC.points[2, :] < maxi[2]
z_filt_min = PC.points[2, :] > mini[2]
close = np.logical_and(x_filt_min, x_filt_max)
close = np.logical_and(close, y_filt_min)
close = np.logical_and(close, y_filt_max)
close = np.logical_and(close, z_filt_min)
close = np.logical_and(close, z_filt_max)
new_PC = PointCloud(PC.points[:, close])
return new_PC
def getPCandBBfromPandas(self, box, calib):
center = [box["x"], box["y"] - box["height"] / 2, box["z"]]
size = [box["width"], box["length"], box["height"]]
orientation = Quaternion(
axis=[0, 1, 0], radians=box["rotation_y"]) * Quaternion(
axis=[1, 0, 0], radians=np.pi / 2)
BB = Box(center, size, orientation)
try:
# VELODYNE PointCloud
velodyne_path = os.path.join(self.KITTI_velo, box["scene"],
f'{box["frame"]:06}.bin')
PC = PointCloud(
np.fromfile(velodyne_path, dtype=np.float32).reshape(-1, 4).T)
PC.transform(calib)
except FileNotFoundError:
# in case the Point cloud is missing
# (0001/[000177-000180].bin)
PC = PointCloud(np.array([[0, 0, 0]]).T)
return PC, BB
def map_pointcloud_to_image(nusc,
radar_points,
pointsensor_token,
camera_token,
target_resolution=(None, None)):
"""
Given a point sensor (lidar/radar) token and camera sample_data token, load point-cloud and map it to the image
plane.
:param radar_pints: [list] list of radar points
:param pointsensor_token: [str] Lidar/radar sample_data token.
:param camera_token: [str] Camera sample_data token.
:param target_resolution: [tuple of int] determining the output size for the radar_image. None for no change
:return (points <np.float: 2, n)
"""
# Initialize the database
cam = nusc.get('sample_data', camera_token)
pointsensor = nusc.get('sample_data', pointsensor_token)
# import pdb;pdb.set_trace()
pc = PointCloud(radar_points)
# Points live in the point sensor frame. So they need to be transformed via global to the image plane.
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor',
pointsensor['calibrated_sensor_token'])
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
pc.translate(np.array(cs_record['translation']))
# Second step: transform to the global frame.
poserecord = nusc.get('ego_pose', pointsensor['ego_pose_token'])
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix)
pc.translate(np.array(poserecord['translation']))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
poserecord = nusc.get('ego_pose', cam['ego_pose_token'])
pc.translate(-np.array(poserecord['translation']))
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix.T)
# Fourth step: transform into the camera.
cs_record = nusc.get('calibrated_sensor', cam['calibrated_sensor_token'])
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
# Fifth step: actually take a "picture" of the point cloud.
# Grab the depths (camera frame z axis points away from the camera).
# intrinsic_resized = np.matmul(camera_resize, np.array(cs_record['camera_intrinsic']))
view = np.array(cs_record['camera_intrinsic'])
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points, view, normalize=True) #resize here
# Resizing to target resolution
if target_resolution[1]: # resizing width
points[0, :] *= (target_resolution[1] / cam['width'])
if target_resolution[0]: # resizing height
points[1, :] *= (target_resolution[0] / cam['height'])
# actual_resolution = (cam['height'], cam['width'])
# for i in range(len(target_resolution)):
# if target_resolution[i]:
# points[i,:] *= (target_resolution[i]/actual_resolution[i])
return points
def main(args):
# Plotting parameters
params = {
'legend.fontsize': 'x-large',
'figure.figsize': (15, 5),
'axes.labelsize': 'x-large',
'axes.titlesize': 'x-large',
'xtick.labelsize': 'x-large',
'ytick.labelsize': 'x-large',
'figure.max_open_warning': 1000
}
pylab.rcParams.update(params)
# Create Model
if "Random" in args.model_name:
AE_chkpt_file = ""
chkpt_file = None
if "PreTrained" in args.model_name:
AE_chkpt_file = os.path.join("..", "models", "Pretrain_ShapeNet",
"model.pth.tar")
chkpt_file = None
if "Ours" in args.model_name:
AE_chkpt_file = os.path.join("..", "models", "Pretrain_ShapeNet",
"model.pth.tar")
chkpt_file = os.path.join("..", "models", "Ours", "model.pth.tar")
model = Model(128, chkpt_file=chkpt_file,
AE_chkpt_file=AE_chkpt_file).cuda()
model.eval()
# Create Dataset
dataset = SiameseTest(model=model,
path=args.path_KITTI,
split="Test",
scale_BB=1.25)
# Create search space
x_space = np.linspace(-4, 4, 9) # min, max, number of sample
y_space = np.linspace(-4, 4, 9) # min, max, number of sample
a_space = np.linspace(-10, 10, 3) # min, max, number of sample
X, Y, A = np.meshgrid(x_space, y_space, a_space) # create mesh grid
dataset.search_grid = np.array([X.flatten(), Y.flatten(), A.flatten()]).T
dataset.num_candidates_perframe = len(dataset.search_grid)
# Get List of PCs and BBs from dataset
PCs, BBs, list_of_anno = dataset[args.track]
print("Length of candidates = ", len(dataset.search_grid),
" length of pointclouds in the tracklet", len(PCs))
# Loop over Point Cloud and Bounding Boxes
for i in tqdm(range(1, len(PCs))):
this_PC = PCs[i]
this_BB = BBs[i]
ref_BB = BBs[i]
# create candidate PCs and BBs
search_space = dataset.search_grid
candidate_BBs = generate_boxes(ref_BB, search_space=search_space)
candidate_PCs = [
cropAndCenterPC(this_PC,
box,
offset=dataset.offset_BB,
scale=dataset.scale_BB,
normalize="PreTrained" in args.model_name)
for box in candidate_BBs
]
candidate_PCs_reg = [regularizePC(PC, model) for PC in candidate_PCs]
candidate_PCs_torch = torch.cat(candidate_PCs_reg, dim=0).cuda()
# create model PC
model_PC = getModel(PCs[:i],
BBs[:i],
offset=dataset.offset_BB,
scale=dataset.scale_BB,
normalize="PreTrained" in args.model_name)
# ax = fig.add_subplot(111, projection='3d')
# ax.scatter(model_pc[] ys, zs, marker=m)
repeat_shape = np.ones(len(candidate_PCs_torch.shape), dtype=np.int32)
repeat_shape[0] = len(candidate_PCs)
model_PC_encoded = regularizePC(model_PC, model).repeat(
tuple(repeat_shape)).cuda()
# infer model
output, model_PC_decoded = model(candidate_PCs_torch, model_PC_encoded)
model_PC_decoded = PointCloud(
model_PC_decoded.detach().cpu().numpy()[0])
if "PreTrained" in args.model_name:
normalizer = [ref_BB.wlh[1], ref_BB.wlh[0], ref_BB.wlh[2]]
model_PC_decoded.points = model_PC_decoded.points * np.atleast_2d(
normalizer).T
# store scores for all candidates
scores = output.detach().cpu().numpy()
idx = np.argmax(scores) # select index of higest score
box = candidate_BBs[idx] # select box with highest score
# keep highest score for angle space
scores0 = scores[1::3] # angle = 0
scoresp10 = scores[2::3] # angle = 10
scoresm10 = scores[0::3] # angle = -10
scores = np.max(np.stack([scores0, scoresp10, scoresm10]), axis=0)
X, Y = np.meshgrid(x_space, y_space)
Z = scores.reshape(len(x_space), len(y_space))
view_PC = cropAndCenterPC(this_PC, this_BB, offset=5)
view_BB = Box([0, 0, 0], this_BB.wlh, Quaternion())
# Crop point clouds around GT
x_filt_max = view_PC.points[0, :] < 5
x_filt_min = view_PC.points[0, :] > -5
y_filt_max = view_PC.points[1, :] < 5
y_filt_min = view_PC.points[1, :] > -5
z_filt_max = view_PC.points[2, :] < 2
z_filt_min = view_PC.points[2, :] > -1
close = np.logical_and(x_filt_min, x_filt_max)
close = np.logical_and(close, y_filt_min)
close = np.logical_and(close, y_filt_max)
close = np.logical_and(close, z_filt_min)
close = np.logical_and(close, z_filt_max)
view_PC = PointCloud(view_PC.points[:, close])
# Create figure for TRACKING
fig = plt.figure(figsize=(15, 10), facecolor="white")
# Create axis in 3D
ax = fig.gca(projection='3d')
# Scatter plot the cropped point cloud
ratio = 1
ax.scatter(view_PC.points[0, ::ratio],
view_PC.points[1, ::ratio],
view_PC.points[2, ::ratio] / 2 - 1,
s=3,
c=view_PC.points[2, ::ratio])
# point order to draw a full Box
order = [0, 4, 0, 1, 5, 1, 2, 6, 2, 3, 7, 3, 0, 4, 5, 6, 7, 4]
# Plot best results
ax.plot(view_BB.corners()[0, order],
view_BB.corners()[1, order],
view_BB.corners()[2, order] / 2 - 1,
color="red",
alpha=0.5,
linewidth=5,
linestyle=":")
# center the box to plot the GT
box.translate(-this_BB.center)
box.rotate(this_BB.orientation.inverse)
# Plot GT box
ax.plot(box.corners()[0, order],
box.corners()[1, order],
box.corners()[2, order] / 2 - 1,
color="blue",
alpha=0.5,
linewidth=2,
linestyle=":")
# point order to draw the visible part of the box
# order = [3, 0, 1, 2, 3, 7, 4, 0, 4, 5, 1]
order = [6, 7, 4, 5, 6, 2, 1, 5, 1, 0, 4]
# Plot best results
ax.plot(view_BB.corners()[0, order],
view_BB.corners()[1, order],
view_BB.corners()[2, order] / 2 - 1,
color="red",
linewidth=5)
# Plot GT box
ax.plot(box.corners()[0, order],
box.corners()[1, order],
box.corners()[2, order] / 2 - 1,
color="blue",
linewidth=2)
# Plot results for all cadidate as a surface
surf = ax.plot_surface(X,
Y,
Z,
cmap=cm.coolwarm,
rstride=1,
cstride=1,
linewidth=0.3,
edgecolors=[0, 0, 0, 0.8],
antialiased=True,
vmin=0,
vmax=1)
ax.zaxis.set_major_locator(LinearLocator(10))
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
# Add a color bar which maps values to colors.
fig.colorbar(surf, shrink=0.5, aspect=5)
ax.w_xaxis.set_pane_color((0.9, 0.9, 0.9, 0.2))
ax.w_yaxis.set_pane_color((0.9, 0.9, 0.9, 0.2))
ax.w_zaxis.set_pane_color((0.9, 0.9, 0.9, 0.2))
ax.w_xaxis.line.set_color((1.0, 1.0, 1.0, 0.0))
ax.w_yaxis.line.set_color((1.0, 1.0, 1.0, 0.0))
ax.w_zaxis.line.set_color((1.0, 1.0, 1.0, 0.0))
ax.set_xticks(x_space[::2])
ax.set_yticks(y_space[::2])
ax.set_xticklabels([])
ax.set_yticklabels([])
ax.set_zticks([0, 0.2, 0.4, 0.6, 0.8, 1])
ax.view_init(20, -140)
plt.tight_layout()
ax.set_xlim(-5, 5)
ax.set_ylim(-5, 5)
ax.set_zlim(-1.5, 1)
# Save figure as Tracking results
os.makedirs(os.path.join(args.path_results, f"{args.track:04.0f}",
"Tracking"),
exist_ok=True)
plt.savefig(os.path.join(args.path_results, f"{args.track:04.0f}",
"Tracking", f"{i}_{args.model_name}.png"),
format='png',
dpi=100)
# Create figure for RECONSTRUCTION
fig = plt.figure(figsize=(9, 6), facecolor="white")
# Create axis in 3D
ax = fig.gca(projection='3d')
# select 2K point to visualize
sample = np.random.randint(low=0,
high=model_PC_decoded.points.shape[1],
size=2048,
dtype=np.int64)
print("Reconstructed size", model_PC_decoded.points.shape[1])
# Scatter plot the point cloud
# ax.scatter(
# model_PC_decoded.points[0, sample],
# model_PC_decoded.points[1, sample],
# model_PC_decoded.points[2, sample],
# s=3,
# c=model_PC_decoded.points[2, sample])
ax.scatter(model_PC_decoded.points[0, :],
model_PC_decoded.points[1, :],
model_PC_decoded.points[2, :],
s=3,
c=model_PC_decoded.points[2, sample])
# Plot the car BB
order = [0, 4, 0, 1, 5, 1, 2, 6, 2, 3, 7, 3, 0, 4, 5, 6, 7, 4]
ax.plot(view_BB.corners()[0, order],
view_BB.corners()[1, order],
view_BB.corners()[2, order],
color="red",
alpha=0.5,
linewidth=3,
linestyle=":")
order = [6, 7, 4, 5, 6, 2, 1, 5, 1, 0, 4]
ax.plot(view_BB.corners()[0, order],
view_BB.corners()[1, order],
view_BB.corners()[2, order],
color="red",
linewidth=3)
# setup axis
ax.set_xticks([-2, -1, 0, 1, 2])
ax.set_xticklabels([], fontsize=10)
ax.set_yticks([-1, 0, 1])
ax.set_yticklabels([], fontsize=10)
ax.set_zticks([-1, 0, 1])
ax.set_zticklabels([], fontsize=10)
ax.view_init(20, -140)
plt.tight_layout()
ax.set_xlim(-2, 2)
ax.set_ylim(-2, 2)
ax.set_zlim(-1.5, 1.5)
# Save figure as Decoded Model results
os.makedirs(os.path.join(args.path_results, f"{args.track:04.0f}",
"Reconstruction"),
exist_ok=True)
plt.savefig(os.path.join(args.path_results, f"{args.track:04.0f}",
"Reconstruction",
f"{i}_{args.model_name}.png"),
format='png',
dpi=100)
# Create figure for MODEL PC
fig = plt.figure(figsize=(9, 6), facecolor="white")
ax = fig.gca(projection='3d')
if "PreTrained" in args.model_name:
model_PC = getModel(PCs[:i],
BBs[:i],
offset=dataset.offset_BB,
scale=dataset.scale_BB)
# sample 2K Points
sample = np.random.randint(low=0,
high=model_PC.points.shape[1],
size=2048,
dtype=np.int64)
# Scatter plot the Point cloud
ax.scatter(model_PC.points[0, sample],
model_PC.points[1, sample],
model_PC.points[2, sample],
s=3,
c=model_PC.points[2, sample])
# Plot the Bounding Box
order = [0, 4, 0, 1, 5, 1, 2, 6, 2, 3, 7, 3, 0, 4, 5, 6, 7, 4]
ax.plot(view_BB.corners()[0, order],
view_BB.corners()[1, order],
view_BB.corners()[2, order],
color="red",
alpha=0.5,
linewidth=3,
linestyle=":")
order = [6, 7, 4, 5, 6, 2, 1, 5, 1, 0, 4]
ax.plot(view_BB.corners()[0, order],
view_BB.corners()[1, order],
view_BB.corners()[2, order],
color="red",
linewidth=3)
# setup axis
ax.set_xticks([-2, -1, 0, 1, 2])
ax.set_xticklabels([], fontsize=10)
ax.set_yticks([-1, 0, 1])
ax.set_yticklabels([], fontsize=10)
ax.set_zticks([-1, 0, 1])
ax.set_zticklabels([], fontsize=10)
ax.view_init(20, -140)
plt.tight_layout()
ax.set_xlim(-2, 2)
ax.set_ylim(-2, 2)
ax.set_zlim(-1.5, 1.5)
ax.set_zlabel('Model')
# Save figure as Model results
os.makedirs(os.path.join(args.path_results, f"{args.track:04.0f}",
"Model"),
exist_ok=True)
plt.savefig(os.path.join(args.path_results, f"{args.track:04.0f}",
"Model", f"{i}_{args.model_name}.png"),
format='png',
dpi=100)
# create GIF Tracking
images = []
for i in tqdm(range(1, len(PCs))):
image_path = os.path.join(args.path_results, f'{args.track:04.0f}',
"Tracking", f'{i}_{args.model_name}.png')
images.append(imageio.imread(image_path))
image_path = os.path.join(
args.path_results,
f'{args.track:04.0f}_{args.model_name}_Tracking.gif')
imageio.mimsave(image_path, images)
# create GIF GT Car
images = []
for i in tqdm(range(1, len(PCs))):
image_path = os.path.join(args.path_results, f'{args.track:04.0f}',
"Model", f'{i}_{args.model_name}.png')
images.append(imageio.imread(image_path))
image_path = os.path.join(
args.path_results, f'{args.track:04.0f}_{args.model_name}_Model.gif')
imageio.mimsave(image_path, images)
# create GIF Reconstructed Car
images = []
for i in tqdm(range(1, len(PCs))):
image_path = os.path.join(args.path_results, f'{args.track:04.0f}',
"Reconstruction",
f'{i}_{args.model_name}.png')
images.append(imageio.imread(image_path))
image_path = os.path.join(
args.path_results,
f'{args.track:04.0f}_{args.model_name}_Reconstruction.gif')
imageio.mimsave(image_path, images)