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

Evaluation code by ... TODO

PoseEstimationError

Bases: Exception

Error if pose estimation encountered an error.

Source code in sdfest/initialization/datasets/nocs_utils.py

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

    pass

estimate_similarity_transform(source, target, verbose=False)

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.

Parameters:

Name	Type	Description	Default
source	ndarray	Source points that will be transformed, shape (N,3).	required
target	ndarray	Target points to which source will be aligned to, shape (N,3).	required
verbose	bool	If true additional information will be printed.	False

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).

Source code in sdfest/initialization/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