def get_lidar_point_cloud_jhuang(img_idx,
frame_calib,
velo_dir,
im_size=None,
min_intensity=None,
prefix=''):
""" Calculates the lidar point cloud, and optionally returns only the
points that are projected to the image.
:param img_idx: image index
:param calib_dir: directory with calibration files
:param velo_dir: directory with velodyne files
:param im_size: (optional) 2 x 1 list containing the size of the image
to filter the point cloud [w, h]
:param min_intensity: (optional) minimum intensity required to keep a point
:return: (3, N) point_cloud in the form [[x,...][y,...][z,...]]
"""
# Read lidar data
x, y, z, i = calib_utils.read_lidar_jhuang(velo_dir=velo_dir,
img_idx=img_idx,
prefix=prefix)
# Calculate the point cloud
pts = np.vstack((x, y, z)).T
pts = calib_utils.lidar_to_cam_frame(pts, frame_calib)
# The given image is assumed to be a 2D image
if not im_size:
point_cloud = pts.T
return point_cloud
else:
# Only keep points in front of camera (positive z)
pts = pts[pts[:, 2] > 0]
point_cloud = pts.T
# Project to image frame
point_in_im = calib_utils.project_to_image(point_cloud,
p=frame_calib.p2).T
# Filter based on the given image size
image_filter = (point_in_im[:, 0] > 0) & \
(point_in_im[:, 0] < im_size[0]) & \
(point_in_im[:, 1] > 0) & \
(point_in_im[:, 1] < im_size[1])
if not min_intensity:
return pts[image_filter].T
else:
intensity_filter = i > min_intensity
point_filter = np.logical_and(image_filter, intensity_filter)
return pts[point_filter].T
def get_lidar_point_cloud_with_color(img_idx,
img_dir,
calib_dir,
velo_dir,
im_size=None):
""" Calculates the lidar point cloud, and optionally returns only the
points that are projected to the image.
:param img_idx: image index
:param calib_dir: directory with calibration files
:param velo_dir: directory with velodyne files
:param im_size: (optional) 2 x 1 list containing the size of the image
to filter the point cloud [w, h]
:param min_intensity: (optional) minimum intensity required to keep a point
:return: (3, N) point_cloud in the form [[x,...][y,...][z,...]]
"""
# Read calibration info
frame_calib = calib_utils.read_calibration(calib_dir, img_idx)
x, y, z, i = calib_utils.read_lidar(velo_dir=velo_dir, img_idx=img_idx)
# Calculate the point cloud
pts = np.vstack((x, y, z)).T
pts = calib_utils.lidar_to_cam_frame(pts, frame_calib)
# The given image is assumed to be a 2D image
if not im_size:
point_cloud = pts.T
return point_cloud
else:
# Only keep points in front of camera (positive z)
pts = pts[pts[:, 2] > 0]
point_cloud = pts.T
# Project to image frame
point_in_im = calib_utils.project_to_image(point_cloud,
p=frame_calib.p2).T
# Filter based on the given image size
image_filter = (point_in_im[:, 0] > 0) & \
(point_in_im[:, 0] < im_size[0]) & \
(point_in_im[:, 1] > 0) & \
(point_in_im[:, 1] < im_size[1])
img_dir = img_dir + "/%06d.png" % img_idx
img = Image.open(img_dir)
img = np.array(img)
point_colors = img[point_in_im[image_filter, 1].astype(np.int),
point_in_im[image_filter, 0].astype(np.int)]
# return np.vstack((pts[image_filter].T, point_colors[image_filter].T))
return pts[image_filter].T, point_colors.T
def get_lidar_point_cloud(img_idx, calib_dir, velo_dir,
im_size=None, min_intensity=None):
""" Calculates the lidar point cloud, and optionally returns only the
points that are projected to the image.
:param img_idx: image index
:param calib_dir: directory with calibration files
:param velo_dir: directory with velodyne files
:param im_size: (optional) 2 x 1 list containing the size of the image
to filter the point cloud [w, h]
:param min_intensity: (optional) minimum intensity required to keep a point
:return: (3, N) point_cloud in the form [[x,...][y,...][z,...]]
"""
# Read calibration info
frame_calib = calib_utils.read_calibration(calib_dir, img_idx)#读取calib文件信息并保存到对象中
x, y, z, i = calib_utils.read_lidar(velo_dir=velo_dir, img_idx=img_idx)#从文件读取点云数据的x,y,z,和密度
# Calculate the point cloud
pts = np.vstack((x, y, z)).T#点云位置信息
pts = calib_utils.lidar_to_cam_frame(pts, frame_calib)#点云投射到相机坐标
# The given image is assumed to be a 2D image
if not im_size:
point_cloud = pts.T
return point_cloud
else:
# Only keep points in front of camera (positive z) 相机坐标是z轴，已经投影到相机坐标了
pts = pts[pts[:, 2] > 0]
point_cloud = pts.T
# Project to image frame #投影到像素坐标
point_in_im = calib_utils.project_to_image(point_cloud, p=frame_calib.p2).T
# Filter based on the given image size 保留在图片范围的点云,坐标在相机坐标系下
image_filter = (point_in_im[:, 0] > 0) & \
(point_in_im[:, 0] < im_size[0]) & \
(point_in_im[:, 1] > 0) & \
(point_in_im[:, 1] < im_size[1])#索引值
if not min_intensity:
return pts[image_filter].T
else:
intensity_filter = i > min_intensity
point_filter = np.logical_and(image_filter, intensity_filter)
return pts[point_filter].T
def test_read_lidar(self):
test_data_dir = ROOTDIR + "/tests/test_data/calib"
velo_mat = scipy.io.loadmat(test_data_dir + '/test_velo.mat')
velo_true = velo_mat['current_frame']['xyz_velodyne'][0][0][:,0:3]
x, y, z, i = calib.read_lidar(velo_dir=test_data_dir,
img_idx=0)
velo_test = np.vstack((x, y, z)).T
np.testing.assert_almost_equal(velo_true, velo_test, decimal=5, verbose=True)
velo_mat = scipy.io.loadmat(test_data_dir + '/test_velo_tf.mat')
velo_true_tf = velo_mat['velo_cam_frame']
calib_out = calib.read_calibration(test_data_dir, 0)
xyz_cam = calib.lidar_to_cam_frame(velo_test, calib_out)
np.testing.assert_almost_equal(velo_true_tf, xyz_cam, decimal=5, verbose=True)
def _get_point_cloud(self,
image_shape,
pointcloud,
frame_calib,
min_intensity=None):
im_size = [image_shape[1], image_shape[0]]
x, y, z, i = pointcloud
print("Shape of x, y, z, i: ", x.shape, y.shape, z.shape, i.shape)
# Calculate the point cloud
pts = np.vstack((x, y, z)).T
pts = calib_utils.lidar_to_cam_frame(pts, frame_calib)
# The given image is assumed to be a 2D image
if not im_size:
point_cloud = pts.T
return point_cloud
else:
# Only keep points in front of camera (positive z)
pts = pts[pts[:, 2] > 0]
point_cloud = pts.T
# Project to image frame
point_in_im = calib_utils.project_to_image(point_cloud,
p=frame_calib.p2).T
# Filter based on the given image size
image_filter = (point_in_im[:, 0] > 0) & \
(point_in_im[:, 0] < im_size[0]) & \
(point_in_im[:, 1] > 0) & \
(point_in_im[:, 1] < im_size[1])
if not min_intensity:
point_cloud = pts[image_filter].T
else:
intensity_filter = i > min_intensity
point_filter = np.logical_and(image_filter, intensity_filter)
point_cloud = pts[point_filter].T
return point_cloud
def get_lidar_point_cloud_sub(img_idx,
calib_dir,
velo_dir,
im_size=None,
min_intensity=None,
stride_sub=1):
""" Calculates the lidar point cloud, and optionally returns only the
points that are projected to the image.
:param img_idx: image index
:param calib_dir: directory with calibration files
:param velo_dir: directory with velodyne files
:param im_size: (optional) 2 x 1 list containing the size of the image
to filter the point cloud [w, h]
:param min_intensity: (optional) minimum intensity required to keep a point
:return: (3, N) point_cloud in the form [[x,...][y,...][z,...]]
"""
# Read calibration info
frame_calib = calib_utils.read_calibration(calib_dir, img_idx)
x, y, z, i = calib_utils.read_lidar(velo_dir=velo_dir, img_idx=img_idx)
starts = np.where(np.diff(np.sign(y)) > 0)[0] + 1
true_starts = np.append(np.diff(starts) > 2, [True])
starts = starts[true_starts]
n_lasers = starts.shape[0] + 1
starts = [0] + starts.tolist() + [len(x)]
include = np.zeros(len(x), dtype=bool)
lasers_num = range(0, SINFields.LASERS_NUM, stride_sub)
for laser in lasers_num:
if laser < n_lasers:
include[starts[laser]:starts[laser + 1]] = True
i = i[include]
# Calculate the point cloud
pts = np.vstack((x[include], y[include], z[include])).T
pts = calib_utils.lidar_to_cam_frame(pts, frame_calib)
# The given image is assumed to be a 2D image
if not im_size:
point_cloud = pts.T
return point_cloud
else:
# Only keep points in front of camera (positive z)
pts = pts[pts[:, 2] > 0]
point_cloud = pts.T
# Project to image frame
point_in_im = calib_utils.project_to_image(point_cloud,
p=frame_calib.p2).T
# Filter based on the given image size
image_filter = (point_in_im[:, 0] > 0) & \
(point_in_im[:, 0] < im_size[0]) & \
(point_in_im[:, 1] > 0) & \
(point_in_im[:, 1] < im_size[1])
if not min_intensity:
return pts[image_filter].T
else:
intensity_filter = i > min_intensity
point_filter = np.logical_and(image_filter, intensity_filter)
return pts[point_filter].T
def plotSINImage(data_dir,
out_dir,
img_idx,
sin_type,
sin_level,
on_img,
sin_input_name,
mask_2d=None):
fname = '{:06d}'.format(img_idx)
path_img = os.path.join(data_dir, 'image_2')
path_velo = os.path.join(data_dir, 'velodyne')
path_calib = os.path.join(data_dir, 'calib')
# Load image
cv_bgr_image = cv2.imread(os.path.join(path_img, fname + '.png'))
rgb_image = cv_bgr_image[..., ::-1]
image_shape = rgb_image.shape[0:2]
# Load point cloud
frame_calib = calib_utils.read_calibration(path_calib, img_idx)
x, y, z, _ = calib_utils.read_lidar(velo_dir=path_velo, img_idx=img_idx)
if sin_type == 'lowres':
starts = np.where(np.diff(np.sign(y)) > 0)[0] + 1
true_starts = np.append(np.diff(starts) > 2, [True])
starts = starts[true_starts]
n_lasers = starts.shape[0] + 1
starts = [0] + starts.tolist() + [len(x)]
include = np.zeros(len(x), dtype=bool)
stride_sub = get_stride_sub(sin_level)
lasers_num = range(0, SINFields.LASERS_NUM, stride_sub)
for laser in lasers_num:
if laser < n_lasers:
include[starts[laser]:starts[laser + 1]] = True
pts_lowres = np.vstack((x[include], y[include], z[include])).T # N x 3
pts_lowres = calib_utils.lidar_to_cam_frame(pts_lowres, frame_calib)
pts_lowres = pts_lowres[
pts_lowres[:, 2] >
0] # Only keep points in front of camera (positive z)
# Project to image frame
point_in_im_lowres = calib_utils.project_to_image(
pts_lowres.T, p=frame_calib.p2).T # N x 3
im_size = [image_shape[1], image_shape[0]]
# Filter based on the given image size
image_filter_lowres = (point_in_im_lowres[:, 0] > 0) & \
(point_in_im_lowres[:, 0] < im_size[0]) & \
(point_in_im_lowres[:, 1] > 0) & \
(point_in_im_lowres[:, 1] < im_size[1])
point_cloud_lowres = pts_lowres[image_filter_lowres, :].T
else:
include = np.ones(len(x), dtype=bool)
pts = np.vstack((x, y, z)).T # N x 3
pts = calib_utils.lidar_to_cam_frame(pts, frame_calib)
pts = pts[pts[:, 2] >
0] # Only keep points in front of camera (positive z)
# Project to image frame
point_in_im = calib_utils.project_to_image(pts.T,
p=frame_calib.p2).T # N x 3
im_size = [image_shape[1], image_shape[0]]
# Filter based on the given image size
image_filter = (point_in_im[:, 0] > 0) & \
(point_in_im[:, 0] < im_size[0]) & \
(point_in_im[:, 1] > 0) & \
(point_in_im[:, 1] < im_size[1])
point_cloud = pts[image_filter, :].T
point_in_im = point_in_im[image_filter, :]
image_input_sin, point_cloud_sin = genSINtoInputs(
rgb_image,
point_cloud,
sin_type=sin_type,
sin_level=sin_level,
sin_input_name=sin_input_name,
mask_2d=mask_2d,
frame_calib_p2=frame_calib.p2)
if sin_type == 'lowres':
point_cloud_sin = point_cloud_lowres
fname_out = fname + '_{}_sin_{}_{}'.format(sin_input_name, sin_type,
sin_level)
if sin_input_name == 'image':
cv2.imwrite(os.path.join(out_dir, fname + '_image_org.png'),
cv_bgr_image)
cv2.imwrite(os.path.join(out_dir, fname_out + '.png'),
image_input_sin[..., ::-1])
elif sin_input_name == 'lidar':
# Clip distance with min/max values (for visualization)
pointsDist = point_cloud[2, :]
# for i_pt,pdist in enumerate(pointsDist):
# pointsDist[i_pt] = D_MAX if pdist>D_MAX \
# else pdist if pdist>D_MIN \
# else D_MIN
image_w_pts = points_on_img(point_in_im,
pointsDist,
rgb_image,
on_img=on_img)
point_in_im2 = calib_utils.project_to_image(point_cloud_sin,
p=frame_calib.p2).T
# Clip distance with min/max values (for visualization)
pointsDist2 = point_cloud_sin[2, :]
# for i_pt,pdist in enumerate(pointsDist2):
# pointsDist2[i_pt] = D_MAX if pdist>D_MAX \
# else pdist if pdist>D_MIN \
# else D_MIN
image_filter2 = (point_in_im2[:, 0] > 0) & \
(point_in_im2[:, 0] < im_size[0]) & \
(point_in_im2[:, 1] > 0) & \
(point_in_im2[:, 1] < im_size[1])
point_in_im2 = point_in_im2[image_filter2, :]
pointsDist2 = pointsDist2[image_filter2]
image_w_pts2 = points_on_img(point_in_im2,
pointsDist2,
rgb_image,
on_img=on_img)
cv2.imwrite(os.path.join(out_dir, fname + '_lidar_org.png'),
image_w_pts[..., ::-1])
if on_img:
cv2.imwrite(os.path.join(out_dir, fname_out + '.png'),
image_w_pts2[..., ::-1])
else:
cv2.imwrite(os.path.join(out_dir, fname_out + '_black.png'),
image_w_pts2[..., ::-1])
else:
raise ValueError("Invalid sin_input_name {}".format(sin_input_name))
