Function draw_camera_frame

utils/visualize_tracking.py:184–227

Draw camera frame if extrinsic calibration is available. The extrinsic calibration stores T_head^camera (transforms points from head to camera). To visualize the camera's POSE in the head frame, we need the inverse: T_camera^head. The camera's Z-axis in ARKit convention points

(ax, head_matrix: np.ndarray, extrinsic: dict | None, scale: float = 0.08)

def draw_camera_frame(ax, head_matrix: np.ndarray, extrinsic: dict | None, scale: float = 0.08):
    """Draw camera frame if extrinsic calibration is available.
    
    The extrinsic calibration stores T_head^camera (transforms points from head to camera).
    To visualize the camera's POSE in the head frame, we need the inverse: T_camera^head.
    
    The camera's Z-axis in ARKit convention points BACKWARD, so the camera looks in -Z direction.
    """
    if extrinsic is None:
        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")
    if left_h2c:
        T_head_camera = matrix_from_array(left_h2c)
        draw_camera(T_head_camera, "L-Cam", scale)
    
    # Right camera for stereo
    right_h2c = extrinsic.get("rightHeadToCamera")
    if right_h2c:
        T_head_camera_r = matrix_from_array(right_h2c)
        draw_camera(T_head_camera_r, "R-Cam", scale * 0.8)


def print_calibration_info(extrinsic: dict):