def integrateClouds(ldr_map, imu_transforms, offset, num_steps, step,
calibrationParameters):
for t in range(num_steps):
fnum = offset + t * step
print fnum
data = loadLDR(ldr_map[fnum])
# filter out the roof rack
dist = np.sqrt(np.sum(data[:, 0:3]**2, axis=1))
# See LidarIntegrator.py to see how this is filtering
data_filter_mask = (dist > 3) &\
(data[:, 3] > 40) &\
(np.abs(data[:, 1]) < 2.2) &\
(np.abs(data[:, 1]) > 1.2) &\
(data[:, 2] < -1.8) &\
(data[:, 2] > -2.5)
data = data[data_filter_mask, :]
# transform data into IMU frame at time t
pts = data[:, 0:3].transpose()
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
T_from_l_to_i = calibrationParameters['lidar']['T_from_l_to_i']
pts = np.dot(T_from_l_to_i, pts)
# transform data into imu_0 frame
# Microseconds till end of the sweep
times = data[:, 5]
transform_points_in_sweep(pts, times, fnum, imu_transforms)
pts = pts.transpose()
# for exporting purposes
pts_copy = np.array(pts[:, 0:3])
pts_copy = np.column_stack((pts_copy, np.array(data[:, 3])))
pts_copy = np.column_stack((pts_copy, fnum * np.ones(
(pts.shape[0], 1))))
all_data.append(pts_copy)
def integrateClouds(ldr_map, imu_transforms, offset, num_steps, step, calibrationParameters):
for t in range(num_steps):
fnum = offset+t*step
print fnum
data = loadLDR(ldr_map[fnum])
# filter out the roof rack
dist = np.sqrt(np.sum(data[:, 0:3] ** 2, axis=1))
# See LidarIntegrator.py to see how this is filtering
data_filter_mask = (dist > 3) &\
(data[:, 3] > 40) &\
(np.abs(data[:, 1]) < 2.2) &\
(np.abs(data[:, 1]) > 1.2) &\
(data[:, 2] < -1.8) &\
(data[:, 2] > -2.5)
data = data[data_filter_mask, :]
# transform data into IMU frame at time t
pts = data[:, 0:3].transpose()
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
T_from_l_to_i = calibrationParameters['lidar']['T_from_l_to_i']
pts = np.dot(T_from_l_to_i, pts)
# transform data into imu_0 frame
# Microseconds till end of the sweep
times = data[:, 5]
transform_points_in_sweep(pts, times, fnum, imu_transforms)
pts = pts.transpose()
# for exporting purposes
pts_copy = np.array(pts[:, 0:3])
pts_copy = np.column_stack((pts_copy, np.array(data[:, 3])))
pts_copy = np.column_stack((pts_copy, fnum*np.ones((pts.shape[0], 1))))
all_data.append(pts_copy)
# FIXME, hack, just include a single point
data = data[0:1, :]
#raise
else:
data = filtered_data
pts = data[:, 0:3].transpose()
if not args.no_transform:
# Transform data into IMU frame at time t
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
T_from_l_to_i = params['lidar']['T_from_l_to_i']
pts = np.dot(T_from_l_to_i, pts)
# Microseconds till end of the sweep
# TODO Switch everything to use transform_points_by_times
times = data[:, 5]
transform_points_in_sweep(pts, times, fnum, imu_transforms)
pts = pts.transpose()
# for exporting purposes
pts_copy = np.array(pts[:, 0:3])
pts_copy = np.column_stack((pts_copy, np.array(data[:, 3])))
pts_copy = np.column_stack((pts_copy, fnum * np.ones((pts.shape[0], 1))))
h5f = h5py.File(args.h5_file, 'w')
dset = h5f.create_dataset('points', pts_copy.shape, dtype='f')
dset[...] = pts_copy
h5f.close()
# FIXME, hack, just include a single point
data = data[0:1, :]
#raise
else:
data = filtered_data
pts = data[:, 0:3].transpose()
if not args.no_transform:
# Transform data into IMU frame at time t
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
T_from_l_to_i = params['lidar']['T_from_l_to_i']
pts = np.dot(T_from_l_to_i, pts)
# Microseconds till end of the sweep
# TODO Switch everything to use transform_points_by_times
times = data[:, 5]
transform_points_in_sweep(pts, times, fnum, imu_transforms)
pts = pts.transpose()
# for exporting purposes
pts_copy = np.array(pts[:, 0:3])
pts_copy = np.column_stack((pts_copy, np.array(data[:, 3])))
pts_copy = np.column_stack((pts_copy, fnum*np.ones((pts.shape[0], 1))))
h5f = h5py.File(args.h5_file, 'w')
dset = h5f.create_dataset('points', pts_copy.shape, dtype='f')
dset[...] = pts_copy
h5f.close()
