def getNextData(reader, LDRFrameMap):
for idx in range(10):
(success, img) = reader.getNextFrame()
#img = cv2.flip(img,-1)
if not success:
reader.setFrame(3)
ldr_frame = loadLDR(LDRFrameMap[reader.framenum])
return (success, img, ldr_frame)
def getNextData(reader, LDRFrameMap):
for idx in range(10):
(success, img) = reader.getNextFrame()
#img = cv2.flip(img,-1)
if not success:
reader.setFrame(3)
ldr_frame = loadLDR(LDRFrameMap[reader.framenum])
return (success, img, ldr_frame)
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)
parser.add_argument('h5_file')
parser.add_argument('--no_transform', action='store_true')
args = parser.parse_args()
if not args.no_transform:
gps_reader = GPSReader(args.gps_file)
gps_data = gps_reader.getNumericData()
#imu_transforms = IMUTransforms(gps_data)
imu_transforms = np.load(OPT_POS_FILE)['data']
params = LoadParameters(PARAMS_TO_LOAD)
# FIXME Assuming that ldr file named after frame num
fnum = int(os.path.splitext(os.path.basename(args.ldr_file))[0])
data = loadLDR(args.ldr_file)
if data.shape[0] == 0:
print '%d data empty' % fnum
raise
# Filter
dist = np.sqrt(np.sum(data[:, 0:3]**2, axis=1))
if LANE_FILTER:
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)
else:
data_filter_mask = (dist > 3) & \
parser.add_argument('h5_file')
parser.add_argument('--no_transform', action='store_true')
args = parser.parse_args()
if not args.no_transform:
gps_reader = GPSReader(args.gps_file)
gps_data = gps_reader.getNumericData()
#imu_transforms = IMUTransforms(gps_data)
imu_transforms = np.load(OPT_POS_FILE)['data']
params = LoadParameters(PARAMS_TO_LOAD)
# FIXME Assuming that ldr file named after frame num
fnum = int(os.path.splitext(os.path.basename(args.ldr_file))[0])
data = loadLDR(args.ldr_file)
if data.shape[0] == 0:
print '%d data empty' % fnum
raise
# Filter
dist = np.sqrt(np.sum(data[:, 0:3] ** 2, axis=1))
if LANE_FILTER:
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)
else:
data_filter_mask = (dist > 3) & \
ldr_interp[k::ni, 0:4] = ((ni - k) * ldr0[inds0, 0:4] +
(k + 1.0) * ldr1[inds1, 0:4]) / (ni + 1.0)
# Average the time as well
ldr_interp[k::ni, 5] = ((ni + - k) * ldr0[inds0, 5] +
(k + 1.0) * ldr1[inds1, 5]) / (ni + 1.0)
# Concatenate the laser numbers
ldr_interp[k::ni, 4] = ldr0[inds0, 4] * 100 + ldr1[inds1, 4]
return ldr_interp
if __name__ == '__main__':
infile = sys.argv[1]
outfile = sys.argv[2]
ni = int(sys.argv[3])
ldr_data = loadLDR(infile)
laser_nums = ldr_data[:, 4]
# Laser num goes from 0 to 31
print np.min(laser_nums), np.max(laser_nums)
laser_data = list()
laser_nbrs = list()
for laser_num in range(32):
ld = ldr_data[laser_nums == laser_num, :]
nbrs = NearestNeighbors(n_neighbors=1, algorithm='ball_tree').fit(ld[:, 0:3])
laser_data.append(ld)
laser_nbrs.append(nbrs)
interp_data = list()
# Refer to HDL-32E manual for laser ordering
