def build_pointcloud(lidar_dir,
poses_file,
extrinsics_dir,
start_time,
end_time,
origin_time=-1):
"""Builds a pointcloud by combining multiple LIDAR scans with odometry information.
Args:
lidar_dir (str): Directory containing LIDAR scans.
poses_file (str): Path to a file containing pose information. Can be VO or INS data.
extrinsics_dir (str): Directory containing extrinsic calibrations.
start_time (int): UNIX timestamp of the start of the window over which to build the pointcloud.
end_time (int): UNIX timestamp of the end of the window over which to build the pointcloud.
origin_time (int): UNIX timestamp of origin frame. Pointcloud coordinates are relative to this frame.
Returns:
numpy.ndarray: 3xn array of (x, y, z) coordinates of pointcloud
numpy.array: array of n reflectance values or None if no reflectance values are recorded (LDMRS)
Raises:
ValueError: if specified window doesn't contain any laser scans.
IOError: if scan files are not found.
"""
if origin_time < 0:
origin_time = start_time
lidar = re.search(
'(lms_front|lms_rear|ldmrs|velodyne_left|velodyne_right)',
lidar_dir).group(0)
timestamps_path = os.path.join(lidar_dir, os.pardir, lidar + '.timestamps')
timestamps = []
with open(timestamps_path) as timestamps_file:
for line in timestamps_file:
timestamp = int(line.split(' ')[0])
if start_time <= timestamp <= end_time:
timestamps.append(timestamp)
if len(timestamps) == 0:
raise ValueError("No LIDAR data in the given time bracket.")
with open(os.path.join(extrinsics_dir, lidar + '.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = build_se3_transform(
[float(x) for x in extrinsics.split(' ')])
poses_type = re.search('(vo|ins|rtk)\.csv', poses_file).group(1)
if poses_type in ['ins', 'rtk']:
with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = np.linalg.solve(
build_se3_transform([float(x) for x in extrinsics.split(' ')]),
G_posesource_laser)
poses = interpolate_ins_poses(poses_file,
timestamps,
origin_time,
use_rtk=(poses_type == 'rtk'))
else:
# sensor is VO, which is located at the main vehicle frame
poses = interpolate_vo_poses(poses_file, timestamps, origin_time)
pointcloud = np.array([[0], [0], [0], [0]])
if lidar == 'ldmrs':
reflectance = None
else:
reflectance = np.empty((0))
for i in range(0, len(poses)):
scan_path = os.path.join(lidar_dir, str(timestamps[i]) + '.bin')
if "velodyne" not in lidar:
if not os.path.isfile(scan_path):
continue
scan_file = open(scan_path)
scan = np.fromfile(scan_file, np.double)
scan_file.close()
scan = scan.reshape((len(scan) // 3, 3)).transpose()
if lidar != 'ldmrs':
# LMS scans are tuples of (x, y, reflectance)
reflectance = np.concatenate(
(reflectance, np.ravel(scan[2, :])))
scan[2, :] = np.zeros((1, scan.shape[1]))
else:
if os.path.isfile(scan_path):
ptcld = load_velodyne_binary(scan_path)
else:
scan_path = os.path.join(lidar_dir,
str(timestamps[i]) + '.png')
if not os.path.isfile(scan_path):
continue
ranges, intensities, angles, approximate_timestamps = load_velodyne_raw(
scan_path)
ptcld = velodyne_raw_to_pointcloud(ranges, intensities, angles)
reflectance = np.concatenate((reflectance, ptcld[3]))
scan = ptcld[:3]
scan = np.dot(np.dot(poses[i], G_posesource_laser),
np.vstack([scan, np.ones((1, scan.shape[1]))]))
pointcloud = np.hstack([pointcloud, scan])
pointcloud = pointcloud[:, 1:]
if pointcloud.shape[1] == 0:
raise IOError(
"Could not find scan files for given time range in directory " +
lidar_dir)
return pointcloud, reflectance
first_idx = np.where(stereo_img_ts > gt_ts[0])[0][0] + 1
last_idx = np.where(stereo_img_ts > gt_ts[-1])[0]
if last_idx != []:
last_idx = last_idx[0] - 1
else:
last_idx = len(gt_ts)-1
stereo_img_idx = list(range(first_idx, last_idx))
stereo_img_ts = list(stereo_img_ts[stereo_img_idx])
gt_poses = interpolate_ins_poses(rtk_filename, stereo_img_ts, stereo_img_ts[0], use_rtk=True)
gt_poses = [se3_to_components(p) for p in gt_poses]
gt_poses = np.array(gt_poses)
vo_filename = '{}/{}'.format(seq_dir, 'vo/vo.csv')
vo_poses = interpolate_vo_poses(vo_filename, stereo_img_ts, stereo_img_ts[0])
vo_poses = [se3_to_components(p) for p in vo_poses]
vo_poses = np.array(vo_poses)
r_w_imu_w = gt_poses[:,0:3]
rpy_w_imu_w = gt_poses[:,3:6]
plt.plot(gt_poses[:,0], gt_poses[:,1])
plt.plot(vo_poses[:,0], -vo_poses[:,1])
plt.savefig('{}_gt_traj'.format(seq))
plt.figure()
plt.plot(rpy_w_imu_w[:,2])
plt.plot(vo_poses[:,5])
plt.savefig('{}_gt_rpy'.format(seq))
plt.figure()
with open(lmsTimestamps) as timestamps_file:
for line in timestamps_file:
timestamp = int(line.split(' ')[0])
Ltimestamps.append(timestamp)
#Get timestamps for camera interpolation, interpolate, and lidar point clouds
stamp = []
stamp.append(Ctimestamps[0])
camera_poses = []
pointc = []
subset = np.asarray(Ltimestamps[1000:1100])
for i in range(len(subset) - 1):
stamp.append(stamp[i] + subset[i + 1] - subset[i])
camera_poses.append(interpolate_vo_poses(vopath, [stamp[i + 1]], stamp[i]))
pointc.append(
np.asarray(
build_pointcloud(lmsFrontDir, vopath, extrinsics, subset[i],
subset[i])[0]).transpose())
#Get transformations between lidar scans, and the error+iterations
LidarPoses, c = icpLidar(subset)
'''
#Plot error against iterations
y = []
for i in range (len(c)):
y.append(i+1)
plt.scatter(y,c)
plt.title("Reduction in squared error deviation by Iterative closest point")
print(len(c))
def build_pointcloud(lidar_dir, poses_file, extrinsics_dir, start_time, end_time, origin_time=-1):
"""Builds a pointcloud by combining multiple LIDAR scans with odometry information.
Args:
lidar_dir (str): Directory containing LIDAR scans.
poses_file (str): Path to a file containing pose information. Can be VO or INS data.
extrinsics_dir (str): Directory containing extrinsic calibrations.
start_time (int): UNIX timestamp of the start of the window over which to build the pointcloud.
end_time (int): UNIX timestamp of the end of the window over which to build the pointcloud.
origin_time (int): UNIX timestamp of origin frame. Pointcloud coordinates are relative to this frame.
Returns:
numpy.ndarray: 3xn array of (x, y, z) coordinates of pointcloud
numpy.array: array of n reflectance values or None if no reflectance values are recorded (LDMRS)
Raises:
ValueError: if specified window doesn't contain any laser scans.
IOError: if scan files are not found.
"""
if origin_time < 0:
origin_time = start_time
lidar = re.search('(lms_front|lms_rear|ldmrs)', lidar_dir).group(0)
timestamps_path = os.path.join(lidar_dir, os.pardir, lidar + '.timestamps')
timestamps = []
with open(timestamps_path) as timestamps_file:
for line in timestamps_file:
timestamp = int(line.split(' ')[0])
if start_time <= timestamp <= end_time:
timestamps.append(timestamp)
if len(timestamps) == 0:
raise ValueError("No LIDAR data in the given time bracket.")
with open(os.path.join(extrinsics_dir, lidar + '.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = build_se3_transform([float(x) for x in extrinsics.split(' ')])
poses_type = re.search('(vo|ins)\.csv', poses_file).group(1)
if poses_type == 'ins':
with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = np.linalg.solve(build_se3_transform([float(x) for x in extrinsics.split(' ')]),
G_posesource_laser)
poses = interpolate_ins_poses(poses_file, timestamps, origin_time)
else:
# sensor is VO, which is located at the main vehicle frame
poses = interpolate_vo_poses(poses_file, timestamps, origin_time)
pointcloud = np.array([[0], [0], [0], [0]])
if lidar == 'ldmrs':
reflectance = None
else:
reflectance = np.empty((0))
for i in range(0, len(poses)):
scan_path = os.path.join(lidar_dir, str(timestamps[i]) + '.bin')
if not os.path.isfile(scan_path):
continue
scan_file = open(scan_path)
scan = np.fromfile(scan_file, np.double)
scan_file.close()
scan = scan.reshape((len(scan) // 3, 3)).transpose()
if lidar != 'ldmrs':
# LMS scans are tuples of (x, y, reflectance)
reflectance = np.concatenate((reflectance, np.ravel(scan[2, :])))
scan[2, :] = np.zeros((1, scan.shape[1]))
scan = np.dot(np.dot(poses[i], G_posesource_laser), np.vstack([scan, np.ones((1, scan.shape[1]))]))
pointcloud = np.hstack([pointcloud, scan])
pointcloud = pointcloud[:, 1:]
if pointcloud.shape[1] == 0:
raise IOError("Could not find scan files for given time range in directory " + lidar_dir)
return pointcloud, reflectance