update

runners/cad_strategy.py

@@ -7,6 +7,10 @@ import PytorchBoot.stereotype as stereotype
 from PytorchBoot.utils.log_util import Log
 from PytorchBoot.status import status_manager
 
+
+import sys
+
+sys.path.append("/home/user/nbv_rec/nbv_rec_control")
 from utils.control_util import ControlUtil
 from utils.communicate_util import CommunicateUtil
 from utils.pts_util import PtsUtil
@@ -101,7 +105,7 @@ class CADStrategyRunner(Runner):
             status_info=self.status_info
         )
         
-        ''' extract cam_to world sequence  '''
+        ''' extract cam_to_world sequence  '''
         cam_to_world_seq = []
         coveraget_rate_seq = []
         
@@ -132,13 +136,13 @@ class CADStrategyRunner(Runner):
             
     
 if __name__ == "__main__":
-    model_path = "/home/yan20/nbv_rec/data/test_CAD/test_model/bear_scaled.ply"
+    model_path = "/home/user/nbv_rec/data/mesh.obj"
     model = trimesh.load(model_path)
-    test_pts_L = np.loadtxt("/home/yan20/nbv_rec/data/test_CAD/cam_pts_0_L.txt")
-    test_pts_R = np.loadtxt("/home/yan20/nbv_rec/data/test_CAD/cam_pts_0_R.txt")
-    cam_to_world_L = PtsUtil.register_icp(test_pts_L, model)
-    cam_to_world_R = PtsUtil.register_icp(test_pts_R, model)
-    print(cam_to_world_L)
-    print("================================")
-    print(cam_to_world_R)
-    
+    test_pts_L = np.loadtxt("/home/user/nbv_rec/data/cam_pts_0_L.txt")
+    test_pts_R = np.loadtxt("/home/user/nbv_rec/data/cam_pts_0_R.txt")
+    cad_to_cam_L = PtsUtil.register_icp(test_pts_L, model)
+    cad_to_cam_R = PtsUtil.register_icp(test_pts_R, model)
+    cad_pts_L = PtsUtil.transform_point_cloud(test_pts_L, cad_to_cam_L)
+    cad_pts_R = PtsUtil.transform_point_cloud(test_pts_R, cad_to_cam_R)
+    np.savetxt("/home/user/nbv_rec/data/cad_pts_0_L.txt", cad_pts_L)
+    np.savetxt("/home/user/nbv_rec/data/cad_pts_0_R.txt", cad_pts_R)

runners/inference.py

@@ -0,0 +1,80 @@
+import os
+import trimesh
+import numpy as np
+from PytorchBoot.runners.runner import Runner
+from PytorchBoot.config import ConfigManager
+import PytorchBoot.stereotype as stereotype
+from PytorchBoot.utils.log_util import Log
+import PytorchBoot.namespace as namespace
+from PytorchBoot.status import status_manager
+
+from utils.control_util import ControlUtil
+from utils.communicate_util import CommunicateUtil
+from utils.pts_util import PtsUtil
+from utils.view_sample_util import ViewSampleUtil
+from utils.reconstruction_util import ReconstructionUtil
+
+
+@stereotype.runner("inferencer")
+class Inferencer(Runner):
+
+    def __init__(self, config_path: str):
+        super().__init__(config_path)
+        self.load_experiment("inferencer")
+        self.reconstruct_config = ConfigManager.get("runner", "reconstruct")
+        self.voxel_size = self.reconstruct_config["voxel_size"]
+        
+    def create_experiment(self, backup_name=None):
+        super().create_experiment(backup_name)
+    
+    def load_experiment(self, backup_name=None):
+        super().load_experiment(backup_name)
+        
+    def run_inference(self, model_name):
+        
+        ''' init robot '''
+        ControlUtil.init()
+        ''' take first view '''
+        view_data = CommunicateUtil.get_view_data()
+        first_cam_pts = None
+        first_cam_pose = None
+        combined_pts = first_cam_pts
+        input_data = {
+            "scanned_target_points_num": [first_cam_pts.shape[0]],
+            "scanned_n_to_world_pose_9d": [first_cam_pose],
+            "combined_scanned_pts": combined_pts
+        }
+        ''' enter loop '''
+        while True:
+            ''' inference '''
+            inference_result = CommunicateUtil.get_inference_data(input_data)
+            cam_to_world = inference_result["cam_to_world"]
+            ''' set pose '''
+            ControlUtil.set_pose(cam_to_world)
+            ''' take view '''
+            view_data = CommunicateUtil.get_view_data()
+            curr_cam_pts = None
+            curr_cam_pose = None
+            ''' update combined pts '''
+            combined_pts = np.concatenate([combined_pts, curr_cam_pts], axis=0)
+            combined_pts = PtsUtil.voxel_downsample_point_cloud(combined_pts, voxel_size=self.voxel_size)
+            
+            
+            ''' update input data '''
+            
+    
+    def run(self):
+        self.run_inference()
+            
+    
+if __name__ == "__main__":
+    model_path = "/home/yan20/nbv_rec/data/test_CAD/test_model/bear_scaled.ply"
+    model = trimesh.load(model_path)
+    test_pts_L = np.loadtxt("/home/yan20/nbv_rec/data/test_CAD/cam_pts_0_L.txt")
+    test_pts_R = np.loadtxt("/home/yan20/nbv_rec/data/test_CAD/cam_pts_0_R.txt")
+    cam_to_world_L = PtsUtil.register_icp(test_pts_L, model)
+    cam_to_world_R = PtsUtil.register_icp(test_pts_R, model)
+    print(cam_to_world_L)
+    print("================================")
+    print(cam_to_world_R)
+    

test.py

@@ -0,0 +1,10 @@
+import flask
+
+app = flask.Flask(__name__)
+
+@app.route('/hello')
+def hello():
+    return "Hello, World!"
+
+if __name__ == '__main__':
+    app.run(host="0.0.0.0", port=7999)

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