fix: complete OpenCV version

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]
 
-
+    M = CAMERA_MATRIX.dot(M)
-    for index in range(pokemon_points.shape[0]):
+    pokemon_points = (M @ (pokemon_points.T)).T
-        pokemon_color = [int(i*255) for i in pokemon_colors[index]]
+    for point , color in zip(pokemon_points, pokemon_colors):
-        # print(pokemon_color)
+        color = [int(i*255) for i in color]
-        # print(pokemon_points[index])
+        color = np.array(color, dtype=int)
-        pokemon_point = CAMERA_MATRIX @ M
+        color = color[0:3]
-        # print(pokemon_point.shape)
+        point2D = point[:2] / point[2]
-        # print(pokemon_points.shape)
+        point2D = np.int_(point2D[::-1])
-        pokemon_point = pokemon_point @ (pokemon_points[index].reshape((4, 1)))
+        if 0 <= point2D[0] < frame.shape[1] and 0 <= point2D[1] < frame.shape[0]:
-        pokemon_point = pokemon_point[:2] / pokemon_point[2]
+            frame = cv2.circle(frame, tuple(point2D), 10,color.astype(int).tolist(), thickness=-1)
-        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)
-
     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(' '):

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.x = 4
-        self.y = 13
+        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']