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)