time.sleep(1) print int(sys.argv[3]) video_reader.setFrame(int(sys.argv[3])) (success, orig) = video_reader.getNextFrame() while True: I = orig.copy() fnum = video_reader.framenum print fnum T_from_l_to_i = np.eye(4) T_from_l_to_i[0:3, 0:3] = R T_from_i_to_l = np.linalg.inv(T_from_l_to_i) t = gps_times_mark2[fnum] builder.start_time = t builder.end_time = t + 3.0 * 1e6 builder.step_time = 0.5 builder.T_from_l_to_i = T_from_l_to_i builder.T_from_i_to_l = T_from_i_to_l builder.buildMap(filters=['no-trees']) data, data_times = builder.getData() fnum_mark1 = bisect.bisect(gps_times_mark1, t) - 1 (pix, mask) = lidarPtsToPixels(data, imu_transforms_mark1[fnum_mark1, :, :], T_from_i_to_l, cam) intensity = data[mask, 3] heat_colors = heatColorMapFast(intensity, 0, 100) for p in range(4):
time.sleep(1) print int(sys.argv[3]) video_reader.setFrame(int(sys.argv[3])) (success, orig) = video_reader.getNextFrame() while True: I = orig.copy() fnum = video_reader.framenum print fnum T_from_l_to_i = np.eye(4) T_from_l_to_i[0:3,0:3] = R T_from_i_to_l = np.linalg.inv(T_from_l_to_i) t = gps_times_mark2[fnum] builder.start_time = t builder.end_time = t + 3.0 * 1e6 builder.step_time = 0.5 builder.T_from_l_to_i = T_from_l_to_i builder.T_from_i_to_l = T_from_i_to_l builder.buildMap(filters=['no-trees']) data, data_times = builder.getData() fnum_mark1 = bisect.bisect(gps_times_mark1, t) - 1 (pix, mask) = lidarPtsToPixels(data, imu_transforms_mark1[fnum_mark1, :,:], T_from_i_to_l, cam) intensity = data[mask, 3] heat_colors = heatColorMapFast(intensity, 0, 100) for p in range(4): I[pix[1,mask]+p, pix[0,mask], :] = heat_colors[0,:,:]