vuer-cli 0.0.3__py3-none-any.whl → 0.0.5__py3-none-any.whl

vuer_cli/scripts/viz_ptc_cams.py

@@ -0,0 +1,613 @@
+"""Visualize 3D Point Cloud with Multiple Camera Frustums using Vuer."""
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, List, Optional
+
+from ..utils import print_error
+
+
+def matrix_to_position_rotation(matrix, debug=False):
+  """Extract position and rotation (Euler angles) from a 4x4 transformation matrix."""
+  import numpy as np
+
+  position = matrix[:3, 3].tolist()
+  R = matrix[:3, :3]
+
+  sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
+  singular = sy < 1e-6
+
+  if not singular:
+    x = np.arctan2(R[2, 1], R[2, 2])
+    y = np.arctan2(-R[2, 0], sy)
+    z = np.arctan2(R[1, 0], R[0, 0])
+  else:
+    x = np.arctan2(-R[1, 2], R[1, 1])
+    y = np.arctan2(-R[2, 0], sy)
+    z = 0
+
+  rotation = [x, y, z]
+  return position, rotation
+
+
+class MultiCameraVisualizer:
+  """Visualize point clouds from multiple cameras merged into one coordinate frame."""
+
+  def __init__(
+    self, data_dir: str, camera_names: List[str] = None, reference_camera: str = None
+  ):
+    import numpy as np
+
+    from .ptc_utils import (
+      PointCloudGenerator,
+      load_camera_intrinsics,
+      load_robot_poses,
+      load_static_transforms,
+    )
+
+    self.data_dir = Path(data_dir)
+
+    if camera_names is None:
+      camera_names = ["nav_front_d455", "nav_right_d455"]
+
+    self.camera_names = camera_names
+    self.reference_camera = reference_camera or camera_names[0]
+
+    self.robot_pose_interpolator = load_robot_poses(self.data_dir)
+
+    self.cameras = {}
+    for cam_name in camera_names:
+      self.cameras[cam_name] = {
+        "name": cam_name,
+        "intrinsics": load_camera_intrinsics(self.data_dir, cam_name),
+        "static_transform": load_static_transforms(self.data_dir, cam_name),
+        "generator": PointCloudGenerator(
+          data_dir=str(self.data_dir), camera_name=cam_name
+        ),
+      }
+
+    self.first_camera_transform = np.eye(4)
+
+  def compute_frustum_params(self, camera_name: str):
+    """Compute frustum visualization parameters from intrinsics."""
+    import numpy as np
+
+    intrinsics = self.cameras[camera_name]["intrinsics"]
+    fov_deg = 2 * np.arctan(intrinsics["height"] / (2 * intrinsics["fy"]))
+    fov_deg = np.degrees(fov_deg)
+    aspect = intrinsics["width"] / intrinsics["height"]
+    return {"fov": fov_deg, "aspect": aspect}
+
+  def apply_coordinate_flip(self, transform_matrix, points_homogeneous=None):
+    """Apply coordinate system transformation from OpenCV/ROS to Three.js conventions."""
+    import numpy as np
+
+    flip_xy = np.array([[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
+    flip_rotation_3x3 = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, -1]])
+
+    flipped_matrix = flip_xy @ transform_matrix
+    flipped_matrix[:3, :3] = flip_rotation_3x3 @ flipped_matrix[:3, :3]
+
+    if points_homogeneous is not None:
+      flipped_points = (flip_xy @ points_homogeneous.T).T
+      return flipped_matrix, flipped_points
+
+    return flipped_matrix
+
+  def setup_progressive_generator(
+    self,
+    max_frames: Optional[int] = None,
+    frame_step: int = 1,
+    max_depth: float = 10.0,
+    pixel_step: int = 1,
+  ):
+    """Setup for progressive point cloud generation from all cameras."""
+    import numpy as np
+
+    pose_time_min = self.robot_pose_interpolator.times[0]
+    pose_time_max = self.robot_pose_interpolator.times[-1]
+
+    for cam_name in self.camera_names:
+      pcg = self.cameras[cam_name]["generator"]
+      pairs = pcg._find_rgb_depth_pairs()
+      pairs = [p for p in pairs if pose_time_min <= p["timestamp_s"] <= pose_time_max]
+      if max_frames:
+        pairs = pairs[:max_frames:frame_step]
+      else:
+        pairs = pairs[::frame_step]
+      self.cameras[cam_name]["frame_pairs"] = pairs
+
+    self.max_depth = max_depth
+    self.pixel_step = pixel_step
+
+    ref_pairs = self.cameras[self.reference_camera]["frame_pairs"]
+    if len(ref_pairs) > 0:
+      first_pair = ref_pairs[0]
+      T_world_to_base = self.robot_pose_interpolator.interpolate_pose(
+        first_pair["timestamp_s"]
+      )
+      if T_world_to_base is not None:
+        T_base_to_camera = self.cameras[self.reference_camera][
+          "static_transform"
+        ].get_transform()
+        self.first_camera_transform = T_world_to_base @ T_base_to_camera
+      else:
+        self.first_camera_transform = np.eye(4)
+    else:
+      self.first_camera_transform = np.eye(4)
+
+    total_frames = sum(
+      len(self.cameras[cam]["frame_pairs"]) for cam in self.camera_names
+    )
+    return total_frames
+
+  def process_frame(
+    self, camera_name: str, frame_idx: int, color_by_camera: bool = False
+  ) -> Optional[Dict]:
+    """Process a single frame from a specific camera."""
+    import cv2
+    import numpy as np
+
+    cam = self.cameras[camera_name]
+    if frame_idx >= len(cam["frame_pairs"]):
+      return None
+
+    pair = cam["frame_pairs"][frame_idx]
+
+    rgb = cv2.imread(str(pair["rgb_file"]))
+    rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB)
+    depth = cv2.imread(str(pair["depth_file"]), cv2.IMREAD_UNCHANGED)
+
+    T_world_to_base = self.robot_pose_interpolator.interpolate_pose(pair["timestamp_s"])
+    if T_world_to_base is None:
+      return None
+
+    T_base_to_camera = cam["static_transform"].get_transform()
+    T_world_to_camera = T_world_to_base @ T_base_to_camera
+    T_ref_to_current_cv = np.linalg.inv(self.first_camera_transform) @ T_world_to_camera
+
+    if np.any(np.isnan(T_ref_to_current_cv)) or np.any(np.isinf(T_ref_to_current_cv)):
+      return None
+
+    points_cam, colors = cam["generator"].backproject_rgbd(
+      rgb, depth, self.max_depth, self.pixel_step
+    )
+    if len(points_cam) == 0:
+      return None
+
+    if color_by_camera:
+      camera_color_map = {
+        "nav_front_d455": np.array([0.0, 0.0, 1.0], dtype=np.float32),
+        "nav_right_d455": np.array([1.0, 0.0, 0.0], dtype=np.float32),
+        "nav_left_d455": np.array([0.0, 1.0, 0.0], dtype=np.float32),
+        "nav_rear_d455": np.array([1.0, 1.0, 0.0], dtype=np.float32),
+        "blindspot_front_d455": np.array([1.0, 0.0, 1.0], dtype=np.float32),
+        "blindspot_rear_d455": np.array([0.0, 1.0, 1.0], dtype=np.float32),
+      }
+      camera_color = camera_color_map.get(
+        camera_name, np.array([0.5, 0.5, 0.5], dtype=np.float32)
+      )
+      colors = np.tile(camera_color, (len(points_cam), 1))
+
+    points_cam_h = np.hstack([points_cam, np.ones((len(points_cam), 1))])
+    points_world_h = (T_world_to_camera @ points_cam_h.T).T
+    points_ref_cv_h = (np.linalg.inv(self.first_camera_transform) @ points_world_h.T).T
+
+    T_ref_to_current, points_ref_h = self.apply_coordinate_flip(
+      T_ref_to_current_cv, points_ref_cv_h
+    )
+    points_relative = points_ref_h[:, :3]
+
+    return {
+      "points": points_relative.astype(np.float32),
+      "colors": colors.astype(np.float32),
+      "camera_matrix": T_ref_to_current,
+      "position": T_ref_to_current[:3, 3],
+      "frame_idx": frame_idx,
+      "timestamp": pair["timestamp_s"],
+      "camera_name": camera_name,
+    }
+
+  def visualize(
+    self,
+    show_frustums: bool = True,
+    show_trajectory: bool = True,
+    show_pointcloud: bool = True,
+    frustum_scale: float = 0.3,
+    frustum_far: float = 0.5,
+    point_size: float = 0.01,
+    host: str = "0.0.0.0",
+    port: int = 8012,
+    progressive_max_frames: Optional[int] = None,
+    progressive_frame_step: int = 5,
+    progressive_pixel_step: int = 5,
+    progressive_fps: float = 5.0,
+    progressive_max_depth: float = 10.0,
+    no_animation: bool = False,
+    color_by_camera: bool = False,
+  ):
+    """Launch interactive visualization with multiple cameras."""
+    import asyncio
+
+    import numpy as np
+    from vuer import Vuer
+    from vuer.schemas import (
+      DefaultScene,
+      Frustum,
+      Line,
+      OrbitControls,
+      PerspectiveCamera,
+      PointCloud,
+    )
+
+    from .ptc_utils import matrix_to_three_js
+
+    app = Vuer(host=host, port=port)
+
+    print(f"Server: http://{host}:{port}")
+
+    viz = self
+
+    @app.spawn(start=True)
+    async def main(session):
+      viz.setup_progressive_generator(
+        max_frames=progressive_max_frames,
+        frame_step=progressive_frame_step,
+        max_depth=progressive_max_depth,
+        pixel_step=progressive_pixel_step,
+      )
+
+      camera_transform_threejs = viz.apply_coordinate_flip(viz.first_camera_transform)
+      frustum_params = viz.compute_frustum_params(viz.reference_camera)
+      camera_position, camera_rotation = matrix_to_position_rotation(
+        camera_transform_threejs
+      )
+
+      session.set @ DefaultScene(
+        show_helper=False,
+        rawChildren=[
+          PerspectiveCamera(
+            key="main-camera",
+            fov=frustum_params["fov"],
+            aspect=frustum_params["aspect"],
+            near=0.1,
+            far=1000,
+            position=camera_position,
+            rotation=camera_rotation,
+            makeDefault=True,
+          )
+        ],
+        bgChildren=[OrbitControls(key="OrbitControls")],
+        up=[0, 1, 0],
+        grid=False,
+      )
+
+      camera_data = {
+        cam_name: {
+          "points": [],
+          "colors": [],
+          "trajectory": [],
+          "points_consolidated": None,
+          "colors_consolidated": None,
+        }
+        for cam_name in viz.camera_names
+      }
+
+      CONSOLIDATION_INTERVAL = 50
+      frame_delay = 1.0 / progressive_fps
+
+      camera_colors = {
+        "nav_front_d455": {"frustum": "cyan", "cone": "blue", "trajectory": "blue"},
+        "nav_right_d455": {"frustum": "magenta", "cone": "red", "trajectory": "red"},
+        "nav_left_d455": {"frustum": "lime", "cone": "green", "trajectory": "green"},
+        "nav_rear_d455": {
+          "frustum": "yellow",
+          "cone": "orange",
+          "trajectory": "orange",
+        },
+        "blindspot_front_d455": {
+          "frustum": "magenta",
+          "cone": "purple",
+          "trajectory": "purple",
+        },
+        "blindspot_rear_d455": {
+          "frustum": "cyan",
+          "cone": "teal",
+          "trajectory": "teal",
+        },
+      }
+      default_colors = {"frustum": "white", "cone": "gray", "trajectory": "gray"}
+
+      def consolidate_camera_points(cam_name):
+        data = camera_data[cam_name]
+        if len(data["points"]) > 0:
+          batch_points = np.vstack(data["points"])
+          batch_colors = np.vstack(data["colors"])
+          if data["points_consolidated"] is not None:
+            data["points_consolidated"] = np.vstack(
+              [data["points_consolidated"], batch_points]
+            )
+            data["colors_consolidated"] = np.vstack(
+              [data["colors_consolidated"], batch_colors]
+            )
+          else:
+            data["points_consolidated"] = batch_points
+            data["colors_consolidated"] = batch_colors
+          data["points"].clear()
+          data["colors"].clear()
+
+      if no_animation:
+        for cam_name in viz.camera_names:
+          num_frames = len(viz.cameras[cam_name]["frame_pairs"])
+          for frame_idx in range(num_frames):
+            result = viz.process_frame(
+              cam_name, frame_idx, color_by_camera=color_by_camera
+            )
+            if result is None:
+              continue
+            camera_data[cam_name]["points"].append(result["points"])
+            camera_data[cam_name]["colors"].append(result["colors"])
+            camera_data[cam_name]["trajectory"].append(result["position"])
+            if (frame_idx + 1) % CONSOLIDATION_INTERVAL == 0:
+              consolidate_camera_points(cam_name)
+          consolidate_camera_points(cam_name)
+
+        if show_trajectory:
+          for cam_name in viz.camera_names:
+            if len(camera_data[cam_name]["trajectory"]) > 1:
+              colors = camera_colors.get(cam_name, default_colors)
+              session.upsert @ Line(
+                key=f"trajectory_{cam_name}",
+                points=np.array(camera_data[cam_name]["trajectory"]),
+                color=colors["trajectory"],
+                closed=False,
+                lineWidth=3,
+              )
+
+        if show_frustums:
+          for cam_name in viz.camera_names:
+            if len(camera_data[cam_name]["trajectory"]) > 0:
+              last_frame_idx = len(viz.cameras[cam_name]["frame_pairs"]) - 1
+              result = viz.process_frame(
+                cam_name, last_frame_idx, color_by_camera=color_by_camera
+              )
+              if result is not None:
+                if not (
+                  np.any(np.isnan(result["camera_matrix"]))
+                  or np.any(np.isinf(result["camera_matrix"]))
+                ):
+                  colors = camera_colors.get(cam_name, default_colors)
+                  session.upsert @ Frustum(
+                    key=f"camera_{cam_name}",
+                    matrix=matrix_to_three_js(result["camera_matrix"]),
+                    showFrustum=True,
+                    showImagePlane=True,
+                    showFocalPlane=False,
+                    colorFrustum=colors["frustum"],
+                    colorCone=colors["cone"],
+                  )
+
+        if show_pointcloud:
+          all_points = []
+          all_colors = []
+          for cam_name in viz.camera_names:
+            if camera_data[cam_name]["points_consolidated"] is not None:
+              all_points.append(camera_data[cam_name]["points_consolidated"])
+              all_colors.append(camera_data[cam_name]["colors_consolidated"])
+          if len(all_points) > 0:
+            combined_points = np.vstack(all_points)
+            combined_colors = np.vstack(all_colors)
+            session.upsert @ PointCloud(
+              key="merged_pointcloud",
+              vertices=combined_points,
+              colors=combined_colors,
+              size=point_size,
+            )
+
+        await session.forever()
+        return
+
+      # Animated mode
+      max_frames_per_cam = max(
+        len(viz.cameras[cam]["frame_pairs"]) for cam in viz.camera_names
+      )
+
+      for frame_idx in range(max_frames_per_cam):
+        for cam_name in viz.camera_names:
+          result = viz.process_frame(
+            cam_name, frame_idx, color_by_camera=color_by_camera
+          )
+          if result is None:
+            continue
+
+          camera_data[cam_name]["points"].append(result["points"])
+          camera_data[cam_name]["colors"].append(result["colors"])
+          camera_data[cam_name]["trajectory"].append(result["position"])
+
+          if (frame_idx + 1) % CONSOLIDATION_INTERVAL == 0:
+            consolidate_camera_points(cam_name)
+
+          if show_frustums:
+            if not (
+              np.any(np.isnan(result["camera_matrix"]))
+              or np.any(np.isinf(result["camera_matrix"]))
+            ):
+              colors = camera_colors.get(cam_name, default_colors)
+              session.upsert @ Frustum(
+                key=f"camera_{cam_name}",
+                matrix=matrix_to_three_js(result["camera_matrix"]),
+                showFrustum=True,
+                showImagePlane=True,
+                showFocalPlane=False,
+                colorFrustum=colors["frustum"],
+                colorCone=colors["cone"],
+              )
+
+        if show_trajectory:
+          for cam_name in viz.camera_names:
+            if len(camera_data[cam_name]["trajectory"]) > 1:
+              colors = camera_colors.get(cam_name, default_colors)
+              session.upsert @ Line(
+                key=f"trajectory_{cam_name}",
+                points=np.array(camera_data[cam_name]["trajectory"]),
+                color=colors["trajectory"],
+                closed=False,
+                lineWidth=3,
+              )
+
+        if show_pointcloud:
+          all_points = []
+          all_colors = []
+          for cam_name in viz.camera_names:
+            if camera_data[cam_name]["points_consolidated"] is not None:
+              all_points.append(camera_data[cam_name]["points_consolidated"])
+              all_colors.append(camera_data[cam_name]["colors_consolidated"])
+            if len(camera_data[cam_name]["points"]) > 0:
+              all_points.extend(camera_data[cam_name]["points"])
+              all_colors.extend(camera_data[cam_name]["colors"])
+          if len(all_points) > 0:
+            combined_points = np.vstack(all_points)
+            combined_colors = np.vstack(all_colors)
+            session.upsert @ PointCloud(
+              key="merged_pointcloud",
+              vertices=combined_points,
+              colors=combined_colors,
+              size=point_size,
+            )
+
+        await asyncio.sleep(frame_delay)
+      await session.forever()
+
+
+@dataclass
+class VizPtcCams:
+  """Visualize point clouds from multiple cameras with frustums and trajectories.
+
+  DESCRIPTION
+      Creates an interactive 3D visualization of RGB-D point clouds from multiple
+      cameras. Displays camera frustums, trajectories, and merged point clouds
+      in a web-based viewer using Vuer.
+
+      Features:
+      - Multi-camera point cloud fusion in a unified coordinate frame
+      - Animated or static point cloud display
+      - Camera frustum visualization showing camera poses
+      - Trajectory lines showing camera movement over time
+
+      The visualization server runs at http://localhost:8012 by default.
+      Open this URL in a browser to view the 3D scene.
+
+  INPUT FORMAT
+      Requires extracted MCAP data (use 'vuer demcap' first):
+          <data_dir>/
+          ├── images/<camera>_color_image_*/    # RGB images
+          ├── depth/<camera>_*/                  # Depth images (PNG, 16-bit mm)
+          ├── transforms/transforms.csv          # Robot poses
+          └── camera_intrinsics.json             # Camera calibration
+
+  EXAMPLES
+      # Basic visualization with default cameras
+      vuer viz_ptc_cams --data-dir /path/to/extracted_data
+
+      # Specify cameras (comma-separated)
+      vuer viz_ptc_cams --data-dir /path/to/data --cameras nav_front_d455,nav_right_d455
+
+      # Faster processing with sparser sampling
+      vuer viz_ptc_cams --data-dir /path/to/data --frame-step 10 --pixel-step 5
+
+      # Static view (no animation)
+      vuer viz_ptc_cams --data-dir /path/to/data --no-animation
+
+      # Custom server port
+      vuer viz_ptc_cams --data-dir /path/to/data --port 8080
+
+  DEPENDENCIES
+      Requires: pip install 'vuer-cli[viz]'
+  """
+
+  # Required: path to extracted MCAP data directory
+  data_dir: str
+
+  # Camera options
+  cameras: str = "nav_front_d455,nav_right_d455"  # Comma-separated camera names
+  reference_camera: str = None  # Reference camera for coordinate frame
+
+  # Display options
+  no_frustums: bool = False  # Hide camera frustums
+  no_trajectory: bool = False  # Hide camera trajectory lines
+  no_pointcloud: bool = False  # Hide point cloud
+  frustum_scale: float = 0.3  # Scale factor for frustum visualization
+  frustum_far: float = 10.0  # Far clipping plane for frustums in meters
+  point_size: float = 0.01  # Size of points
+
+  # Server options
+  host: str = "0.0.0.0"  # Server host
+  port: int = 8012  # Server port
+
+  # Processing options
+  max_frames: int = None  # Max frames per camera (default: all)
+  frame_step: int = 10  # Process every Nth frame
+  pixel_step: int = 5  # Sample every Nth pixel
+  fps: float = 5.0  # Frames per second for animation
+  max_depth: float = 10.0  # Maximum depth in meters
+  no_animation: bool = False  # Show complete point cloud at once
+
+  def __call__(self) -> int:
+    """Execute viz_ptc_cams command."""
+    try:
+      import cv2
+      import numpy as np
+      import pandas as pd
+      from vuer import Vuer
+    except ImportError as e:
+      print_error(
+        f"Missing dependencies for viz_ptc_cams command: {e}\n\n"
+        "Install with:\n"
+        "  pip install 'vuer-cli[viz]'\n"
+        "  # or: uv add 'vuer-cli[viz]'"
+      )
+      return 1
+
+    try:
+      data_path = Path(self.data_dir)
+      if not data_path.exists():
+        raise FileNotFoundError(f"Data directory {self.data_dir} does not exist")
+
+      # Parse camera names (comma-separated)
+      camera_list = [c.strip() for c in self.cameras.split(",")]
+
+      viz = MultiCameraVisualizer(
+        data_dir=self.data_dir,
+        camera_names=camera_list,
+        reference_camera=self.reference_camera,
+      )
+
+      viz.visualize(
+        show_frustums=not self.no_frustums,
+        show_trajectory=not self.no_trajectory,
+        show_pointcloud=not self.no_pointcloud,
+        frustum_scale=self.frustum_scale,
+        frustum_far=self.frustum_far,
+        point_size=self.point_size,
+        host=self.host,
+        port=self.port,
+        progressive_max_frames=self.max_frames,
+        progressive_frame_step=self.frame_step,
+        progressive_pixel_step=self.pixel_step,
+        progressive_fps=self.fps,
+        progressive_max_depth=self.max_depth,
+        no_animation=self.no_animation,
+        color_by_camera=False,
+      )
+
+      return 0
+
+    except FileNotFoundError as e:
+      print_error(str(e))
+      return 1
+    except Exception as e:
+      print_error(f"Unexpected error: {e}")
+      import traceback
+
+      traceback.print_exc()
+      return 1