while True:
(success, I) = video_reader.getNextFrame()
fnum = video_reader.framenum * 2
#t = utc_from_gps_log(gps_data_mark2[fnum, :])
t = gps_times_mark2[fnum]
data, t_data = lidar_loader.loadLDRWindow(t-50000, 0.1)
print data.shape
# Transform data into IMU frame at time t
pts = data[:, 0:3].transpose()
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
pts = np.dot(T_from_l_to_i, pts)
#fnum_mark1 = bisect.bisect(gps_times_mark1, t)
# Shift points according to timestamps instead of using transform of full sweep
#transform_points_by_times(pts, t_data, imu_transforms_mark1, gps_times_mark1)
transform_points_by_times(pts, t_data, imu_transforms_mark2, gps_times_mark2)
(pix, mask) = lidarPtsToPixels(pts, imu_transforms_mark2[fnum, :, :], 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,:,:]
I[pix[1,mask], pix[0,mask]+p, :] = heat_colors[0,:,:]
cv2.imshow('vid', cv2.pyrDown(I))
cv2.waitKey(5)
def execute(self, obj, event):
global fnum
images = [ ]
for c in cameras:
for _ in range(5):
(success, I) = c.video_reader.getNextFrame()
images.append(I.copy())
fnum = cameras[0].video_reader.framenum * 2
if fnum < START_LIDAR:
return
t = gps_times_mark2[fnum]
data, t_data = lidar_loader.loadLDRWindow(t-50000, 0.1)
pts = data[:, 0:3].transpose()
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
pts = np.dot(T_from_l_to_i, pts)
# Shift points according to timestamps instead of using transform of full sweep
transform_points_by_times(pts, t_data, imu_transforms_mark2, gps_times_mark2)
pts_wrt_imu_0 = pts.copy()
pts_wrt_imu_t = np.dot(np.linalg.inv(imu_transforms_mark2[fnum,:,:]), pts)
# transform points from imu_t to lidar_t
pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t)
color_data = np.zeros((data.shape[0], 3))
for idx in range(len(cameras)):
I = images[idx]
(pix, mask) = lidarPtsToPixels(pts_wrt_lidar_t, cameras[idx].cam)
intensity = data[mask, 3]
heat_colors = heatColorMapFast(intensity, 0, 100)
I_color_data = np.zeros((np.count_nonzero(mask), 3))
I_color_data[:,0] = I[pix[1,mask], pix[0,mask], 2]
I_color_data[:,1] = I[pix[1,mask], pix[0,mask], 1]
I_color_data[:,2] = I[pix[1,mask], pix[0,mask], 0]
color_data[mask, :] = I_color_data
for p in range(4):
I[pix[1,mask]+p, pix[0,mask], :] = heat_colors[0,:,:]
I[pix[1,mask], pix[0,mask]+p, :] = heat_colors[0,:,:]
cv2.imshow("video"+str(idx), cv2.pyrDown(I))
#pts = pts_wrt_lidar_t.transpose()
#print pts
pts = pts_wrt_imu_0.transpose()
#self.pointCloud = VtkPointCloud(pts[:, 0:3], data[:, 3])
#actor = self.pointCloud.get_vtk_cloud(zMin=0, zMax=255)
self.pointCloud = VtkPointCloud(pts_wrt_lidar_t.transpose()[:,0:3], color_data)
actor = self.pointCloud.get_vtk_color_cloud()
#self.pointCloud.append(VtkPointCloud(pts[:,0:3], color_data))
#actor = self.pointCloud[-1].get_vtk_color_cloud()
iren = obj
if fnum > START_LIDAR:
iren.GetRenderWindow().GetRenderers().GetFirstRenderer().RemoveActor(self.actor)
self.actor = actor
iren.GetRenderWindow().GetRenderers().GetFirstRenderer().AddActor(self.actor)
if fnum == START_LIDAR:
iren.GetRenderWindow().GetRenderers().GetFirstRenderer().ResetCamera()
iren.GetRenderWindow().Render()
def execute(self, obj, event):
global fnum
images = []
for c in cameras:
for _ in range(5):
(success, I) = c.video_reader.getNextFrame()
images.append(I.copy())
fnum = cameras[0].video_reader.framenum * 2
if fnum < START_LIDAR:
return
t = gps_times_mark2[fnum]
data, t_data = lidar_loader.loadLDRWindow(t - 50000, 0.1)
pts = data[:, 0:3].transpose()
pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
pts = np.dot(T_from_l_to_i, pts)
# Shift points according to timestamps instead of using transform of full sweep
transform_points_by_times(pts, t_data, imu_transforms_mark2,
gps_times_mark2)
pts_wrt_imu_0 = pts.copy()
pts_wrt_imu_t = np.dot(np.linalg.inv(imu_transforms_mark2[fnum, :, :]),
pts)
# transform points from imu_t to lidar_t
pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t)
color_data = np.zeros((data.shape[0], 3))
for idx in range(len(cameras)):
I = images[idx]
(pix, mask) = lidarPtsToPixels(pts_wrt_lidar_t, cameras[idx].cam)
intensity = data[mask, 3]
heat_colors = heatColorMapFast(intensity, 0, 100)
I_color_data = np.zeros((np.count_nonzero(mask), 3))
I_color_data[:, 0] = I[pix[1, mask], pix[0, mask], 2]
I_color_data[:, 1] = I[pix[1, mask], pix[0, mask], 1]
I_color_data[:, 2] = I[pix[1, mask], pix[0, mask], 0]
color_data[mask, :] = I_color_data
for p in range(4):
I[pix[1, mask] + p, pix[0, mask], :] = heat_colors[0, :, :]
I[pix[1, mask], pix[0, mask] + p, :] = heat_colors[0, :, :]
cv2.imshow("video" + str(idx), cv2.pyrDown(I))
#pts = pts_wrt_lidar_t.transpose()
#print pts
pts = pts_wrt_imu_0.transpose()
#self.pointCloud = VtkPointCloud(pts[:, 0:3], data[:, 3])
#actor = self.pointCloud.get_vtk_cloud(zMin=0, zMax=255)
self.pointCloud = VtkPointCloud(pts_wrt_lidar_t.transpose()[:, 0:3],
color_data)
actor = self.pointCloud.get_vtk_color_cloud()
#self.pointCloud.append(VtkPointCloud(pts[:,0:3], color_data))
#actor = self.pointCloud[-1].get_vtk_color_cloud()
iren = obj
if fnum > START_LIDAR:
iren.GetRenderWindow().GetRenderers().GetFirstRenderer(
).RemoveActor(self.actor)
self.actor = actor
iren.GetRenderWindow().GetRenderers().GetFirstRenderer().AddActor(
self.actor)
if fnum == START_LIDAR:
iren.GetRenderWindow().GetRenderers().GetFirstRenderer(
).ResetCamera()
iren.GetRenderWindow().Render()
