print('Frame-to-model icp') T_world_to_cam = np.linalg.inv(T_cam_to_world) T_world_to_cam = icp(m.points[::down_factor], m.normals[::down_factor], vertex_map, normal_map, intrinsic, T_world_to_cam, debug_association=False) T_cam_to_world = np.linalg.inv(T_world_to_cam) print('Point-based fusion') m.fuse(vertex_map, normal_map, color_map, intrinsic, T_cam_to_world, time=i) # A shift is required as gt starts from 1 T_gt.append(gt_poses[i - 1]) T_est.append(T_cam_to_world) global_pcd = o3d_utility.make_point_cloud(m.points, colors=m.colors, normals=m.normals) o3d.visualization.draw_geometries([global_pcd.transform(o3d_utility.flip_transform)]) # Visualize the trajectories T_gt = np.stack(T_gt) T_est = np.stack(T_est) fig = plt.figure() ax = plt.axes(projection='3d') pos_gt = T_gt[:, :3, 3] pos_est = T_est[:, :3, 3] ax.plot3D(pos_gt[:, 0], pos_gt[:, 1], pos_gt[:, 2]) ax.plot3D(pos_est[:, 0], pos_est[:, 1], pos_est[:, 2]) plt.show()
if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument( 'path', help='path to the dataset folder containing rgb/ and depth/') args = parser.parse_args() # Load intrinsics and gt poses for evaluation intrinsic_struct = o3d.io.read_pinhole_camera_intrinsic('intrinsics.json') intrinsic = np.array(intrinsic_struct.intrinsic_matrix) indices, gt_poses = load_gt_poses( os.path.join(args.path, 'livingRoom2.gt.freiburg')) depth_scale = 5000.0 depth_path = os.path.join(args.path, 'depth') normal_path = os.path.join(args.path, 'normal') os.makedirs(normal_path, exist_ok=True) # Generate normal maps # WARNING: please start from index 1, as ground truth poses are provided starting from index 1. for i in indices: print('Preprocessing frame {:03d}'.format(i)) depth = np.asarray(o3d.io.read_image('{}/{}.png'.format( depth_path, i))) / depth_scale vertex_map = transforms.unproject(depth, intrinsic) pcd = o3d_utility.make_point_cloud(vertex_map.reshape((-1, 3))) pcd.estimate_normals() normal_map = np.asarray(pcd.normals).reshape(vertex_map.shape) np.save('{}/{}.npy'.format(normal_path, i), normal_map)