(self, frame_id, tracked_frame, update_pose)
| 137 | print("idx: ", self.idx) |
| 138 | |
| 139 | def mapping_step(self, frame_id, tracked_frame, update_pose): |
| 140 | ###################### |
| 141 | self.idx = tracked_frame.stamp |
| 142 | self.create_voxels(tracked_frame) |
| 143 | |
| 144 | self.map_states, self.voxel_initialized, self.vertex_initialized = initemb_sdf(tracked_frame, |
| 145 | self.map_states, |
| 146 | self.sdf_truncation, |
| 147 | voxel_size=self.voxel_size, |
| 148 | voxel_initialized=self.voxel_initialized, |
| 149 | octant_idx=self.octant_idx, |
| 150 | vertex_initialized=self.vertex_initialized, |
| 151 | use_gt=self.use_gt) |
| 152 | self.sdf_priors = self.map_states["sdf_priors"] |
| 153 | if self.idx == 0: |
| 154 | self.insert_kf(tracked_frame) |
| 155 | self.do_mapping(tracked_frame=tracked_frame, update_pose=update_pose) |
| 156 | # Fixed 50 frames to insert pictures(naive) |
| 157 | if (tracked_frame.stamp - self.current_kf.stamp) > 50 and self.insert_method == "naive": |
| 158 | self.insert_kf(tracked_frame) |
| 159 | # The keyframe strategy we designed |
| 160 | if self.insert_method == "intersection": |
| 161 | insert_bool = self.voxel_field_insert_kf(self.insert_ratio) |
| 162 | if insert_bool \ |
| 163 | or (tracked_frame.stamp - self.current_kf.stamp) > 100: |
| 164 | self.insert_kf(tracked_frame) |
| 165 | |
| 166 | self.tracked_pose = tracked_frame.get_ref_pose().detach() @ tracked_frame.get_d_pose().detach() |
| 167 | ref_pose = self.current_kf.get_ref_pose().detach() @ self.current_kf.get_d_pose().detach() |
| 168 | rel_pose = torch.linalg.inv(ref_pose) @ self.tracked_pose |
| 169 | self.frame_poses += [(len(self.kf_graph) - 1, rel_pose.cpu())] |
| 170 | |
| 171 | if self.mesh_freq > 0 and (tracked_frame.stamp + 1) % self.mesh_freq == 0: |
| 172 | self.logger.log_mesh(self.extract_mesh( |
| 173 | res=self.mesh_res, clean_mesh=False, map_states=self.map_states), |
| 174 | name=f"mesh_{tracked_frame.stamp:06d}.ply") |
| 175 | |
| 176 | if self.save_data_freq > 0 and (tracked_frame.stamp + 1) % self.save_data_freq == 0: |
| 177 | self.save_debug_data(tracked_frame) |
| 178 | |
| 179 | if self.render_freq > 0 and (frame_id + 1) % self.render_freq == 0: |
| 180 | self.render_debug_images(tracked_frame) |
| 181 | |
| 182 | if self.save_ckpt_freq > 0 and (tracked_frame.stamp + 1) % self.save_ckpt_freq == 0: |
| 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 |
no test coverage detected