save per-frame voxel, mesh and pose
(self, tracked_frame)
| 472 | |
| 473 | @torch.no_grad() |
| 474 | def save_debug_data(self, tracked_frame): |
| 475 | """ |
| 476 | save per-frame voxel, mesh and pose |
| 477 | """ |
| 478 | if self.use_gt: |
| 479 | pose = tracked_frame.get_ref_pose().detach().cpu().numpy() |
| 480 | else: |
| 481 | pose = tracked_frame.get_ref_pose().detach().cpu().numpy() @ tracked_frame.get_d_pose().detach().cpu().numpy() |
| 482 | pose[:3, 3] -= self.offset |
| 483 | frame_poses = self.get_updated_poses() |
| 484 | mesh = self.extract_mesh(res=8, clean_mesh=True) |
| 485 | voxels = self.extract_voxels(map_states=self.map_states).detach().cpu().numpy() |
| 486 | if self.use_gt: |
| 487 | kf_poses = [p.get_ref_pose().detach().cpu().numpy() |
| 488 | for p in self.kf_graph] |
| 489 | else: |
| 490 | kf_poses = [p.get_ref_pose().detach().cpu().numpy() @ p.get_d_pose().detach().cpu().numpy() |
| 491 | for p in self.kf_graph] |
| 492 | |
| 493 | for f in frame_poses: |
| 494 | f[:3, 3] -= self.offset |
| 495 | for kf in kf_poses: |
| 496 | kf[:3, 3] -= self.offset |
| 497 | |
| 498 | verts = np.asarray(mesh.vertices) |
| 499 | faces = np.asarray(mesh.triangles) |
| 500 | color = np.asarray(mesh.vertex_colors) |
| 501 | |
| 502 | self.logger.log_debug_data({ |
| 503 | "pose": pose, |
| 504 | "updated_poses": frame_poses, |
| 505 | "mesh": {"verts": verts, "faces": faces, "color": color}, |
| 506 | "voxels": voxels, |
| 507 | "voxel_size": self.voxel_size, |
| 508 | "keyframes": kf_poses, |
| 509 | "is_kf": (tracked_frame == self.current_kf) |
| 510 | }, tracked_frame.stamp) |
| 511 | |
| 512 | """" |
| 513 | Used to render a complete picture |
no test coverage detected