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