class Reconstruction:
__imageHeight = 480
__imageWidth = 640
__mtx = np.array([[620.423, 0, 327.327], [0, 620.95, 239.649], [0, 0, 1]])
__Ms_keyframe = {}
__Ms = []
scalingFactor = 5000
__max_depth = 0.6
def __init__(self, frame_doc, out):
self.__frame = frame_doc
self.__out_doc = out
self.drawer = Drawer()
def set_max_depth(self, num):
self.__max_depth = num
def load_camera_pose(self, traj_doc):
keyframe_file = open(traj_doc + '/KeyFrameTrajectoryMatrix.txt', 'r')
keyframe_matrix_lines = keyframe_file.readlines()
for matrix_line in keyframe_matrix_lines:
matrix_list = matrix_line.split()
matrix = np.zeros((3, 4))
rr = np.zeros((3, 3))
matrix[:, 3] = [matrix_list[1], matrix_list[2], matrix_list[3]]
rr[0, 0:3] = [matrix_list[4], matrix_list[5], matrix_list[6]]
rr[1, 0:3] = [matrix_list[7], matrix_list[8], matrix_list[9]]
rr[2, 0:3] = [matrix_list[10], matrix_list[11], matrix_list[12]]
matrix[:, 3] = matrix[:, 3] / 0.001
matrix[0:3, 0:3] = rr.T
self.__Ms_keyframe[matrix_list[0]] = matrix
print(" Camera pose matirx loaded.")
def load_camera_pose_tmu(self, traj_doc):
keyframe_file = open(traj_doc + '/KeyFrameTrajectory.txt', 'r')
keyframe_matrix_lines = keyframe_file.readlines()
for matrix_line in keyframe_matrix_lines:
matrix_list = matrix_line.split()
matrix = transform34(matrix_list)
self.__Ms_keyframe[matrix_list[0]] = matrix
print(" Camera pose matirx loaded.")
"""
def vert_new(self, img,D, T, if_inv = False, rate = 1):
l = img.shape[0]
h = img.shape[1]
t = 0
temp = np.zeros((l*h, 6))
T = np.concatenate((T, np.array([[0,0,0,1]])), axis=0)
if if_inv:
T = np.linalg.inv(T)
for i in range(0,l,rate):
for j in range(0,h,rate):
dep = D[i,j]
if 0.1 < dep < self.__max_depth:
#camera caliberation
img_p = np.array([i, j, 1])
real_p = np.dot(np.linalg.inv(self.__mtx),(img_p)*dep)
real_p = np.append(real_p, 1)
#real_p[1] = - real_p[1]
aa = np.dot(T, real_p).reshape(1,4)
temp[t,0:3] = aa[0,0:3]/aa[0,3]
#temp[t,1] = -temp[t,1]
temp[t,3:6] = img[i,j]
t = t+1
return temp[0:t,:]
"""
def vert_new(self,
rgb_file,
depth_file,
transform,
downsample=1,
pcd=False):
rgb = Image.open(rgb_file)
depth = Image.open(depth_file)
points = []
for v in range(0, rgb.size[1], downsample):
for u in range(0, rgb.size[0], downsample):
color = rgb.getpixel((u, v))
Z = depth.getpixel((u, v)) / self.scalingFactor
if Z == 0 or Z > self.__max_depth: continue
X = (u - self.__mtx[0, 2]) * Z / self.__mtx[0, 0]
Y = (v - self.__mtx[1, 2]) * Z / self.__mtx[1, 1]
vec_org = np.matrix([[X], [Y], [Z], [1]])
if pcd:
points.append(
struct.pack(
"fffI", vec_org[0, 0], vec_org[1, 0],
vec_org[2, 0], color[0] * 2**16 + color[1] * 2**8 +
color[2] * 2**0))
else:
vec_transf = np.dot(transform, vec_org)
points.append(
"%f %f %f %d %d %d 0\n" %
(vec_transf[0, 0], vec_transf[1, 0], vec_transf[2, 0],
color[0], color[1], color[2]))
return points
def write_ply_list(self, ply_file, points):
file = open(ply_file, "w")
file.write('''ply
format ascii 1.0
element vertex %d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
property uchar alpha
end_header
%s
''' % (len(points), "".join(points)))
file.close()
#print "Saved %d points to '%s'"%(len(points),ply_file)
def write_ply_array(self, fn, verts):
ply_header = '''ply
format ascii 1.0
element vertex %(vert_num)d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
'''
with open(fn, 'wb') as f:
f.write((ply_header % dict(vert_num=len(verts))).encode('utf-8'))
np.savetxt(f, verts, fmt='%f %f %f %d %d %d')
def make_ply(self, rate=1):
length_m = len(self.__Ms_keyframe)
bar = progressbar.ProgressBar(maxval=length_m, \
widgets=[progressbar.Bar('=', '[', ']'), ' ', progressbar.Percentage()])
bar.start()
i = 0
idx_old = 0
for idx, matrix in self.__Ms_keyframe.items():
idx = int(float(idx))
if idx > idx_old + 10 or idx_old == 0:
idx1 = idx
#image = cv2.imread(self.__frame + '/rgb/' + str(idx).zfill(6) + '.png')
#depth = cv2.imread(self.__frame + '/depth/' + str(idx).zfill(6) + '.png')[:,:,0]
#depth = np.load(self.__frame + '/depth_npy/' + str(idx).zfill(6) + '.npy')
#if idx > 350:
# idx1 = idx + 651
image = self.__frame + '/rgb/' + str(idx1).zfill(6) + '.png'
depth = self.__frame + '/depth/' + str(idx1).zfill(6) + '.png'
points = self.vert_new(image, depth, matrix, rate)
self.write_ply_list(self.__out_doc + '/' + str(idx) + '.ply',
points)
idx_old = int(idx)
#print(' finished one image')
bar.update(i + 1)
i = i + 1
bar.finish()
#print('----- finished -----')
def draw_camera(self):
res0 = np.zeros((1, 6))
poss0 = np.zeros(3)
for idx, matrix in self.__Ms_keyframe.items():
res, poss1 = self.drawer.draw_camera(matrix)
res0 = np.concatenate((res0, res), axis=0)
""""
for idx in self.main_loop_idx:
Mss = self.Ms_multi[idx]
for M in Mss:
res, poss1 = self.drawer.draw_camera(M)
lene = self.drawer.draw_line(poss0, poss1)
poss0 = poss1
res0 = np.concatenate((res0,res,lene), axis=0)
"""
self.write_ply_array(self.__out_doc + '/camera.ply', res0)
print(" --- camera drawn")
return 0
