1# Open3D: www.open3d.org 2# The MIT License (MIT) 3# See license file or visit www.open3d.org for details 4 5import sys 6sys.path.append("../Utility") 7from open3d import * 8from common import * 9 10def run_posegraph_optimization(pose_graph_name, pose_graph_optmized_name, 11 max_correspondence_distance, preference_loop_closure): 12 # to display messages from global_optimization 13 set_verbosity_level(VerbosityLevel.Debug) 14 method = GlobalOptimizationLevenbergMarquardt() 15 criteria = GlobalOptimizationConvergenceCriteria() 16 option = GlobalOptimizationOption( 17 max_correspondence_distance = max_correspondence_distance, 18 edge_prune_threshold = 0.25, 19 preference_loop_closure = preference_loop_closure, 20 reference_node = 0) 21 pose_graph = read_pose_graph(pose_graph_name) 22 global_optimization(pose_graph, method, criteria, option) 23 write_pose_graph(pose_graph_optmized_name, pose_graph) 24 set_verbosity_level(VerbosityLevel.Error) 25 26 27def optimize_posegraph_for_fragment(path_dataset, fragment_id): 28 pose_graph_name = path_dataset + template_fragment_posegraph % fragment_id 29 pose_graph_optmized_name = path_dataset + \ 30 template_fragment_posegraph_optimized % fragment_id 31 run_posegraph_optimization(pose_graph_name, pose_graph_optmized_name, 32 max_correspondence_distance = 0.07, 33 preference_loop_closure = 0.1) 34 35 36def optimize_posegraph_for_scene(path_dataset): 37 pose_graph_name = path_dataset + template_global_posegraph 38 pose_graph_optmized_name = path_dataset + \ 39 template_global_posegraph_optimized 40 run_posegraph_optimization(pose_graph_name, pose_graph_optmized_name, 41 max_correspondence_distance = 0.07, 42 preference_loop_closure = 2.0) 43