| 37 | |
| 38 | |
| 39 | class Model: |
| 40 | def __init__(self) -> None: |
| 41 | self.reconstruction: pycolmap.Reconstruction |
| 42 | self.visualizer: open3d.visualization.Visualizer |
| 43 | |
| 44 | def read_model(self, path: str) -> None: |
| 45 | self.reconstruction = pycolmap.Reconstruction(path) |
| 46 | |
| 47 | def add_points( |
| 48 | self, min_track_len: int = 3, remove_statistical_outlier: bool = True |
| 49 | ) -> None: |
| 50 | pcd = open3d.geometry.PointCloud() |
| 51 | |
| 52 | xyz = [] |
| 53 | rgb = [] |
| 54 | for point in self.reconstruction.points3D.values(): |
| 55 | if point.track.length() < min_track_len: |
| 56 | continue |
| 57 | xyz.append(point.xyz) |
| 58 | rgb.append(point.color / 255) |
| 59 | |
| 60 | pcd.points = open3d.utility.Vector3dVector(xyz) |
| 61 | pcd.colors = open3d.utility.Vector3dVector(rgb) |
| 62 | |
| 63 | # remove obvious outliers |
| 64 | if remove_statistical_outlier: |
| 65 | [pcd, _] = pcd.remove_statistical_outlier( |
| 66 | nb_neighbors=20, std_ratio=2.0 |
| 67 | ) |
| 68 | |
| 69 | # open3d.visualization.draw_geometries([pcd]) |
| 70 | self.visualizer.add_geometry(pcd) |
| 71 | self.visualizer.poll_events() |
| 72 | self.visualizer.update_renderer() |
| 73 | |
| 74 | def add_cameras(self, scale: float = 1) -> None: |
| 75 | frustums = [] |
| 76 | for img in self.reconstruction.images.values(): |
| 77 | # extrinsics |
| 78 | world_from_cam = img.cam_from_world().inverse() |
| 79 | R = world_from_cam.rotation.matrix() |
| 80 | t = world_from_cam.translation |
| 81 | |
| 82 | # intrinsics |
| 83 | cam = img.camera |
| 84 | if cam.model in ( |
| 85 | pycolmap.CameraModelId.SIMPLE_PINHOLE, |
| 86 | pycolmap.CameraModelId.SIMPLE_RADIAL, |
| 87 | pycolmap.CameraModelId.RADIAL, |
| 88 | ): |
| 89 | fx = fy = cam.params[0] |
| 90 | cx = cam.params[1] |
| 91 | cy = cam.params[2] |
| 92 | elif cam.model in ( |
| 93 | pycolmap.CameraModelId.PINHOLE, |
| 94 | pycolmap.CameraModelId.OPENCV, |
| 95 | pycolmap.CameraModelId.OPENCV_FISHEYE, |
| 96 | pycolmap.CameraModelId.FULL_OPENCV, |