1# Open3D: www.open3d.org
2# The MIT License (MIT)
3# See license file or visit www.open3d.org for details
4
5from open3d import *
6import os, sys
7sys.path.append("../Utility")
8from common import *
9sys.path.append("../Advanced")
10from trajectory_io import *
11from shutil import copyfile
12
13if __name__ == "__main__":
14    set_verbosity_level(VerbosityLevel.Debug)
15
16    path = "[path_to_reconstruction_system_output]"
17    out_path = "[path_to_sampled_frames_are_located]"
18    make_folder(out_path)
19    make_folder(out_path + "depth/")
20    make_folder(out_path + "image/")
21    make_folder(out_path + "scene/")
22    sampling_rate = 30
23
24    depth_image_path = get_file_list(
25            os.path.join(path, "depth/"), extension = ".png")
26    color_image_path = get_file_list(
27            os.path.join(path, "image/"), extension = ".jpg")
28    pose_graph_global = read_pose_graph(path +
29            template_global_posegraph_optimized)
30    n_fragments = len(depth_image_path) // n_frames_per_fragment + 1
31    pose_graph_fragments = []
32    for i in range(n_fragments):
33        pose_graph_fragment = read_pose_graph(path +
34                template_fragment_posegraph_optimized % i)
35        pose_graph_fragments.append(pose_graph_fragment)
36
37    depth_image_path_new = []
38    color_image_path_new = []
39    traj = []
40    cnt = 0
41    for i in range(len(depth_image_path)):
42        if i % sampling_rate == 0:
43            metadata = [cnt, cnt, len(depth_image_path) // sampling_rate + 1]
44            print(metadata)
45            fragment_id = i // n_frames_per_fragment
46            local_frame_id = i - fragment_id * n_frames_per_fragment
47            traj.append(CameraPose(metadata, np.dot(
48                    pose_graph_global.nodes[fragment_id].pose,
49                    pose_graph_fragments[fragment_id].nodes[local_frame_id].pose)))
50            copyfile(depth_image_path[i], out_path + "depth/" + \
51                    os.path.basename(depth_image_path[i]))
52            copyfile(color_image_path[i], out_path + "image/" + \
53                    os.path.basename(color_image_path[i]))
54            cnt += 1
55    copyfile(path + "/scene/cropped.ply",
56            out_path + "/scene/integrated.ply")
57    write_trajectory(traj, out_path + "scene/key.log")
58