def teaser_pp_registration( xyz, xyz_target, noise_bound=0.05, cbar2=1, rotation_gnc_factor=1.4, rotation_max_iterations=100, rotation_cost_threshold=1e-12, ): assert xyz.shape == xyz_target.shape import teaserpp_python # Populating the parameters solver_params = teaserpp_python.RobustRegistrationSolver.Params() solver_params.cbar2 = cbar2 solver_params.noise_bound = noise_bound solver_params.estimate_scaling = False solver_params.rotation_estimation_algorithm = ( teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM. GNC_TLS) solver_params.rotation_gnc_factor = rotation_gnc_factor solver_params.rotation_max_iterations = rotation_max_iterations solver_params.rotation_cost_threshold = rotation_cost_threshold solver = teaserpp_python.RobustRegistrationSolver(solver_params) solver.solve(xyz.T.detach().cpu().numpy(), xyz_target.T.detach().cpu().numpy()) solution = solver.getSolution() T_res = torch.eye(4, device=xyz.device) T_res[:3, :3] = torch.from_numpy(solution.rotation).to(xyz.device) T_res[:3, 3] = torch.from_numpy(solution.translation).to(xyz.device) return T_res
def execute_teaser_global_registration(source, target): """ Use TEASER++ to perform global registration """ # Prepare TEASER++ Solver solver_params = teaserpp_python.RobustRegistrationSolver.Params() solver_params.cbar2 = 1 solver_params.noise_bound = NOISE_BOUND solver_params.estimate_scaling = False solver_params.rotation_estimation_algorithm = ( teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM. GNC_TLS) solver_params.rotation_gnc_factor = 1.4 solver_params.rotation_max_iterations = 100 solver_params.rotation_cost_threshold = 1e-12 print("TEASER++ Parameters are:", solver_params) teaserpp_solver = teaserpp_python.RobustRegistrationSolver(solver_params) # Solve with TEASER++ start = timer() teaserpp_solver.solve(source, target) end = timer() est_solution = teaserpp_solver.getSolution() est_mat = bench_utils.compose_mat4_from_teaserpp_solution(est_solution) max_clique = teaserpp_solver.getTranslationInliersMap() print("Max clique size:", len(max_clique)) final_inliers = teaserpp_solver.getTranslationInliers() return est_mat, max_clique, end - start
def __init__(self): self.NOISE_BOUND = 0.1 # 0.05 self.solver_params = teaserpp_python.RobustRegistrationSolver.Params() self.solver_params.cbar2 = 0.6 # 1 self.solver_params.noise_bound = self.NOISE_BOUND self.solver_params.estimate_scaling = False self.solver_params.rotation_estimation_algorithm = \ teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS self.solver_params.rotation_gnc_factor = 1.4 self.solver_params.rotation_max_iterations = 200 self.solver_params.rotation_cost_threshold = 1e-12 self.solver = teaserpp_python.RobustRegistrationSolver( self.solver_params)
def get_teaser_solver(noise_bound): solver_params = teaserpp_python.RobustRegistrationSolver.Params() solver_params.cbar2 = 1.0 solver_params.noise_bound = noise_bound solver_params.estimate_scaling = False solver_params.inlier_selection_mode = \ teaserpp_python.RobustRegistrationSolver.INLIER_SELECTION_MODE.PMC_EXACT solver_params.rotation_tim_graph = \ teaserpp_python.RobustRegistrationSolver.INLIER_GRAPH_FORMULATION.CHAIN solver_params.rotation_estimation_algorithm = \ teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS solver_params.rotation_gnc_factor = 1.4 solver_params.rotation_max_iterations = 10000 solver_params.rotation_cost_threshold = 1e-16 solver = teaserpp_python.RobustRegistrationSolver(solver_params) return solver
for i in range(outlier_indices.size): shift = OUTLIER_TRANSLATION_LB + np.random.rand( 3, 1) * (OUTLIER_TRANSLATION_UB - OUTLIER_TRANSLATION_LB) dst[:, outlier_indices[i]] += shift.squeeze() # Populating the parameters solver_params = teaserpp_python.RobustRegistrationSolver.Params() solver_params.cbar2 = 1 solver_params.noise_bound = NOISE_BOUND solver_params.estimate_scaling = False solver_params.rotation_estimation_algorithm = teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS solver_params.rotation_gnc_factor = 1.4 solver_params.rotation_max_iterations = 100 solver_params.rotation_cost_threshold = 1e-12 solver = teaserpp_python.RobustRegistrationSolver(solver_params) start = time.time() solver.solve(src, dst) end = time.time() solution = solver.getSolution() print("=====================================") print(" TEASER++ Results ") print("=====================================") print("Expected rotation: ") print(T[:3, :3]) print("Estimated rotation: ") print(solution.rotation) print("Error (rad): ")