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()