def triangulate_ransac(self, proj_matricies, points, n_iters=10, reprojection_error_epsilon=15, direct_optimization=True): assert len(proj_matricies) == len(points) assert len(points) >= 2 proj_matricies = np.array(proj_matricies) points = np.array(points) n_views = len(points) # determine inliers view_set = set(range(n_views)) inlier_set = set() for i in range(n_iters): sampled_views = sorted(random.sample(view_set, 2)) keypoint_3d_in_base_camera = multiview.triangulate_point_from_multiple_views_linear( proj_matricies[sampled_views], points[sampled_views]) reprojection_error_vector = multiview.calc_reprojection_error_matrix( np.array([keypoint_3d_in_base_camera]), points, proj_matricies)[0] new_inlier_set = set(sampled_views) for view in view_set: current_reprojection_error = reprojection_error_vector[view] if current_reprojection_error < reprojection_error_epsilon: new_inlier_set.add(view) if len(new_inlier_set) > len(inlier_set): inlier_set = new_inlier_set # triangulate using inlier_set if len(inlier_set) == 0: inlier_set = view_set.copy() inlier_list = np.array(sorted(inlier_set)) inlier_proj_matricies = proj_matricies[inlier_list] inlier_points = points[inlier_list] keypoint_3d_in_base_camera = multiview.triangulate_point_from_multiple_views_linear( inlier_proj_matricies, inlier_points) reprojection_error_vector = multiview.calc_reprojection_error_matrix( np.array([keypoint_3d_in_base_camera]), inlier_points, inlier_proj_matricies)[0] reprojection_error_mean = np.mean(reprojection_error_vector) keypoint_3d_in_base_camera_before_direct_optimization = keypoint_3d_in_base_camera reprojection_error_before_direct_optimization = reprojection_error_mean # direct reprojection error minimization if direct_optimization: def residual_function(x): reprojection_error_vector = multiview.calc_reprojection_error_matrix( np.array([x]), inlier_points, inlier_proj_matricies)[0] residuals = reprojection_error_vector return residuals x_0 = np.array(keypoint_3d_in_base_camera) res = least_squares(residual_function, x_0, loss='huber', method='trf') keypoint_3d_in_base_camera = res.x reprojection_error_vector = multiview.calc_reprojection_error_matrix( np.array([keypoint_3d_in_base_camera]), inlier_points, inlier_proj_matricies)[0] reprojection_error_mean = np.mean(reprojection_error_vector) return keypoint_3d_in_base_camera, inlier_list
def residual_function(x): reprojection_error_vector = multiview.calc_reprojection_error_matrix( np.array([x]), inlier_points, inlier_proj_matricies)[0] residuals = reprojection_error_vector return residuals