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): ")