def test_outliers_absolute_pose_known_rotation_ransac(shots_and_their_points):
for pose, bearings, points in shots_and_their_points:
scale = 1e-3
bearings = copy.deepcopy(bearings)
bearings += np.random.rand(*bearings.shape) * scale
ratio_outliers = 0.3
add_outliers(ratio_outliers, bearings, 0.1, 1.0)
bearings /= np.linalg.norm(bearings, axis=1)[:, None]
R = pose.get_rotation_matrix()
p_rotated = np.array([R.dot(p) for p in points])
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_absolute_pose_known_rotation(
bearings, p_rotated, scale, params, pyrobust.RansacType.RANSAC)
tolerance = 0.05
inliers_count = (1 - ratio_outliers) * len(points)
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=tolerance)
assert np.linalg.norm(pose.translation - result.lo_model) < 8e-2
def absolute_pose_known_rotation_ransac(bs, Xs, method, threshold, iterations,
probabilty):
# in-house estimation
if in_house_multiview:
threshold = np.arccos(1 - threshold)
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_absolute_pose_known_rotation(
bs, Xs, threshold, params, pyrobust.RansacType.RANSAC)
t = -result.lo_model.copy()
R = np.identity(3)
return np.concatenate((R, [[t[0]], [t[1]], [t[2]]]), axis=1)
else:
try:
return pyopengv.absolute_pose_ransac(bs,
Xs,
method,
threshold,
iterations=iterations,
probabilty=probabilty)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.absolute_pose_ransac(bs, Xs, method, threshold,
iterations)
def absolute_pose_known_rotation_ransac(bs, Xs, threshold, iterations, probabilty):
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_absolute_pose_known_rotation(bs, Xs, threshold, params, pyrobust.RansacType.RANSAC)
t = -result.lo_model.copy()
R = np.identity(3)
return np.concatenate((R, [[t[0]], [t[1]], [t[2]]]), axis=1)
def absolute_pose_known_rotation_ransac(
bs: np.ndarray,
Xs: np.ndarray,
threshold: float,
iterations: int,
probability: float,
) -> np.ndarray:
params = pyrobust.RobustEstimatorParams()
params.iterations = iterations
result = pyrobust.ransac_absolute_pose_known_rotation(
bs, Xs, threshold, params, pyrobust.RansacType.RANSAC)
t = -result.lo_model.copy()
R = np.identity(3)
return np.concatenate((R, [[t[0]], [t[1]], [t[2]]]), axis=1)