(self, first_frame, update_pose)
| 183 | self.logger.log_ckpt(self, name=f"{tracked_frame.stamp:06d}.pth") |
| 184 | |
| 185 | def run(self, first_frame, update_pose): |
| 186 | self.idx = 0 |
| 187 | self.voxel_initialized = torch.zeros(self.num_vertexes).cuda().bool() |
| 188 | self.vertex_initialized = torch.zeros(self.num_vertexes).cuda().bool() |
| 189 | self.kf_unoptimized_voxels = None |
| 190 | self.kf_optimized_voxels = None |
| 191 | self.kf_all_voxels = None |
| 192 | if self.run_ros: |
| 193 | rospy.init_node('listener', anonymous=True) |
| 194 | # realsense |
| 195 | color_sub = message_filters.Subscriber(self.color_topic, Image) |
| 196 | depth_sub = message_filters.Subscriber(self.depth_topic, Image) |
| 197 | pose_sub = message_filters.Subscriber(self.pose_topic, Odometry) |
| 198 | |
| 199 | ts = message_filters.ApproximateTimeSynchronizer([color_sub, depth_sub, pose_sub], 2, 1 / 10, |
| 200 | allow_headerless=False) |
| 201 | print(" ========== MAPPING START ===========") |
| 202 | ts.registerCallback(self.callback) |
| 203 | rospy.spin() |
| 204 | else: |
| 205 | if self.mesher is not None: |
| 206 | self.mesher.rays_d = first_frame.get_rays() |
| 207 | self.create_voxels(first_frame) |
| 208 | self.map_states, self.voxel_initialized, self.vertex_initialized = initemb_sdf(first_frame, |
| 209 | self.map_states, |
| 210 | self.sdf_truncation, |
| 211 | voxel_size=self.voxel_size, |
| 212 | voxel_initialized=self.voxel_initialized, |
| 213 | octant_idx=self.octant_idx, |
| 214 | vertex_initialized=self.vertex_initialized, |
| 215 | use_gt=self.use_gt) |
| 216 | self.sdf_priors = self.map_states["sdf_priors"] |
| 217 | self.insert_kf(first_frame) |
| 218 | self.do_mapping(tracked_frame=first_frame, update_pose=update_pose) |
| 219 | |
| 220 | self.tracked_pose = first_frame.get_ref_pose().detach() @ first_frame.get_d_pose().detach() |
| 221 | ref_pose = self.current_kf.get_ref_pose().detach() @ self.current_kf.get_d_pose().detach() |
| 222 | rel_pose = torch.linalg.inv(ref_pose) @ self.tracked_pose |
| 223 | self.frame_poses += [(len(self.kf_graph) - 1, rel_pose.cpu())] |
| 224 | self.render_debug_images(first_frame) |
| 225 | |
| 226 | print("mapping started!") |
| 227 | |
| 228 | progress_bar = tqdm(range(self.start_frame, self.end_frame), position=0) |
| 229 | progress_bar.set_description("mapping frame") |
| 230 | for frame_id in progress_bar: |
| 231 | data_in = self.data_stream[frame_id] |
| 232 | if self.use_gt: |
| 233 | tracked_frame = RGBDFrame(*data_in[:-1], offset=self.offset, ref_pose=data_in[-1]) |
| 234 | else: |
| 235 | tracked_frame = RGBDFrame(*data_in[:-1], offset=self.offset, ref_pose=self.tracked_pose.clone()) |
| 236 | if update_pose is False: |
| 237 | tracked_frame.d_pose.requires_grad_(False) |
| 238 | if tracked_frame.ref_pose.isinf().any(): |
| 239 | continue |
| 240 | self.mapping_step(frame_id, tracked_frame, update_pose) |
| 241 | |
| 242 | print("******* mapping process died *******") |
no test coverage detected