Manages per-frame point cloud data and provides filtered subsets.
| 128 | |
| 129 | |
| 130 | class PointCloudManager: |
| 131 | """Manages per-frame point cloud data and provides filtered subsets.""" |
| 132 | |
| 133 | def __init__(self): |
| 134 | # Per-frame data: list of (xyz [N,3], rgb [N,3], conf [N]) numpy arrays |
| 135 | self.frame_xyz: List[np.ndarray] = [] |
| 136 | self.frame_rgb: List[np.ndarray] = [] |
| 137 | self.frame_conf: List[Optional[np.ndarray]] = [] |
| 138 | self.num_frames = 0 |
| 139 | |
| 140 | def add_frame( |
| 141 | self, xyz: np.ndarray, rgb: np.ndarray, conf: Optional[np.ndarray] = None |
| 142 | ): |
| 143 | self.frame_xyz.append(xyz) |
| 144 | self.frame_rgb.append(rgb) |
| 145 | self.frame_conf.append(conf) |
| 146 | self.num_frames = len(self.frame_xyz) |
| 147 | |
| 148 | def get_points( |
| 149 | self, |
| 150 | frame_idx: int, |
| 151 | display_mode: str = "all", # "single", "cumulative", "all" |
| 152 | conf_threshold: float = 0.0, |
| 153 | ) -> Tuple[np.ndarray, np.ndarray]: |
| 154 | """Return (xyz, rgb) for the requested view, filtered by confidence.""" |
| 155 | if self.num_frames == 0: |
| 156 | return np.zeros((0, 3)), np.zeros((0, 3)) |
| 157 | |
| 158 | if display_mode == "single": |
| 159 | indices = [min(frame_idx, self.num_frames - 1)] |
| 160 | elif display_mode == "cumulative": |
| 161 | indices = list(range(min(frame_idx + 1, self.num_frames))) |
| 162 | else: # "all" |
| 163 | indices = list(range(self.num_frames)) |
| 164 | |
| 165 | xyz_parts, rgb_parts = [], [] |
| 166 | for i in indices: |
| 167 | x, r, c = self.frame_xyz[i], self.frame_rgb[i], self.frame_conf[i] |
| 168 | if conf_threshold > 0 and c is not None: |
| 169 | mask = c >= conf_threshold |
| 170 | x, r = x[mask], r[mask] |
| 171 | xyz_parts.append(x) |
| 172 | rgb_parts.append(r) |
| 173 | |
| 174 | if not xyz_parts: |
| 175 | return np.zeros((0, 3)), np.zeros((0, 3)) |
| 176 | return np.concatenate(xyz_parts), np.concatenate(rgb_parts) |
| 177 | |
| 178 | |
| 179 | # --------------------------------------------------------------------------- |