Function draw_camera

utils/visualize_tracking.py:195–215

Draw a single camera with its coordinate frame and viewing direction.

(T_head_camera: np.ndarray, label: str, cam_scale: float)

        return
    
    def draw_camera(T_head_camera: np.ndarray, label: str, cam_scale: float):
        """Draw a single camera with its coordinate frame and viewing direction."""
        # T_head^camera transforms points from head to camera
        # To get camera pose in head frame, we need the inverse: T_camera^head
        T_camera_in_head = np.linalg.inv(T_head_camera)
        
        # Now transform from head frame to world frame
        T_world_camera = head_matrix @ T_camera_in_head
        
        # Draw coordinate frame
        draw_coordinate_frame(ax, T_world_camera, label, cam_scale)
        
        # Draw viewing direction (camera looks in -Z direction in ARKit convention)
        origin = get_position(T_world_camera)
        # -Z direction is the viewing direction
        look_dir = -T_world_camera[:3, 2]  # Negative Z axis
        look_end = origin + look_dir * cam_scale * 2
        
        # Draw a dashed cyan line for viewing direction
        ax.plot([origin[0], look_end[0]], [origin[1], look_end[1]], [origin[2], look_end[2]], 
                'c--', linewidth=1.5, alpha=0.7)
    
    # Get head-to-camera transform
    left_h2c = extrinsic.get("leftHeadToCamera")