From 2c910bc5bda8a8c8d5e54f9144741c6615f200cb Mon Sep 17 00:00:00 2001 From: Ting-Jun Wang Date: Mon, 18 Dec 2023 14:25:19 +0800 Subject: [PATCH] fix: complete OpenCV version --- main.py | 46 +++++++++++++++++++++++----------------------- pokemon.py | 29 ++++++++++++++++++++++------- 2 files changed, 45 insertions(+), 30 deletions(-) diff --git a/main.py b/main.py index 0ab84ec..63e67f0 100644 --- a/main.py +++ b/main.py @@ -104,7 +104,7 @@ def visualization(inverse_matrix_M, pokemon): coordinate_line_set = o3d.geometry.LineSet() coordinate_line_set.points = o3d.utility.Vector3dVector(np.array([[0, 0, 0],[10, 0, 0], [0, 10, 0], [0, 0, 10]])) coordinate_line_set.lines = o3d.utility.Vector2iVector(np.array([[0, 1], [0, 2], [0, 3]])) - coordinate_line_set.colors = o3d.utility.Vector3dVector(np.array([[0, 255, 0], [0, 255, 0],[ 0, 255, 0]])) + coordinate_line_set.colors = o3d.utility.Vector3dVector(np.array([[255, 0, 0], [0, 255, 0],[ 0, 0, 255]])) vis.add_geometry(coordinate_line_set) # 化紙張三角形 @@ -113,10 +113,11 @@ def visualization(inverse_matrix_M, pokemon): paper_triangle_set.triangles = o3d.utility.Vector3iVector(paper_triangles) vis.add_geometry(paper_triangle_set) - # Pokemon model pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(pokemon.get_position()) + pokemon_point = pokemon.get_position()[:, :3] + pokemon_point[:, 2] = -pokemon_point[:, 2] + pcd.points = o3d.utility.Vector3dVector(pokemon_point) pcd.colors = o3d.utility.Vector3dVector(pokemon.get_colors()) vis.add_geometry(pcd) @@ -141,6 +142,7 @@ def visualization(inverse_matrix_M, pokemon): location[2] = -location[2] points.append(location) + # 方框的線 lines.append(np.array([ start_index+1, start_index+2 ])) lines.append(np.array([ start_index+2, start_index+3 ])) @@ -197,26 +199,20 @@ def render_pokemon(frame, M, camera_position, pokemon): pokemon_points = pokemon_points[sorted_indices] pokemon_colors = pokemon_colors[sorted_indices] - - for index in range(pokemon_points.shape[0]): - pokemon_color = [int(i*255) for i in pokemon_colors[index]] - # print(pokemon_color) - # print(pokemon_points[index]) - pokemon_point = CAMERA_MATRIX @ M - # print(pokemon_point.shape) - # print(pokemon_points.shape) - pokemon_point = pokemon_point @ (pokemon_points[index].reshape((4, 1))) - pokemon_point = pokemon_point[:2] / pokemon_point[2] - pokemon_point = [int(i) for i in pokemon_point] - # print(pokemon_point) - - # print(frame.shape) - - if pokemon_point[0] >= 0 and pokemon_point[0] <= frame.shape[0] and pokemon_point[1] >= 0 and pokemon_point[1] <= frame.shape[1]: - frame = cv2.circle(frame, pokemon_point, 5, pokemon_color, thickness=-1) - + M = CAMERA_MATRIX.dot(M) + pokemon_points = (M @ (pokemon_points.T)).T + for point , color in zip(pokemon_points, pokemon_colors): + color = [int(i*255) for i in color] + color = np.array(color, dtype=int) + color = color[0:3] + point2D = point[:2] / point[2] + point2D = np.int_(point2D[::-1]) + if 0 <= point2D[0] < frame.shape[1] and 0 <= point2D[1] < frame.shape[0]: + frame = cv2.circle(frame, tuple(point2D), 10,color.astype(int).tolist(), thickness=-1) return frame + + if __name__ == '__main__': # 設定連接到 Android 手機的相機 @@ -264,6 +260,7 @@ if __name__ == '__main__': if previous_approx == []: print("INITIAL") previous_approx = initial_approx(approx) + print(previous_approx) new_approx = get_new_approx(approx, previous_approx) previous_approx = new_approx @@ -280,20 +277,23 @@ if __name__ == '__main__': rotation_vectors.append(rotation_matrix) translation_vectors.append(translation_vector) + ''' print("R:", rotation_matrix) print("t:", translation_vector) print() + ''' R = rotation_matrix t = translation_vector rt = np.concatenate((R, t), axis=1) + # print('rt: ', rt) M = np.concatenate((rt, [[0, 0, 0, 1]]), axis=0) M_inv = np.linalg.inv(M) camera_position = M_inv @ np.array([0, 0, 0, 1]) camera_position[2] = -camera_position[2] + # print("CAMERA: ", camera_position) frame = render_pokemon(frame, rt, camera_position, pokemon) - cv2.imwrite('test.jpg', frame) # 顯示結果 @@ -305,7 +305,7 @@ if __name__ == '__main__': cv2.imshow('Paper Detection', frame) # 按下 'q' 鍵退出迴圈 - key = cv2.waitKey(17) & 0xFF + key = cv2.waitKey(5) & 0xFF if key == ord('q'): # 等待 33ms (1秒 = 1000ms, 1秒顯示幀) break if key == ord(' '): diff --git a/pokemon.py b/pokemon.py index f5533ad..19624e5 100644 --- a/pokemon.py +++ b/pokemon.py @@ -1,4 +1,4 @@ - +import open3d as o3d import numpy as np class Pokemon(): @@ -8,8 +8,8 @@ class Pokemon(): self.scalar = 10 - self.x = 8 - self.y = 13 + self.x = 4 + self.y = 20 self.z = 8 self.cloudpoints = { @@ -26,23 +26,38 @@ class Pokemon(): self.cloudpoints['colors'] = self.cloudpoints['colors'] / 255 self.cloudpoints['colors'] = self.cloudpoints['colors'][:, :3] + # down sample + point_cloud = o3d.geometry.PointCloud() + point_cloud.points = o3d.utility.Vector3dVector(self.cloudpoints['points']) + point_cloud.colors = o3d.utility.Vector3dVector(self.cloudpoints['colors']) + downsampled_cloud = point_cloud.uniform_down_sample(every_k_points=5) + downsampled_points = np.asarray(downsampled_cloud.points) + downsampled_colors = np.asarray(downsampled_cloud.colors) - def get_position(self): + self.cloudpoints['points'] = downsampled_points + self.cloudpoints['colors'] = downsampled_colors + + self.cal_position() + + def cal_position(self): transform_matrix = np.array([ [self.scalar, 0, 0, 0], [0, 0, self.scalar, 0], [0, self.scalar, 0, 0], - [self.x, self.y, self.z, 0]]) # shift (前後, 左右, 高度) + [self.x, self.y, self.z, 1]]) # shift (前後, 左右, 高度) pokemon_points = self.cloudpoints['points'] ones = np.ones((pokemon_points.shape[0], 1)) pokemon_points = np.concatenate([pokemon_points, ones], axis=1) pokemon_points = pokemon_points @ transform_matrix - # pokemon_points = pokemon_points[:, :3] + pokemon_points[:, 2] = -pokemon_points[:, 2] + self.cloudpoints['points'] = pokemon_points - return pokemon_points + def get_position(self): + + return self.cloudpoints['points'] def get_colors(self): return self.cloudpoints['colors']