comparison to OpenCV Ransac PNP

[sherwoac]

Hi.

I've been comparing Superansac to the OpenCV RANSAC PnP implementation (solvePnPRansac).

I think I must be doing something wrong - likely in the myriad of settings - below is a minimal example shows a typical result of the result from a much bigger dataset.

import numpy as np
import pysuperansac
import pycolmap
import cv2
from scipy.spatial.transform import Rotation

def rotation_error(R_inf, R_gt):
    R_inf = Rotation.from_matrix(R_inf)
    R_gt = Rotation.from_matrix(gt_pose[:3, :3])
    R_rel = R_inf.inv() * R_gt
    return np.rad2deg(R_rel.magnitude())

# %%
gt_pose = np.array(
    [[ 0.8933921 , -0.44218102, -0.07953956,  0.0868178 ],
     [ 0.26641935,  0.37885556,  0.8862783 ,  0.16766696],
     [-0.36176145, -0.8129849 ,  0.45627213,  1.9704077 ],
     [ 0.        ,  0.        ,  0.        ,  1.        ]]
    )

keypoints2D = np.array([[[302.4921, 358.3686,   2.0000],
         [248.1180, 340.2681,   2.0000],
         [321.6395, 332.0792,   2.0000],
         [272.6149, 316.8930,   2.0000],
         [309.8420, 295.2504,   2.0000],
         [255.9953, 277.6184,   2.0000],
         [331.8448, 272.9306,   2.0000],
         [279.3428, 257.0728,   2.0000]]])

keypoints3D = np.array([[[ 0.1440,  0.1440,  0.1720],
         [-0.1440,  0.1440,  0.1720],
         [ 0.1440, -0.1440,  0.1720],
         [-0.1440, -0.1440,  0.1720],
         [ 0.1440,  0.1440, -0.1720],
         [-0.1440,  0.1440, -0.1720],
         [ 0.1440, -0.1440, -0.1720],
         [-0.1440, -0.1440, -0.1720]]])

intrinsics = np.array([[390.59848,   0.     , 273.     ],
       [  0.     , 390.59848, 273.     ],
       [  0.     ,   0.     ,   1.     ]])

D = np.array([0., 0., 0., 0., 0.])

default_config = pysuperansac.RANSACSettings()
default_config.confidence = .9999
# default_config.inlier_selector = pysuperansac.InlierSelectorType.SpacePartitioning # nb this breaks
default_config.inlier_threshold = 1.
default_config.min_iterations = 100
default_config.max_iterations = 1000
default_config.sampler = pysuperansac.SamplerType.PROSAC
default_config.scoring = pysuperansac.ScoringType.MAGSAC
default_config.local_optimization = pysuperansac.LocalOptimizationType.GCRANSAC
default_config.final_optimization = pysuperansac.LocalOptimizationType.IteratedLSQ
default_config.neighborhood = pysuperansac.NeighborhoodType.Grid

if default_config.neighborhood == pysuperansac.NeighborhoodType.Grid:
    default_config.neighborhood_settings.neighborhood_grid_density = .2    
    
keypoints2D = keypoints2D[:, :, :2]

image_height = int(intrinsics[1, 2] * 2)
image_width = int(intrinsics[0, 2] * 2)

query_camera = pycolmap.Camera().create(
    camera_id=0,
    height=image_height,
    width=image_width,
    model=pycolmap.CameraModelId.SIMPLE_PINHOLE,
    focal_length=intrinsics[0, 0],
)

min_coords = np.min(keypoints3D[0], axis=0)
shifted_points3D = keypoints3D[0] - min_coords

corrs = np.hstack((keypoints2D[0], shifted_points3D))
bounding_box = np.hstack([np.array([image_width, image_height]), np.max(shifted_points3D, axis=0)])

R_inf_super, t_sr, inliers, score, iterations  = pysuperansac.estimateAbsolutePose(
    np.ascontiguousarray(corrs), 
    pysuperansac.CameraType.SimplePinhole,
    query_camera.params,
    bounding_box,
    [],
    config = default_config)

t_sr = t_sr - R_inf_super @ min_coords

print(f'{R_inf_super=} {t_sr=} {inliers=} {score=} {iterations}')
super_t_error = np.linalg.norm(t_sr - gt_pose[:3, 3])

# OpenCV PnP for comparison
ok_ransac, rvec_opencv, tvec_opencv, _ = cv2.solvePnPRansac(
            objectPoints=keypoints3D,
            imagePoints=keypoints2D,
            cameraMatrix=intrinsics,
            distCoeffs=D,
            flags=cv2.SOLVEPNP_ITERATIVE,
            confidence=0.9999,
            reprojectionError=1000
        )

R_inf_opencv, _ = cv2.Rodrigues(rvec_opencv)
opencv_t_error = np.linalg.norm(tvec_opencv[:3, 0] - gt_pose[:3, 3]).astype(np.float32)

print(f'OpenCV rot error: {rotation_error(R_inf_opencv, gt_pose[:3, :3]):.2f}deg')
print(f'SuperRANSAC rot error: {rotation_error(R_inf_super, gt_pose[:3, :3]):.2f}deg')
print(f'OpenCV t error: {opencv_t_error:.3f}')
print(f'SuperRANSAC t error: {super_t_error:.3f}')

output:

R_inf_super=array([[ 0.90292292, -0.41888801, -0.09624464],
       [ 0.2394802 ,  0.30437418,  0.92195748],
       [-0.35690255, -0.85550523,  0.37514181]]) t_sr=array([0.08411187, 0.16402046, 1.93472582]) inliers=[0, 1, 2, 3, 6] score=0.008386665629109894 100
OpenCV rot error: 3.59deg
SuperRANSAC rot error: 5.26deg
OpenCV t error: 0.017
SuperRANSAC t error: 0.036

[danini]

After surviving CVPR SM deadline, I will look into this!

[sherwoac]

all good, no rush, was also trying a test case where you increase the number of projected key points and try to recover, see if that makes a difference Vs opencv ransac, can post that at some point.