-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathframe_utils.py
140 lines (113 loc) · 5.53 KB
/
frame_utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
import cv2
import json
import numpy as np
import open3d as o3d
from PIL import Image
import shutil
import os, sys
dir_path = os.path.dirname(os.path.realpath(__file__))
sys.path.append(os.path.join(dir_path, ".."))
def validate_extension(ext):
if ext not in ["jpg", "png"]:
raise "Invalid Extension! {0}".format(ext)
def write_bgr(frames_dir, frame_id, frame, ext):
validate_extension(ext)
frame_name = os.path.join(frames_dir, str(frame_id).zfill(5) + "_color.{0}".format(ext))
cv2.imwrite(frame_name, frame)
def load_bgr(frames_dir, frame_id, ext):
validate_extension(ext)
frame = cv2.imread(os.path.join(frames_dir, str(frame_id).zfill(5) + "_color.{0}".format(ext)), cv2.IMREAD_COLOR)
return frame
def write_rgb(frames_dir, frame_id, frame, ext):
validate_extension(ext)
frame = Image.fromarray(frame)
frame_name = os.path.join(frames_dir, str(frame_id).zfill(5) + "_color.{0}".format(ext))
frame.save(frame_name)
def write_debug_rgb(frames_dir, frame_id, frame, ext):
validate_extension(ext)
frame = Image.fromarray(frame)
frame_name = os.path.join(frames_dir, str(frame_id).zfill(5) + "_color_debug.{0}".format(ext))
frame.save(frame_name)
def load_rgb(frames_dir, frame_id, ext):
validate_extension(ext)
frame = np.array(Image.open(os.path.join(frames_dir, str(frame_id).zfill(5) + "_color.{0}".format(ext))))
return frame
def transfer_color(old_frames_dir, old_frame_id, old_ext, new_frames_dir, new_frame_id, new_ext):
validate_extension(old_ext)
validate_extension(new_ext)
old_frame_name = os.path.join(old_frames_dir, str(old_frame_id).zfill(5) + "_color.{0}".format(old_ext))
new_frame_name = os.path.join(new_frames_dir, str(new_frame_id).zfill(5) + "_color.{0}".format(new_ext))
if old_ext == new_ext:
shutil.copyfile(old_frame_name, new_frame_name)
else:
img = cv2.imread(old_frame_name, cv2.IMREAD_UNCHANGED)
cv2.imwrite(new_frame_name, img)
def get_color_ext(frames_dir):
frames = os.listdir(frames_dir)
color_frames = [f for f in frames if "color" in f]
color_exts = [f[f.rfind(".") + 1:] for f in color_frames]
if color_exts.count(color_exts[0]) != len(color_exts):
raise("Not all same extension!")
validate_extension(color_exts[0])
return color_exts[0]
def write_depth(frames_dir, frame_id, frame):
assert(frame.dtype == np.uint16)
frame_name = os.path.join(frames_dir, str(frame_id).zfill(5) + "_depth.png")
cv2.imwrite(frame_name, frame)
def load_depth(frames_dir, frame_id):
frame = cv2.imread(os.path.join(frames_dir, str(frame_id).zfill(5) + "_depth.png"), cv2.IMREAD_UNCHANGED)
return frame
def transfer_depth(old_frames_dir, old_frame_id, new_frames_dir, new_frame_id):
old_frame_name = os.path.join(old_frames_dir, str(old_frame_id).zfill(5) + "_depth.png")
new_frame_name = os.path.join(new_frames_dir, str(new_frame_id).zfill(5) + "_depth.png")
shutil.copyfile(old_frame_name, new_frame_name)
def load_o3d_rgb(frames_dir, frame_id, ext):
validate_extension(ext)
path = os.path.join(frames_dir, str(frame_id).zfill(5) + "_color.{0}".format(ext))
frame = o3d.io.read_image(path)
return frame
def load_o3d_depth(frames_dir, frame_id):
path = os.path.join(frames_dir, str(frame_id).zfill(5) + "_depth.png")
frame = o3d.io.read_image(path)
return frame
def write_label(frames_dir, frame_id, frame):
frame_name = os.path.join(frames_dir, str(frame_id).zfill(5) + "_label.png")
cv2.imwrite(frame_name, frame)
def write_debug_label(frames_dir, frame_id, frame):
frame_name = os.path.join(frames_dir, str(frame_id).zfill(5) + "_label_debug.png")
cv2.imwrite(frame_name, frame)
def load_label(frames_dir, frame_id):
frame = cv2.imread(os.path.join(frames_dir, str(frame_id).zfill(5) + "_label.png"), cv2.IMREAD_UNCHANGED)
return frame
def write_meta(frames_dir, frame_id, meta):
meta_file = os.path.join(frames_dir, str(frame_id).zfill(5) + "_meta.json")
with open(meta_file, "w") as f:
json.dump(meta, f)
def load_meta(frames_dir, frame_id):
meta_file = os.path.join(frames_dir, str(frame_id).zfill(5) + "_meta.json")
with open(meta_file, "r") as f:
meta = json.load(f)
return meta
def calculate_aruco_from_bgr_and_depth(bgr, depth, depth_scale, camera_matrix, camera_dist, aruco_dictionary, aruco_parameters):
# Detect the markers in the image
corners, ids, _ = cv2.aruco.detectMarkers(bgr, aruco_dictionary, parameters=aruco_parameters)
if np.all(ids is not None): # If there are markers found by detector
assert(len(ids) == 1)
# Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners[0], 0.02, camera_matrix,
camera_dist)
#1x3
rvec = np.squeeze(rvec, axis = 1)
#1x3
tvec = np.squeeze(tvec, axis = 1) * 9
#refine with Azure Kinect depth
center_projected, _ = cv2.projectPoints(tvec, np.array([[0, 0, 0]]).astype(np.float32), np.array([0, 0, 0]).astype(np.float32), camera_matrix, camera_dist)
center_projected = center_projected.squeeze()
center_x, center_y = center_projected
center_x, center_y = int(center_x), int(center_y)
center_depth = np.array([depth[center_y, center_x] * depth_scale])
center_pt = tvec / tvec[0, 2] * center_depth
tvec = center_pt
return rvec, tvec, corners
else:
return None