update

utils/communicate_util.py

@@ -4,10 +4,10 @@ class CommunicateUtil:
     INFERENCE_HOST = "127.0.0.1:5000"
     
     def get_view_data() -> dict:
-        data = None
+        data = {}
         return data
     
-    def get_inference_data() -> dict:
-        data = None
+    def get_inference_data(view_data:dict) -> dict:
+        data = {}
         return data
         

utils/control_util.py

@@ -114,7 +114,7 @@ class ControlUtil:
 # ----------- Debug Test -------------
 
 if __name__ == "__main__":
-    #ControlUtil.init()
+    ControlUtil.init()
     import time
     start = time.time()
     rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(ControlUtil.INIT_POSE)

utils/pts_util.py

@@ -102,23 +102,23 @@ class PtsUtil:
         return filtered_sampled_points[:, :3]
     
     @staticmethod
-    def register_icp(pcl: np.ndarray, model: trimesh.Trimesh, threshold = 0.005) -> np.ndarray:
-        """
-        Register point cloud to CAD model.
-        Returns the transformation matrix.
-        """
-
+    def register_icp(pcl: np.ndarray, model: trimesh.Trimesh, threshold=0.5) -> np.ndarray:
         mesh_points = np.asarray(model.vertices)
+        downsampled_mesh_points = PtsUtil.random_downsample_point_cloud(mesh_points, pcl.shape[0])
         mesh_point_cloud = o3d.geometry.PointCloud()
-        mesh_point_cloud.points = o3d.utility.Vector3dVector(mesh_points)
-
+        mesh_point_cloud.points = o3d.utility.Vector3dVector(downsampled_mesh_points)
+        np.savetxt("mesh_point_cloud.txt", downsampled_mesh_points)
         pcl_point_cloud = o3d.geometry.PointCloud()
         pcl_point_cloud.points = o3d.utility.Vector3dVector(pcl)
-
-         
+        initial_transform = np.eye(4)
         reg_icp = o3d.pipelines.registration.registration_icp(
             pcl_point_cloud, mesh_point_cloud, threshold,
-            np.eye(4),
+            initial_transform,
             o3d.pipelines.registration.TransformationEstimationPointToPoint()
         )
+        if np.allclose(reg_icp.transformation, np.eye(4)):
+            print("Registration failed. Check your initial alignment and point cloud quality.")
+        else:
+            print("Registration successful.")
+        print(reg_icp.transformation)
         return reg_icp.transformation