feat: rotate pokemon

main.py

@@ -277,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, \
@@ -319,21 +319,20 @@ if __name__ == '__main__':
             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('s'):
+                pokemon.walk_forward()
             if key == ord('a'):
-                print("walk left")
                 pokemon.walk_left()
             if key == ord('d'):
-                print("walk right")
                 pokemon.walk_right()
+            if key == ord('o'):
+                pokemon.rotate_right()
+            if key == ord('p'):
+                pokemon.rotate_left()
 
 
     cap.release()
@@ -341,7 +340,6 @@ if __name__ == '__main__':
 
 
     #  畫 camera pose
-    '''
     M_inv = []
     for index in range(len(rotation_vectors)):
         R = rotation_vectors[index]
@@ -352,7 +350,7 @@ if __name__ == '__main__':
 
     # load Pokemon points
     visualization(M_inv, pokemon)
-    '''
+
 
     
 

pokemon.py

@@ -1,6 +1,6 @@
 import open3d as o3d
 import numpy as np
-
+from math import pi
 class Pokemon():
     def __init__(self, pokemon_type, animal):
         self.type = pokemon_type
@@ -10,7 +10,8 @@ class Pokemon():
         self.x = 7
         self.y = 10
         self.z = 5
-        
+        self.theta = 2*pi
+        self.accum_rotate = np.array([[1, 0, 0],[0, 1, 0],[0,0,1]])
         self.cloudpoints = {
             'points': None,
             'colors': None,
@@ -56,15 +57,30 @@ class Pokemon():
         self.y += 2
         self.cal_position()
     
+    def rotate_left(self):
+        self.theta += pi/4
+        self.cal_position()
+    
+    def rotate_right(self):
+        self.theta -= pi/4
+        self.cal_position()
+
+    
+
     def cal_position(self):
         transform_matrix = np.array([
-            [self.scalar, 0, 0, 0],
+            [np.cos(self.theta)*self.scalar,  -np.sin(self.theta)*self.scalar,0, 0],
-            [0, 0, self.scalar, 0],
+            [0,0 , self.scalar, 0],
-            [0, self.scalar, 0, 0],
+            [np.sin(self.theta)*self.scalar, np.cos(self.theta)*self.scalar, 0, 0],
-            [self.x, self.y, self.z, 1]])             # shift (前後, 左右, 高度)
+            [self.x, self.y, self.z, 1]])                              # rotate (x, y, z, theta)
-
         pokemon_points = self.cloudpoints['points']
 
+        '''
+        transform_matrix[3, :3] = transform_matrix[3, :3] @ self.accum_rotate
+        tmp  =  transform_matrix[[0,2, 1], :3]
+        self.accum_rotate = self.accum_rotate @ tmp
+'''
+
         ones = np.ones((pokemon_points.shape[0], 1))
         pokemon_points = np.concatenate([pokemon_points, ones], axis=1)
         pokemon_points = pokemon_points @ transform_matrix
@@ -72,7 +88,9 @@ class Pokemon():
         self.wcs_point = pokemon_points
 
     def get_position(self):
+        # return self.cloudpoints['points']
         return self.wcs_point
     
     def get_colors(self):
         return self.cloudpoints['colors']
+