(self,opt)
| 57 | |
| 58 | class Cloud2ImgProjection: |
| 59 | def __init__(self,opt): |
| 60 | self.opt=opt |
| 61 | self.path = self.opt.save_path |
| 62 | self.tf_listener = tf.TransformListener() |
| 63 | # Subscribe map once and keep it as a torch tensor format in GPU memory |
| 64 | cloud_msg = rospy.wait_for_message("/rtabmap/cloud_map",PointCloud2,timeout=3.0) |
| 65 | raw_cloud_array = ros_numpy.point_cloud2.pointcloud2_to_array(cloud_msg,squeeze=False) |
| 66 | splited_cloud_array = ros_numpy.point_cloud2.split_rgb_field(raw_cloud_array) |
| 67 | self.pointcloud_dtype = splited_cloud_array.dtype |
| 68 | nd_cloud_array = np.concatenate([splited_cloud_array[idx] for idx in ['x','y','z','r','g','b']],axis=0) # (6,num_points) |
| 69 | # new_arr = np.core.records.fromarrays(nd_cloud_array,names=self.pointcloud_dtype.names) |
| 70 | # new_arr = np.array(new_arr.astype(dtype=self.pointcloud_dtype).tolist(),dtype=self.pointcloud_dtype) |
| 71 | num_points = nd_cloud_array.shape[1] |
| 72 | raw_cloud_xyz, raw_cloud_rgb = torch.from_numpy(nd_cloud_array[:3]).to(opt.device),torch.from_numpy(nd_cloud_array[3:]).to(opt.device) |
| 73 | raw_cloud_xyz1 = torch.cat([raw_cloud_xyz,torch.ones(1,num_points).to(opt.device)],dim=0) # (4,num_points), pad 1 for transform |
| 74 | |
| 75 | self.raw_cloud_xyz1 = raw_cloud_xyz1 |
| 76 | self.raw_cloud_rgb = raw_cloud_rgb |
| 77 | self.num_points = num_points |
| 78 | self.depth_threshold = 15.0 |
| 79 | # Subscribe camera_info(intrinsics) once and keep it |
| 80 | cam_info_msg = rospy.wait_for_message("/camera/rgb/camera_info",CameraInfo, timeout=3.0) |
| 81 | |
| 82 | self.camera_model = PinholeCameraModel() |
| 83 | self.camera_model.fromCameraInfo(cam_info_msg) |
| 84 | intr_matrix = self.camera_model.intrinsicMatrix() # (3,3) |
| 85 | intr_matrix = torch.from_numpy(intr_matrix)[None,:].to(opt.device) |
| 86 | self.intr_matrix = intr_matrix |
| 87 | |
| 88 | # Prepare empty tensor for img re-synthesis |
| 89 | if opt.proj_method == 'resize': |
| 90 | self.img_projected = torch.zeros(self.camera_model.height//opt.resize_ratio, |
| 91 | self.camera_model.width//opt.resize_ratio, 3).long().to(opt.device) |
| 92 | else: |
| 93 | self.img_projected = torch.zeros(self.camera_model.height, self.camera_model.width, 3).long().to(opt.device) |
| 94 | |
| 95 | # Prepare pose (trajectory) for frame matching |
| 96 | self.mapping_path = self.path.replace('Localization','Mapping') |
| 97 | self.poses = load_trajectory(os.path.join(self.mapping_path,'raw','poses_odom.g2o')) |
| 98 | |
| 99 | # Prepare publishers |
| 100 | self.pub_imgfied_cloud = rospy.Publisher("/imgfied_cloud", Image, queue_size=1) |
| 101 | self.pub_current_rgb = rospy.Publisher("/current_rgb", Image, queue_size=1) |
| 102 | self.pub_current_depth = rospy.Publisher("/current_depth", Image, queue_size=1) |
| 103 | self.pub_current_sem_seg = rospy.Publisher("/current_sem_seg", Image, queue_size=1) |
| 104 | self.pub_current_change_seg = rospy.Publisher("/current_change_seg", Image, queue_size=1) |
| 105 | self.pub_marked_cloud = rospy.Publisher("/marked_cloud", PointCloud2, queue_size=1) |
| 106 | self.pub_cloud_in_fov = rospy.Publisher("/cloud_in_fov", PointCloud2, queue_size=1) |
| 107 | |
| 108 | # To measure FPS |
| 109 | self.prevTime=time.time() |
| 110 | self.cnt = -1 |
| 111 | |
| 112 | self.seg_helper = SegHelper(opt,idx2color_path=opt.idx2color_path) |
| 113 | self.save_interval = self.opt.save_interval |
| 114 | self.filter_ratio = 0.8 |
| 115 | print("data will be saved in:{0}".format(self.path)) |
| 116 | if not os.path.exists(self.path): |
nothing calls this directly
no test coverage detected