error: want to render pokemon to frame, but error

main.py

@@ -188,6 +188,35 @@ def visualization(inverse_matrix_M, pokemon):
 
     vis.run()
 
+def render_pokemon(frame, M, camera_position, pokemon):
+    pokemon_points = pokemon.get_position()
+    pokemon_colors = pokemon.get_colors()
+    distance = np.linalg.norm(pokemon_points - camera_position, axis=1)
+
+    sorted_indices = np.argsort(-distance)
+    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)
+
+    return frame
+
 if __name__ == '__main__':
 
     # 設定連接到 Android 手機的相機
@@ -255,6 +284,17 @@ if __name__ == '__main__':
                 print("t:", translation_vector)
                 print()
 
+                R = rotation_matrix
+                t = translation_vector
+                rt = np.concatenate((R, t), axis=1)
+                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]
+
+                frame = render_pokemon(frame, rt, camera_position, pokemon)
+                cv2.imwrite('test.jpg', frame)
+
 
             # 顯示結果
             cv2.namedWindow('Paper Detection(edge)', 0)

mesh_to_points.py

@@ -1,4 +1,4 @@
-
+import os
 import open3d as o3d
 import trimesh
 import matplotlib.pyplot as plt
@@ -6,7 +6,9 @@ from PIL import Image
 import numpy as np
 
 # 读取 OBJ 模型文件
-mesh = trimesh.load('output.obj')
+for d in os.listdir('models'):
+    print(d)
+    mesh = trimesh.load('models/{}/0/output.obj'.format(d))
 
     colors = mesh.visual.to_color().vertex_colors
     points = mesh.vertices
@@ -14,7 +16,7 @@ print(colors.shape)
     print(colors)
     print(points.shape)
 
-np.save('colors.npy', colors)
+    np.save('models/{}/0/colors.npy'.format(d), colors)
-np.save('points.npy', points)
+    np.save('models/{}/0/points.npy'.format(d), points)
 
-mesh.show()
+    # mesh.show()

pokemon.py

@@ -26,6 +26,8 @@ class Pokemon():
         self.cloudpoints['colors'] = self.cloudpoints['colors'] / 255
         self.cloudpoints['colors'] = self.cloudpoints['colors'][:, :3]
 
+        
+
     def get_position(self):
         transform_matrix = np.array([
             [self.scalar, 0, 0, 0],
@@ -38,7 +40,7 @@ class Pokemon():
         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 = pokemon_points[:, :3]
 
         return pokemon_points