(self, frame)
| 353 | return updownsample_points |
| 354 | |
| 355 | def create_voxels(self, frame): |
| 356 | points_raw = frame.get_points().cuda() |
| 357 | if self.use_gt: |
| 358 | pose = frame.get_ref_pose().cuda() |
| 359 | else: |
| 360 | pose = frame.get_ref_pose().cuda() @ frame.get_d_pose().cuda() |
| 361 | points = points_raw @ pose[:3, :3].transpose(-1, -2) + pose[:3, 3] # change to world frame (Rx)^T = x^T R^T |
| 362 | |
| 363 | voxels = torch.div(points, self.voxel_size, rounding_mode='floor') # Divides each element |
| 364 | |
| 365 | inflate_margin_ratio = self.inflate_margin_ratio |
| 366 | |
| 367 | voxels_raw, inverse_indices, counts = torch.unique(voxels, dim=0, return_inverse=True, return_counts=True) |
| 368 | |
| 369 | voxels_vaild = voxels_raw[counts > 10] |
| 370 | self.voxels_vaild = voxels_vaild |
| 371 | offsets = torch.LongTensor([[-1, 0, 0], [1, 0, 0], [0, -1, 0], [0, 1, 0], [0, 0, 1], [0, 0, -1]]).to( |
| 372 | voxels.device) |
| 373 | |
| 374 | updownsampling_points = self.updownsampling_voxel(points, inverse_indices, counts) |
| 375 | for offset in offsets: |
| 376 | offset_axis = offset.nonzero().item() |
| 377 | if offset[offset_axis] > 0: |
| 378 | margin_mask = updownsampling_points[:, offset_axis] % self.voxel_size > ( |
| 379 | 1 - inflate_margin_ratio) * self.voxel_size |
| 380 | else: |
| 381 | margin_mask = updownsampling_points[:, |
| 382 | offset_axis] % self.voxel_size < inflate_margin_ratio * self.voxel_size |
| 383 | margin_vox = voxels_raw[margin_mask * (counts > 10)] |
| 384 | voxels_vaild = torch.cat((voxels_vaild, torch.clip(margin_vox + offset, min=0)), dim=0) |
| 385 | |
| 386 | voxels_unique = torch.unique(voxels_vaild, dim=0) |
| 387 | self.seen_voxel = voxels_unique |
| 388 | self.current_seen_voxel = voxels_unique.shape[0] |
| 389 | voxels_svo, children_svo, vertexes_svo, svo_mask, svo_idx = self.svo.insert(voxels_unique.cpu().int()) |
| 390 | svo_mask = svo_mask[:, 0].bool() |
| 391 | voxels_svo = voxels_svo[svo_mask] |
| 392 | children_svo = children_svo[svo_mask] |
| 393 | vertexes_svo = vertexes_svo[svo_mask] |
| 394 | |
| 395 | self.octant_idx = svo_mask.nonzero().cuda() |
| 396 | self.svo_idx = svo_idx |
| 397 | self.update_grid(voxels_svo, children_svo, vertexes_svo, svo_idx) |
| 398 | |
| 399 | @torch.enable_grad() |
| 400 | def update_grid(self, voxels, children, vertexes, svo_idx): |
no test coverage detected