nocs_utils.py

cpas_toolbox.datasets.nocs_utils

Module for utility function related to NOCS dataset.

This module contains functions to find similarity transform from NOCS maps and evaluation function for typical metrics on the NOCS datasets.

Aligning code by Srinath Sridhar

https://raw.githubusercontent.com/hughw19/NOCS_CVPR2019/master/aligning.py

PoseEstimationError

Bases: Exception

Error if pose estimation encountered an error.

Source code in cpas_toolbox/datasets/nocs_utils.py

class PoseEstimationError(Exception):
    """Error if pose estimation encountered an error."""

    pass

estimate_similarity_transform

estimate_similarity_transform(
    source: np.ndarray, target: np.ndarray, verbose: bool = False
) -> tuple

Estimate similarity transform from source to target from point correspondences.

Source and target are pairwise correponding pointsets, i.e., they include same number of points and the first point of source corresponds to the first point of target. RANSAC is used for outlier-robust estimation.

A similarity transform is estimated (i.e., isotropic scale, rotation and translation) that transforms source points onto the target points.

Note that the returned values fulfill the following equations transform @ source_points = scale * rotation_matrix @ source_points + position when ignoring homogeneous coordinate for left-hand side.

PARAMETER	DESCRIPTION
source	Source points that will be transformed, shape (N,3). TYPE: ndarray
target	Target points to which source will be aligned to, shape (N,3). TYPE: ndarray
verbose	If true additional information will be printed. TYPE: bool DEFAULT: False

RETURNS	DESCRIPTION
position	Translation to translate source to target, shape (3,). TYPE: ndarray
rotation_matrix	Rotation to rotate source to target, shape (3,3). TYPE: ndarray
scale	Scaling factor along each axis, to scale source to target. TYPE: float
transform	Homogeneous transformation matrix, shape (4,4). TYPE: ndarray

Source code in cpas_toolbox/datasets/nocs_utils.py

def estimate_similarity_transform(
    source: np.ndarray, target: np.ndarray, verbose: bool = False
) -> tuple:
    """Estimate similarity transform from source to target from point correspondences.

    Source and target are pairwise correponding pointsets, i.e., they include same
    number of points and the first point of source corresponds to the first point of
    target. RANSAC is used for outlier-robust estimation.

    A similarity transform is estimated (i.e., isotropic scale, rotation and
    translation) that transforms source points onto the target points.

    Note that the returned values fulfill the following equations
        transform @ source_points = scale * rotation_matrix @ source_points + position
    when ignoring homogeneous coordinate for left-hand side.

    Args:
        source: Source points that will be transformed, shape (N,3).
        target: Target points to which source will be aligned to, shape (N,3).
        verbose: If true additional information will be printed.

    Returns:
        position (np.ndarray): Translation to translate source to target, shape (3,).
        rotation_matrix (np.ndarray): Rotation to rotate source to target, shape (3,3).
        scale (float):
            Scaling factor along each axis, to scale source to target.
        transform (np.ndarray): Homogeneous transformation matrix, shape (4,4).
    """
    if len(source) < 5 or len(target) < 5:
        print("Pose estimation failed. Not enough point correspondences: ", len(source))
        return None, None, None, None

    # make points homogeneous
    source_hom = np.transpose(np.hstack([source, np.ones([source.shape[0], 1])]))  # 4,N
    target_hom = np.transpose(np.hstack([target, np.ones([source.shape[0], 1])]))  # 4,N

    # Auto-parameter selection based on source-target heuristics
    target_norm = np.mean(np.linalg.norm(target, axis=1))  # mean distance from origin
    source_norm = np.mean(np.linalg.norm(source, axis=1))
    ratio_ts = target_norm / source_norm
    ratio_st = source_norm / target_norm
    pass_t = ratio_st if (ratio_st > ratio_ts) else ratio_ts
    pass_t *= 0.01  # tighter bound
    stop_t = pass_t / 100
    n_iter = 100
    if verbose:
        print("Pass threshold: ", pass_t)
        print("Stop threshold: ", stop_t)
        print("Number of iterations: ", n_iter)

    source_inliers_hom, target_inliers_hom, best_inlier_ratio = _get_ransac_inliers(
        source_hom,
        target_hom,
        max_iterations=n_iter,
        pass_threshold=pass_t,
        stop_threshold=stop_t,
    )

    if best_inlier_ratio < 0.1:
        print("Pose estimation failed. Small inlier ratio: ", best_inlier_ratio)
        return None, None, None, None

    scales, rotation_matrix, position, out_transform = _estimate_similarity_umeyama(
        source_inliers_hom, target_inliers_hom
    )
    scale = scales[0]

    if verbose:
        print("BestInlierRatio:", best_inlier_ratio)
        print("Rotation:\n", rotation_matrix)
        print("Position:\n", position)
        print("Scales:", scales)

    return position, rotation_matrix, scale, out_transform