Function estimate_sim3

benchmark/utils/sim3_align.py:9–43

Estimate SIM(3) transform: source -> target. Use SVD to solve for s, R, t such that target ≈ s * R @ source + t. Args: source_points: (M, 3) source points target_points: (M, 3) target points Returns: s: float, scale factor R: (3, 3) rotation matrix

(source_points, target_points)

def estimate_sim3(source_points, target_points):
    """Estimate SIM(3) transform: source -> target.

    Use SVD to solve for s, R, t such that target ≈ s * R @ source + t.

    Args:
        source_points: (M, 3) source points
        target_points: (M, 3) target points

    Returns:
        s: float, scale factor
        R: (3, 3) rotation matrix
        t: (3,) translation vector
    """
    mu_src = np.mean(source_points, axis=0)
    mu_tgt = np.mean(target_points, axis=0)

    src_centered = source_points - mu_src
    tgt_centered = target_points - mu_tgt

    scale_src = np.sqrt((src_centered ** 2).sum(axis=1).mean())
    scale_tgt = np.sqrt((tgt_centered ** 2).sum(axis=1).mean())
    s = scale_tgt / (scale_src + 1e-12)

    src_scaled = src_centered * s

    H = src_scaled.T @ tgt_centered
    U, _, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:
        Vt[2, :] *= -1
        R = Vt.T @ U.T

    t = mu_tgt - s * R @ mu_src
    return s, R, t


def weighted_estimate_sim3(source_points, target_points, weights):