def test_outliers_absolute_pose_ransac(one_shot_and_its_points):
pose, bearings, points = one_shot_and_its_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]
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_absolute_pose(bearings, points, scale, params,
pyrobust.RansacType.RANSAC)
expected = pose.get_Rt()
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(expected - result.lo_model, ord='fro') < 8e-2
def relative_pose_ransac_rotation_only(b1, b2, threshold, iterations,
probability):
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_rotation(b1, b2, threshold, params,
pyrobust.RansacType.RANSAC)
return result.lo_model.T
def test_outliers_essential_ransac(one_pair_and_its_E):
f1, f2, E, _ = one_pair_and_its_E
points = np.concatenate((f1, f2), axis=1)
scale = 1e-3
points += np.random.rand(*points.shape) * scale
ratio_outliers = 0.4
add_outliers(ratio_outliers, points, 0.1, 0.4)
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_essential(f1, f2, scale, params,
pyrobust.RansacType.RANSAC)
tolerance = 0.04 # some outliers might have been moved along the epipolar
inliers_count = (1 - ratio_outliers) * len(points)
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=tolerance)
# sometimes, the negative of E is the good one
correct_found = 0
for sign in [-1, 1]:
correct_found += np.linalg.norm(E - sign * result.lo_model,
ord='fro') < 5e-2
assert correct_found == 1
def test_outliers_relative_pose_ransac(one_pair_and_its_E):
f1, f2, _, pose = one_pair_and_its_E
points = np.concatenate((f1, f2), axis=1)
scale = 1e-3
points += np.random.rand(*points.shape) * scale
ratio_outliers = 0.3
add_outliers(ratio_outliers, points, 0.1, 1.0)
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_pose(f1, f2, scale, params,
pyrobust.RansacType.RANSAC)
pose.translation /= np.linalg.norm(pose.translation)
expected = pose.get_Rt()
tolerance = 0.04 # some outliers might have been moved along the epipolar
inliers_count = (1 - ratio_outliers) * len(points)
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=tolerance)
assert np.linalg.norm(expected - result.lo_model, ord='fro') < 8e-2
def test_outliers_relative_pose_ransac(pairs_and_their_E):
for f1, f2, _, pose in pairs_and_their_E:
points = np.concatenate((f1, f2), axis=1)
scale = 1e-3
points += np.random.rand(*points.shape) * scale
ratio_outliers = 0.3
add_outliers(ratio_outliers, points, 0.1, 1.0)
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
scale_eps_ratio = 1e-1
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_pose(f1, f2,
scale * (1.0 + scale_eps_ratio),
params,
pyrobust.RansacType.RANSAC)
pose.translation /= np.linalg.norm(pose.translation)
expected = pose.get_Rt()
tolerance = 0.1
inliers_count = (1 - ratio_outliers) * len(points)
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=tolerance)
assert np.linalg.norm(expected - result.lo_model, ord='fro') < 16e-2
def test_outliers_essential_ransac(pairs_and_their_E):
for f1, f2, E, _ in pairs_and_their_E:
points = np.concatenate((f1, f2), axis=1)
scale = 1e-3
points += np.random.rand(*points.shape) * scale
ratio_outliers = 0.3
add_outliers(ratio_outliers, points, 0.1, 0.4)
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
scale_eps_ratio = 1e-1
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_essential(f1, f2,
scale * (1.0 + scale_eps_ratio),
params, pyrobust.RansacType.RANSAC)
tolerance = 0.12 # some outliers might have been moved along the epipolar
inliers_count = (1 - ratio_outliers) * len(points)
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=tolerance)
def relative_pose_ransac(b1, b2, method, threshold, iterations, probability):
# in-house estimation
if in_house_multiview:
threshold = np.arccos(1 - threshold)
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_pose(b1, b2, threshold, params,
pyrobust.RansacType.RANSAC)
Rt = result.lo_model.copy()
R, t = Rt[:3, :3].copy(), Rt[:, 3].copy()
Rt[:3, :3] = R.T
Rt[:, 3] = -R.T.dot(t)
return Rt
# fallback to opengv
else:
try:
return pyopengv.relative_pose_ransac(b1,
b2,
method,
threshold,
iterations=iterations,
probability=probability)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.relative_pose_ransac(b1, b2, method, threshold,
iterations)
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 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 test_outliers_line_LMedS():
a, b, x, samples = line_data()
sigma = 2.0
y = a * x + b + np.random.normal(scale=sigma, size=x.shape[0])
multiplier = 1.96
ratio_outliers = 0.4
outliers_max = 5.0
add_outliers(ratio_outliers, x, multiplier * sigma,
multiplier * outliers_max)
data = np.array([x, y]).transpose()
params = pyrobust.RobustEstimatorParams()
# can't be used with LMedS as an over-estimated sigma will make it stop early
params.use_iteration_reduction = False
result = pyrobust.ransac_line(data, multiplier, params,
pyrobust.RansacType.LMedS)
inliers_count = (1 - ratio_outliers) * samples
confidence = 0.95 # 1.96*MAD -> 95% rejecting inliers
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=(1 - confidence),
atol=8)
def test_outliers_line_msac():
a, b, x, samples = line_data()
sigma = 2.0
y = a * x + b + np.random.normal(scale=sigma, size=x.shape[0])
multiplier = 1.96
ratio_outliers = 0.4
outliers_max = 5.0
add_outliers(ratio_outliers, x, multiplier * sigma,
multiplier * outliers_max)
data = np.array([x, y]).transpose()
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_line(data, multiplier * sigma, params,
pyrobust.RansacType.MSAC)
inliers_count = (1 - ratio_outliers) * samples
confidence = 0.95 # 1.96*MAD -> 95% rejecting inliers
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=(1 - confidence),
atol=5)
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 relative_pose_ransac(b1, b2, threshold, iterations, probability):
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_pose(b1, b2, threshold, params, pyrobust.RansacType.RANSAC)
Rt = result.lo_model.copy()
R, t = Rt[:3, :3].copy(), Rt[:, 3].copy()
Rt[:3, :3] = R.T
Rt[:, 3] = -R.T.dot(t)
return Rt
def relative_pose_ransac_rotation_only(
b1: np.ndarray,
b2: np.ndarray,
threshold: float,
iterations: int,
probability: float,
) -> np.ndarray:
params = pyrobust.RobustEstimatorParams()
params.iterations = iterations
result = pyrobust.ransac_relative_rotation(b1, b2, threshold, params,
pyrobust.RansacType.RANSAC)
return result.lo_model.T
def test_uniform_line_ransac():
a, b, x, samples = line_data()
scale = 2.0
y = a * x + b + np.random.rand(x.shape[0]) * scale
data = np.array([x, y]).transpose()
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_line(data, scale, params,
pyrobust.RansacType.RANSAC)
assert result.score == samples
assert len(result.inliers_indices) == samples
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)
def test_uniform_essential_ransac(one_pair_and_its_E):
f1, f2, E, _ = one_pair_and_its_E
points = np.concatenate((f1, f2), axis=1)
scale = 1e-2
points += np.random.rand(*points.shape) * scale
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_essential(f1, f2, scale, params,
pyrobust.RansacType.RANSAC)
assert len(result.inliers_indices) == len(f1) == len(f2)
def relative_pose_ransac(
b1: np.ndarray,
b2: np.ndarray,
threshold: float,
iterations: int,
probability: float,
) -> np.ndarray:
params = pyrobust.RobustEstimatorParams()
params.iterations = iterations
result = pyrobust.ransac_relative_pose(b1, b2, threshold, params,
pyrobust.RansacType.RANSAC)
Rt = result.lo_model.copy()
R, t = Rt[:3, :3].copy(), Rt[:, 3].copy()
Rt[:3, :3] = R.T
Rt[:, 3] = -R.T.dot(t)
return Rt
def test_normal_line_msac():
a, b, x, samples = line_data()
sigma = 2.0
y = a * x + b + np.random.normal(scale=sigma, size=x.shape[0])
multiplier = 1.96
data = np.array([x, y]).transpose()
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_line(data, multiplier * sigma, params,
pyrobust.RansacType.MSAC)
confidence = 0.95 # 1.96*MAD -> 95% rejecting inliers
assert np.isclose(len(result.inliers_indices),
samples,
rtol=(1 - confidence),
atol=7)
def test_outliers_line_ransac():
a, b, x, samples = line_data()
scale = 2.0
y = a * x + b + np.random.rand(x.shape[0]) * scale
ratio_outliers = 0.4
outliers_max = 5.0
add_outliers(ratio_outliers, x, scale, outliers_max)
data = np.array([x, y]).transpose()
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_line(data, scale, params,
pyrobust.RansacType.RANSAC)
inliers_count = (1 - ratio_outliers) * samples
assert result.score == inliers_count
assert len(result.inliers_indices) == inliers_count
def test_uniform_essential_ransac(pairs_and_their_E):
for f1, f2, E, _ in pairs_and_their_E:
points = np.concatenate((f1, f2), axis=1)
scale = 1e-2
points += np.random.rand(*points.shape) * scale
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
scale_eps_ratio = 5e-1
params = pyrobust.RobustEstimatorParams()
params.use_iteration_reduction = False
result = pyrobust.ransac_essential(f1, f2,
scale * (1.0 + scale_eps_ratio),
params, pyrobust.RansacType.RANSAC)
assert len(result.inliers_indices) == len(f1) == len(f2)
def test_outliers_relative_rotation_ransac(pairs_and_their_E):
for f1, _, _, _ in pairs_and_their_E:
vec_x = np.random.rand(3)
vec_x /= np.linalg.norm(vec_x)
vec_y = np.array([-vec_x[1], vec_x[0], 0.])
vec_y /= np.linalg.norm(vec_y)
vec_z = np.cross(vec_x, vec_y)
rotation = np.array([vec_x, vec_y, vec_z])
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 = [rotation.dot(x) for x in f1]
points = np.concatenate((f1, f2), axis=1)
scale = 1e-3
points += np.random.rand(*points.shape) * scale
ratio_outliers = 0.3
add_outliers(ratio_outliers, points, 0.1, 1.0)
f1, f2 = points[:, 0:3], points[:, 3:6]
f1 /= np.linalg.norm(f1, axis=1)[:, None]
f2 /= np.linalg.norm(f2, axis=1)[:, None]
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_rotation(f1, f2,
np.sqrt(3 * scale * scale),
params,
pyrobust.RansacType.RANSAC)
tolerance = 0.04
inliers_count = (1 - ratio_outliers) * len(points)
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=tolerance)
assert np.linalg.norm(rotation - result.lo_model, ord='fro') < 8e-2
def relative_pose_ransac_rotation_only(b1, b2, threshold, iterations,
probability):
# in-house estimation
if in_house_multiview:
threshold = np.arccos(1 - threshold)
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_relative_rotation(b1, b2, threshold, params,
pyrobust.RansacType.RANSAC)
return result.lo_model.T
else:
try:
return pyopengv.relative_pose_ransac_rotation_only(
b1,
b2,
threshold,
iterations=iterations,
probability=probability)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.relative_pose_ransac_rotation_only(
b1, b2, threshold, iterations)
def test_outliers_similarity_ransac() -> None:
rotation, translation, scale, x, samples = similarity_data()
similarity = pygeometry.Similarity(rotation, translation, scale)
y = np.array([similarity.transform(p) for p in x])
sigma = 0.001
y += np.random.normal(scale=sigma, size=y.shape)
outliers_max = 1.0
ratio_outliers = 0.3
add_outliers(ratio_outliers, x, scale, outliers_max)
params = pyrobust.RobustEstimatorParams()
result = pyrobust.ransac_similarity(x, y, 0.1, params,
pyrobust.RansacType.RANSAC)
inliers_count = (1 - ratio_outliers) * samples
confidence = 0.95 # 1.96*MAD -> 95% rejecting inliers
assert np.isclose(len(result.inliers_indices),
inliers_count,
rtol=(1 - confidence),
atol=8)
