Implemented basic pointcloud and camera pose visualization

commit-id:c4886df5

Author: integraledelebesgue

packages/child-lab-visualization/pyproject.toml

@@ -3,11 +3,19 @@ name = "child-lab-visualization"
 version = "0.1.0"
 description = "Add your description here"
 readme = "README.md"
-authors = [
-    { name = "Jan Smółka", email = "[email protected]" }
-]
+authors = [{ name = "Jan Smółka", email = "[email protected]" }]
 requires-python = ">=3.12.7"
-dependencies = []
+dependencies = [
+    "numpy>=1.26.4",
+    "torch>=2.6.0",
+    "viser>=0.2.23",
+    "transformation-buffer",
+    "video-io",
+]
+
+[tool.uv.sources]
+transformation-buffer = { workspace = true }
+video-io = { workspace = true }
 
 [build-system]
 requires = ["hatchling"]

packages/child-lab-visualization/src/child_lab_visualization/__init__.py

@@ -1 +1,3 @@
+from .server import show_pointcloud_and_camera_poses
 
+__all__ = ['show_pointcloud_and_camera_poses']

packages/child-lab-visualization/src/child_lab_visualization/server.py

@@ -0,0 +1,60 @@
+from pathlib import Path
+
+import numpy
+import torch
+from transformation_buffer.buffer import Buffer
+from video_io.calibration import Calibration
+from video_io.reader import Reader
+from viser import ViserServer
+
+
+def show_pointcloud_and_camera_poses(
+    server: ViserServer,
+    origin_name: str,
+    reader: Reader,
+    points_files: list[Path],
+    calibration: Calibration,
+    buffer: Buffer,
+) -> None:
+    height = reader.metadata.height
+    width = reader.metadata.width
+    fov = 2 * numpy.arctan2(height / 2, calibration.focal_length[0])
+    aspect = width / height
+
+    server.scene.add_camera_frustum(f'/frames/0/cameras/{origin_name}', fov, aspect)
+
+    for frame in buffer.frames_visible_from(origin_name):
+        transformation = buffer[(frame, origin_name)]
+        assert transformation is not None
+
+        if 'marker' in frame:
+            server.scene.add_frame(
+                f'/frames/0/ref_frames/{frame}',
+                axes_length=0.1,
+                axes_radius=0.0025,
+                wxyz=transformation.rotation_quaternion().numpy(),
+                # Dividing by 1000 is a workaround for working with old intrinsic parameters.
+                position=transformation.translation().div(1000.0).numpy(),
+            )
+
+        else:
+            server.scene.add_camera_frustum(
+                f'/frames/0/cameras/{frame}',
+                fov,
+                aspect,
+                wxyz=transformation.rotation_quaternion().numpy(),
+                position=transformation.translation().div(1000.0).numpy(),
+            )
+
+    points_first_batch: torch.Tensor = torch.load(points_files[0])
+    frames_first_batch = reader.read_batch(points_first_batch.shape[0])
+    assert frames_first_batch is not None
+
+    server.scene.add_point_cloud(
+        '/frames/0/point_cloud',
+        points=points_first_batch[0].permute((1, 2, 0)).flatten(0, -2).numpy(),
+        colors=frames_first_batch[0].permute((1, 2, 0)).flatten(0, -2).numpy(),
+        point_size=0.001,
+        point_shape='circle',
+        position=(0.0, 0.0, -2.0),
+    )