update

utils/control_util.py

@@ -19,7 +19,7 @@ class ControlUtil:
     
     CAMERA_TO_GRIPPER:np.ndarray = np.asarray([
         [0, -1, 0, 0.01],
-        [1, 0, 0, -0.05],
+        [1, 0, 0, 0],
         [0, 0, 1, 0.075],
         [0, 0, 0, 1]
         ])
@@ -33,8 +33,10 @@ class ControlUtil:
     
     @staticmethod
     def connect_robot():
-        ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200) 
-        ControlUtil.__fa = FrankaArm(robot_num=2)
+        if ControlUtil.__fa is None:
+            ControlUtil.__fa = FrankaArm(robot_num=2)
+        if ControlUtil.__ser is None:
+            ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200) 
         
     @staticmethod
     def franka_reset() -> None:
@@ -63,8 +65,7 @@ class ControlUtil:
             "right": 1
         }
         ControlUtil.cnt_rotation += degree
-        
-        print("total rot:", ControlUtil.cnt_rotation)
+        print(f"Table rotated {ControlUtil.cnt_rotation} degree")
         if degree >= 0:
             turn_angle = degree
             turn_direction = turn_directions["right"]
@@ -146,7 +147,6 @@ class ControlUtil:
     @staticmethod
     def move_to(pose: np.ndarray):
         rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
-        print("table rot degree:", rot_degree)
         exec_time = rot_degree/9
         start_time = time.time()
         ControlUtil.rotate_display_table(rot_degree)
@@ -154,25 +154,17 @@ class ControlUtil:
         end_time = time.time()
         if end_time - start_time < exec_time:
             time.sleep(exec_time - (end_time - start_time))
+            
 # ----------- Debug Test -------------
 
 if __name__ == "__main__":
     ControlUtil.connect_robot()
-    print(ControlUtil.get_curr_gripper_to_base_pose())
-    ControlUtil.init()
-#     import time
-#     start = time.time()
-#     test_pose = np.asarray([[ 0.06023737 ,-0.81328565,  0.57872715, -0.23602393],
-#  [-0.99107872 ,-0.11773834, -0.06230158,  0.24174289],
-#  [ 0.11880735 ,-0.56981127 ,-0.813138  ,  0.15199069],
-#  [ 0.  ,        0.      ,   0.      ,    1.    ,    ]])
-#     print(test_pose)
-#     rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(test_pose)
-#     print(rot_degree, cam_to_world)
-#     end = time.time()
-#     print(f"Time: {end-start}")
     
-#     ControlUtil.set_pose(test_pose)
-#     import ipdb; ipdb.set_trace()
-#     ControlUtil.move_to(test_pose)
-#     print("End!")
+    def main_test():
+        print(ControlUtil.get_curr_gripper_to_base_pose())
+        ControlUtil.init()
+    
+    def rotate_back(rotation):
+        ControlUtil.rotate_display_table(-rotation)
+    
+    rotate_back(122.0661478599)

utils/preprocess_util.py

@@ -23,6 +23,7 @@ def save_target_points(root, scene, frame_idx, target_points: np.ndarray, file_t
     save_np_pts(pts_path, target_points, file_type)
     
 def save_scan_points_indices(root, scene, frame_idx, scan_points_indices: np.ndarray, file_type="txt"):
+    file_type="npy"
     indices_path = os.path.join(root,scene, "scan_points_indices", f"{frame_idx}.{file_type}")
     if not os.path.exists(os.path.join(root,scene, "scan_points_indices")):
         os.makedirs(os.path.join(root,scene, "scan_points_indices"))
@@ -69,7 +70,7 @@ def save_scene_data(root, scene, file_type="txt"):
     random_downsample_N = 32768
     voxel_size=0.002
     filter_degree = 75
-    min_z = 0.2
+    min_z = 0.25
     max_z = 0.5
     
     ''' scan points '''
@@ -81,7 +82,7 @@ def save_scene_data(root, scene, file_type="txt"):
     ''' read frame data(depth|mask|normal) '''
     frame_num = DataLoadUtil.get_scene_seq_length(root, scene)
     for frame_id in range(frame_num):
-        #print(f"[scene({scene_idx}/{scene_total})|frame({frame_id}/{frame_num})]Processing {scene} frame {frame_id}")
+        Log.info(f"frame({frame_id}/{frame_num})]Processing {scene} frame {frame_id}")
         path = DataLoadUtil.get_path(root, scene, frame_id)
         cam_info = DataLoadUtil.load_cam_info(path, binocular=True)
         depth_L, depth_R = DataLoadUtil.load_depth(

utils/pts_util.py

@@ -92,7 +92,7 @@ class PtsUtil:
         cam_pose,
         voxel_size=0.002,
         theta=45,
-        z_range=(0.2, 0.45),
+        z_range=(0.25, 0.5),
     ):
         """filter with z range"""
         points_cam = PtsUtil.transform_point_cloud(points, np.linalg.inv(cam_pose))

utils/reconstruction_util.py

@@ -8,7 +8,7 @@ class ReconstructionUtil:
     def compute_coverage_rate(target_point_cloud, combined_point_cloud, threshold=0.01):
         kdtree = cKDTree(combined_point_cloud)
         distances, _ = kdtree.query(target_point_cloud)
-        covered_points_num = np.sum(distances < threshold)
+        covered_points_num = np.sum(distances < threshold*2)
         coverage_rate = covered_points_num / target_point_cloud.shape[0]
         return coverage_rate, covered_points_num
     
@@ -16,7 +16,7 @@ class ReconstructionUtil:
     def compute_overlap_rate(new_point_cloud, combined_point_cloud, threshold=0.01):
         kdtree = cKDTree(combined_point_cloud)
         distances, _ = kdtree.query(new_point_cloud)
-        overlapping_points = np.sum(distances < threshold)
+        overlapping_points = np.sum(distances < threshold*2)
         if new_point_cloud.shape[0] == 0:
             overlap_rate = 0
         else:

utils/view_util.py

@@ -108,7 +108,6 @@ class ViewUtil:
         if image_id is None:
             return None
         else:
-            print(f"Image ID: {image_id}")
             camera_intrinsics = CameraIntrinsics(
                 depth_intrinsics['width'],
                 depth_intrinsics['height'],