From d8a30447965e3c4d5622475a89a97837cf10d53d Mon Sep 17 00:00:00 2001 From: Ting-Jun Wang Date: Mon, 18 Dec 2023 15:57:54 +0800 Subject: [PATCH] feat: pokemon can move --- main.py | 42 ++++++++++++++++++++++++++++++++++-------- pokemon.py | 31 +++++++++++++++++++++++-------- 2 files changed, 57 insertions(+), 16 deletions(-) diff --git a/main.py b/main.py index 63e67f0..cd5de8c 100644 --- a/main.py +++ b/main.py @@ -2,11 +2,14 @@ import numpy as np import open3d as o3d import cv2 from pokemon import Pokemon +import random PAPER_WCS_POINT = np.array([[0, 0, 0], [18.5, 0, 0], [0, 26, 0], [18.5, 26, 0]], dtype=np.float32) CAMERA_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['K'] DISTORTION_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['dist'] +types = ['fire', 'water', 'electric', 'rock', 'grass', 'ice', 'steel'] +animals = ['cat', 'dog', 'rat', 'rabbit', 'dragon', 'duck', 'turtle', 'butterfly', 'monkey', 'bee', 'fox', 'flower', 'horse', 'jellyfish', 'snake', 'Tyrannosaurus', 'dinosaur', 'fish', 'whale', 'bat', 'bear', 'deer', 'pig', 'eagle', 'chicken'] def get_new_approx(approx, previous_approx): def distance(point, pre_point): @@ -68,6 +71,12 @@ def initial_approx(approx): return new_approx +def new_pokemon(): + new_type = types[ random.randint(0, len(types)-1) ] + new_animal = animals[ random.randint(0, len(animals)-1) ] + print(new_type, new_animal) + return Pokemon(new_type, new_animal) + def plot_corner_points(frame, approx): y, x = approx[0][0] cv2.circle(frame, (x, y), 15, (0, 0, 255), -1) # 在角點位置畫紅色圓圈 @@ -207,7 +216,7 @@ def render_pokemon(frame, M, camera_position, pokemon): 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]: + if 0 <= point2D[0] and point2D[0] < frame.shape[1] and 0 <= point2D[1] and point2D[1] < frame.shape[0]: frame = cv2.circle(frame, tuple(point2D), 10,color.astype(int).tolist(), thickness=-1) return frame @@ -216,12 +225,13 @@ def render_pokemon(frame, M, camera_position, pokemon): if __name__ == '__main__': # 設定連接到 Android 手機的相機 - # cap = cv2.VideoCapture(0) # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1 - cap = cv2.VideoCapture('demo1.mp4') + cap = cv2.VideoCapture(0) # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1 + # cap = cv2.VideoCapture('demo1.mp4') previous_approx = [] - pokemon = Pokemon('water', 'cat') + + pokemon = new_pokemon() rotation_vectors = [] translation_vectors = [] @@ -267,8 +277,8 @@ if __name__ == '__main__': paper_ccs_point = np.concatenate(new_approx, axis=0, dtype=np.float32) # 畫邊緣 & 四點 - plot_corner_points(frame, new_approx) - cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2) # 繪製輪廓 + # plot_corner_points(frame, new_approx) + # cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2) # 繪製輪廓 # 算 rotaion & translation success, rotation_vector, translation_vector = cv2.solvePnP(PAPER_WCS_POINT, paper_ccs_point, \ @@ -290,7 +300,7 @@ if __name__ == '__main__': 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] + # camera_position[2] = -camera_position[2] # print("CAMERA: ", camera_position) frame = render_pokemon(frame, rt, camera_position, pokemon) @@ -305,18 +315,33 @@ if __name__ == '__main__': cv2.imshow('Paper Detection', frame) # 按下 'q' 鍵退出迴圈 - key = cv2.waitKey(5) & 0xFF + key = cv2.waitKey(33) & 0xFF if key == ord('q'): # 等待 33ms (1秒 = 1000ms, 1秒顯示幀) break if key == ord(' '): print("Clear previous approxes") + pokemon = new_pokemon() previous_approx = [] + if key == ord('w'): + print("walk forward") + pokemon.walk_forward() + if key == ord('s'): + print("walk back") + pokemon.walk_backward() + if key == ord('a'): + print("walk left") + pokemon.walk_left() + if key == ord('d'): + print("walk right") + pokemon.walk_right() + cap.release() cv2.destroyAllWindows() # 畫 camera pose + ''' M_inv = [] for index in range(len(rotation_vectors)): R = rotation_vectors[index] @@ -327,6 +352,7 @@ if __name__ == '__main__': # load Pokemon points visualization(M_inv, pokemon) + ''' diff --git a/pokemon.py b/pokemon.py index 19624e5..db0b181 100644 --- a/pokemon.py +++ b/pokemon.py @@ -6,11 +6,10 @@ class Pokemon(): self.type = pokemon_type self.animal = animal - self.scalar = 10 - - self.x = 4 - self.y = 20 - self.z = 8 + self.scalar = 8 + self.x = 7 + self.y = 10 + self.z = 5 self.cloudpoints = { 'points': None, @@ -40,6 +39,22 @@ class Pokemon(): self.cal_position() + def walk_forward(self): + self.x += 2 + self.cal_position() + + def walk_backward(self): + self.x -= 2 + self.cal_position() + + def walk_left(self): + self.y -= 2 + self.cal_position() + + def walk_right(self): + self.y += 2 + self.cal_position() + def cal_position(self): transform_matrix = np.array([ [self.scalar, 0, 0, 0], @@ -53,11 +68,11 @@ class Pokemon(): pokemon_points = np.concatenate([pokemon_points, ones], axis=1) pokemon_points = pokemon_points @ transform_matrix pokemon_points[:, 2] = -pokemon_points[:, 2] - self.cloudpoints['points'] = pokemon_points + self.wcs_point = pokemon_points def get_position(self): - - return self.cloudpoints['points'] + # return self.cloudpoints['points'] + return self.wcs_point def get_colors(self): return self.cloudpoints['colors']