| 242 | print(f"Marching Cubes failed: {e}") |
| 243 | |
| 244 | def extract_mesh(tsdf_vol, min_bound, voxel_size, output_file): |
| 245 | if not tsdf_vol: |
| 246 | print("Error: TSDF volume is empty.") |
| 247 | return |
| 248 | |
| 249 | print("Converting sparse volume to dense grid...") |
| 250 | keys = np.array(list(tsdf_vol.keys())) |
| 251 | vals = np.array(list(tsdf_vol.values())) |
| 252 | |
| 253 | # Recover TSDF values: weighted_sdf / weight |
| 254 | tsdf_values = vals[:, 1] / vals[:, 0] |
| 255 | |
| 256 | # Determine grid bounds |
| 257 | min_idx = np.min(keys, axis=0) |
| 258 | max_idx = np.max(keys, axis=0) |
| 259 | dims = max_idx - min_idx + 1 |
| 260 | |
| 261 | print(f"Grid dimensions: {dims}") |
| 262 | |
| 263 | # Allocate dense grid (pad with 1 to ensure boundaries for marching cubes) |
| 264 | pad = 1 |
| 265 | grid_shape = tuple(dims + 2 * pad) |
| 266 | sdf_grid = np.ones(grid_shape, dtype=np.float32) # Initialize with +1 (outside) or truncation? |
| 267 | # Usually Initialize with truncation value (positive) |
| 268 | |
| 269 | # Fill grid |
| 270 | # Shift indices to 0-based with padding |
| 271 | shifted_keys = keys - min_idx + pad |
| 272 | |
| 273 | sdf_grid[shifted_keys[:, 0], shifted_keys[:, 1], shifted_keys[:, 2]] = tsdf_values |
| 274 | |
| 275 | print("Running Marching Cubes...") |
| 276 | try: |
| 277 | # Marching cubes |
| 278 | verts, faces, normals, values = marching_cubes(sdf_grid, level=0.0, spacing=(voxel_size, voxel_size, voxel_size)) |
| 279 | |
| 280 | # Transform vertices back to world space |
| 281 | # Grid origin matches min_idx - pad |
| 282 | grid_origin_idx = min_idx - pad |
| 283 | grid_origin_pos = min_bound + grid_origin_idx * voxel_size |
| 284 | |
| 285 | verts += grid_origin_pos |
| 286 | |
| 287 | print(f"Saving mesh to {output_file}...") |
| 288 | save_ply(output_file, verts, faces, normals) |
| 289 | |
| 290 | except ValueError as e: |
| 291 | print(f"Marching Cubes failed: {e}") |
| 292 | except RuntimeError as e: |
| 293 | print(f"Marching Cubes failed: {e}") |
| 294 | |
| 295 | def save_ply(path, vertices, faces, normals=None): |
| 296 | vertex_dtype = [('x', 'f4'), ('y', 'f4'), ('z', 'f4')] |