feat: pokemon can move

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(0)  # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1
-    cap = cv2.VideoCapture('demo1.mp4')
+    # 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)
+                # plot_corner_points(frame, new_approx)
-                cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2)  # 繪製輪廓
+                # 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)
+    '''
 
 
     

pokemon.py

@@ -6,11 +6,10 @@ class Pokemon():
         self.type = pokemon_type
         self.animal = animal
 
-        self.scalar = 10
+        self.scalar = 8
-        
+        self.x = 7
-        self.x = 4
+        self.y = 10
-        self.y = 20
+        self.z = 5
-        self.z = 8
         
         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']