Function get_min_R

plib/rigid_motion.py:268–310

Return the rotation matrix that rotates v1 to v2 in a geodestic manner. Args: v1: (*, 3) direction vector (unit norm), v2: (*, 3) direction vector (unit norm) Returns: (*, 3, 3) rotation matrix (s.t. v2 = R @ v1)

(
        v1: T.Union[np.ndarray, torch.Tensor],
        v2: T.Union[np.ndarray, torch.Tensor],
)

def get_min_R(
        v1: T.Union[np.ndarray, torch.Tensor],
        v2: T.Union[np.ndarray, torch.Tensor],
) -> T.Union[np.ndarray, torch.Tensor]:
    """
    Return the rotation matrix that rotates v1 to v2 in a geodestic manner.

    Args:
        v1: (*, 3) direction vector (unit norm),
        v2: (*, 3) direction vector (unit norm)

    Returns:
        (*, 3, 3) rotation matrix (s.t. v2 = R @ v1)
    """
    is_numpy = False
    if isinstance(v1, np.ndarray):
        is_numpy = True
        v1 = torch.from_numpy(v1)
    if isinstance(v2, np.ndarray):
        is_numpy = True
        v2 = torch.from_numpy(v2)

    v1_norm = torch.linalg.vector_norm(v1, ord=2, dim=-1)
    v2_norm = torch.linalg.vector_norm(v2, ord=2, dim=-1)
    assert torch.allclose(v1_norm, torch.ones(1, dtype=v1_norm.dtype, device=v1.device))
    assert torch.allclose(v2_norm, torch.ones(1, dtype=v2_norm.dtype, device=v2.device))

    k = torch.cross(v1, v2, dim=-1)  # (*, 3)
    cos_theta = (v1 * v2).sum(-1)  # (*,)

    *b_shape, d = v1.shape
    eye3 = torch.eye(3, device=v1.device, dtype=v1.dtype).expand(*b_shape, 3, 3)  # (*, 3, 3)
    if cos_theta > -1:
        Kx = get_cross_product_matrix(k)  # (*, 3, 3)
        R = eye3 + Kx + (Kx @ Kx) / (1 + cos_theta).unsqueeze(-1).unsqueeze(-1)  # (*, 3, 3)
    else:
        # v1 and v2 point to opposite directions
        R = -1 * eye3

    if is_numpy:
        R = R.detach().cpu().numpy()

    return R  # (*, 3, 3)


def get_cross_product_matrix(