Function calibrate_joint

scripts/stereo_calibration.py:780–1070

Perform joint intrinsic/extrinsic calibration using position-only constraints. This method uses multiple markers at different locations to simultaneously solve for camera intrinsics and the head-to-camera extrinsic transform. Uses differential evolution (global optimizer)

(
    session_dir: Path,
    intrinsic_calib: dict,
    optimize_intrinsics: bool = True,
    max_iterations: int = 100,
    use_global_optimizer: bool = True
)

def calibrate_joint(
    session_dir: Path,
    intrinsic_calib: dict,
    optimize_intrinsics: bool = True,
    max_iterations: int = 100,
    use_global_optimizer: bool = True
) -> dict:
    """
    Perform joint intrinsic/extrinsic calibration using position-only constraints.
    
    This method uses multiple markers at different locations to simultaneously
    solve for camera intrinsics and the head-to-camera extrinsic transform.
    
    Uses differential evolution (global optimizer) to avoid local minima.
    
    The cost function minimizes:
        sum_t sum_i || T_b^v(t) @ T_v^c @ solvePnP(corners_i(t), K, D) - p_b^{m_i} ||²
    
    Where p_b^{m_i} is the ground truth marker position in world frame.
    
    Args:
        session_dir: Path to session directory
        intrinsic_calib: Initial intrinsic calibration dictionary (used for image size, not values)
        optimize_intrinsics: Whether to also optimize intrinsics or just extrinsics
        max_iterations: Maximum iterations for the optimizer
        use_global_optimizer: If True, use differential evolution; else use LM
        
    Returns:
        Dictionary with optimized calibration results
    """
    from scipy.optimize import differential_evolution
    
    extrinsic_dir = session_dir / "extrinsic"
    if not extrinsic_dir.exists():
        raise ValueError(f"Extrinsic directory not found: {extrinsic_dir}")
    
    # Load session info for marker size
    session_info = load_session_info(session_dir)
    marker_size = session_info.get("marker_size_meters", 0.055)
    
    # Get image size from intrinsic calib
    img_width = intrinsic_calib.get("image_width", 640)
    img_height = intrinsic_calib.get("image_height", 480)
    
    # Marker 3D points in marker frame (centered at origin)
    marker_3d_points = np.array([
        [-marker_size / 2, marker_size / 2, 0],
        [marker_size / 2, marker_size / 2, 0],
        [marker_size / 2, -marker_size / 2, 0],
        [-marker_size / 2, -marker_size / 2, 0]
    ], dtype=np.float32)
    
    # Collect observations for both cameras
    results = {}
    
    for side in ["left", "right"]:
        print(f"\n{'='*60}")
        print(f"JOINT CALIBRATION - {side.upper()} CAMERA")