update

runners/cad_strategy.py

@@ -91,8 +91,6 @@ class CADStrategyRunner(Runner):
         cad_model_real_world.export(os.path.join(temp_dir, f"real_world_{temp_name}.obj"))
         cad_model_blender_world:trimesh.Trimesh = cad_model.apply_transform(real_world_to_blender_world)
         
-        
-        
         with tempfile.TemporaryDirectory() as temp_dir:
             temp_dir = "/home/yan20/nbv_rec/project/franka_control/temp_output"
             cad_model_blender_world.export(os.path.join(temp_dir, f"{temp_name}.obj"))
@@ -173,7 +171,7 @@ class CADStrategyRunner(Runner):
             try:
                 ControlUtil.move_to(cam_to_world)
                 ''' get world pts '''
-                time.sleep(1)
+                time.sleep(0.5)
                 view_data = CommunicateUtil.get_view_data()
                 if view_data is None:
                     Log.error("Failed to get view data")
@@ -224,23 +222,3 @@ if __name__ == "__main__":
     model_path = r"C:\Users\hofee\Downloads\mesh.obj"
     model = trimesh.load(model_path)
     
-    ''' test register '''
-    # test_pts_L = np.load(r"C:\Users\hofee\Downloads\0.npy")
-    
-    # import open3d as o3d
-    # def add_noise(points, translation, rotation):
-    #     R = o3d.geometry.get_rotation_matrix_from_axis_angle(rotation)
-    #     noisy_points = points @ R.T + translation
-    #     return noisy_points
-    
-    # translation_noise = np.random.uniform(-0.5, 0.5, size=3)
-    # rotation_noise = np.random.uniform(-np.pi/4, np.pi/4, size=3) 
-    # noisy_pts_L = add_noise(test_pts_L, translation_noise, rotation_noise)
-    
-    # cad_to_cam_L = PtsUtil.register(noisy_pts_L, model)
-    
-    # cad_pts_L = PtsUtil.transform_point_cloud(noisy_pts_L, cad_to_cam_L)
-    # np.savetxt(r"test.txt", cad_pts_L)
-    # np.savetxt(r"src.txt", noisy_pts_L)
-
-    

utils/control_util.py

@@ -61,8 +61,8 @@ class ControlUtil:
     @staticmethod
     def rotate_display_table(degree):
         turn_directions = {
-            "left": 0,
+            "left": 1,
-            "right": 1
+            "right": 0
         }
         ControlUtil.cnt_rotation += degree
         print(f"Table rotated {ControlUtil.cnt_rotation} degree")
@@ -114,7 +114,7 @@ class ControlUtil:
             min_new_cam_to_world = None
             for display_table_rot in np.linspace(0.1,360, 1800):
                 display_table_rot_z_pose = ControlUtil.get_z_axis_rot_mat(display_table_rot)
-                new_cam_to_world = display_table_rot_z_pose @ cam_to_world
+                new_cam_to_world = np.linalg.inv(display_table_rot_z_pose) @ cam_to_world
                 if ControlUtil.check_limit(new_cam_to_world):
                     if display_table_rot < min_display_table_rot:
                         min_display_table_rot, min_new_cam_to_world = display_table_rot, new_cam_to_world
@@ -147,11 +147,12 @@ class ControlUtil:
     @staticmethod
     def move_to(pose: np.ndarray):
         rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
-        exec_time = rot_degree/9
+        exec_time = abs(rot_degree)/9
         start_time = time.time()
         ControlUtil.rotate_display_table(rot_degree)
         ControlUtil.set_pose(cam_to_world)
         end_time = time.time()
+        print(f"Move to pose with rotation {rot_degree} degree, exec time: {end_time - start_time}|exec time: {exec_time}")
         if end_time - start_time < exec_time:
             time.sleep(exec_time - (end_time - start_time))
             
@@ -167,7 +168,59 @@ if __name__ == "__main__":
     def rotate_back(rotation):
         ControlUtil.rotate_display_table(-rotation)
     
-    #rotate_back(45.3125623)
+    #main_test()
+    import sys; sys.path.append("/home/yan20/nbv_rec/project/franka_control")
+    from utils.communicate_util import CommunicateUtil
+    import ipdb
     ControlUtil.init()
-    #print(ControlUtil.get_curr_gripper_to_base_pose())
+    view_data_0 = CommunicateUtil.get_view_data(init=True)
+    depth_extrinsics_0 = view_data_0["depth_extrinsics"]
+    cam_to_world_0 = ControlUtil.get_pose()
+    print("cam_extrinsics_0")
+    print(depth_extrinsics_0)
+    print("cam_to_world_0")
+    print(cam_to_world_0)
     
+    ipdb.set_trace()
+    TEST_POSE:np.ndarray = np.asarray([
+        [ 0.46532393,  0.62171798,  0.63002284,  0.30230963],
+        [ 0.43205618, -0.78075723,  0.45136491, -0.29127173],
+        [ 0.77251656,  0.06217437, -0.63193429,  0.559957  ],
+        [ 0.        ,  0.        ,  0.        ,  1.        ],
+        ])
+    TEST_POSE_CAM_TO_WORLD = ControlUtil.BASE_TO_WORLD @ TEST_POSE @ ControlUtil.CAMERA_TO_GRIPPER
+    ControlUtil.move_to(TEST_POSE_CAM_TO_WORLD)
+    view_data_1 = CommunicateUtil.get_view_data()
+    depth_extrinsics_1 = view_data_1["depth_extrinsics"]
+    depth_extrinsics_1[:3,3] = depth_extrinsics_1[:3,3] / 1000
+    cam_to_world_1 = ControlUtil.get_pose()
+    print("cam_extrinsics_1")
+    print(depth_extrinsics_1)
+    print("cam_to_world_1")
+    print(TEST_POSE_CAM_TO_WORLD)
+    actual_cam_to_world_1 = cam_to_world_0 @ depth_extrinsics_1
+    print("actual_cam_to_world_1")
+    print(actual_cam_to_world_1)
+    ipdb.set_trace()
+    TEST_POSE_2:np.ndarray = np.asarray(
+        [[ 0.74398544, -0.61922696,  0.251049,    0.47000935],
+        [-0.47287207, -0.75338888, -0.45692666,  0.20843903],
+        [ 0.47207883 , 0.22123272, -0.85334192,  0.57863381],
+        [ 0. ,         0.   ,       0.  ,        1.  ,      ]]
+        )
+    TEST_POSE_CAM_TO_WORLD_2 = ControlUtil.BASE_TO_WORLD @ TEST_POSE_2 @ ControlUtil.CAMERA_TO_GRIPPER
+    
+    #ControlUtil.move_to(TEST_POSE_CAM_TO_WORLD_2)
+    ControlUtil.set_pose(TEST_POSE_CAM_TO_WORLD_2)
+    view_data_2 = CommunicateUtil.get_view_data()
+    depth_extrinsics_2 = view_data_2["depth_extrinsics"]
+    depth_extrinsics_2[:3,3] = depth_extrinsics_2[:3,3] / 1000
+    cam_to_world_2 = ControlUtil.get_pose()
+    print("cam_extrinsics_2")
+    print(depth_extrinsics_2)
+    print("cam_to_world_2")
+    print(TEST_POSE_CAM_TO_WORLD_2)
+    actual_cam_to_world_2 = cam_to_world_0 @ depth_extrinsics_2
+    print("actual_cam_to_world_2")
+    print(actual_cam_to_world_2)
+    ipdb.set_trace()

utils/pts_util.py

@@ -178,43 +178,143 @@ class PtsUtil:
         return np.sum(min_distances)
     
     @staticmethod
-    def register(pcl: np.ndarray, model: trimesh.Trimesh, voxel_size=0.008, max_iter=100000):
+    def multi_scale_icp(
-        model_pts = model.vertices
+        source, target, voxel_size_range, init_transformation=None, steps=20
-        sampled_world_pts = PtsUtil.voxel_downsample_point_cloud(pcl, voxel_size)
+    ):
-        sampled_model_pts = PtsUtil.voxel_downsample_point_cloud(model_pts, voxel_size)
+        pipreg = o3d.pipelines.registration
-        best_pose = np.eye(4)
-        best_pose[:3, 3] = np.mean(sampled_world_pts, axis=0) - np.mean(sampled_model_pts, axis=0)
-        best_distance = float('inf')
-        temperature = 1.0
-        cnt_unchange = 0
-        for _ in range(max_iter):
-            print(best_distance)
-            new_pose = best_pose.copy()
-            rotation_noise = np.random.randn(3)
-            rotation_noise /= np.linalg.norm(rotation_noise)
-            rotation_noise *= temperature
-            translation_noise = np.random.randn(3) * 0.1 * temperature
-            rotation_matrix = PoseUtil.get_uniform_rotation(0, 360)
-            new_pose[:3, :3] = rotation_matrix @ best_pose[:3, :3]
-            new_pose[:3, 3] += translation_noise
 
-            distance = PtsUtil.chamfer_distance(
+        if init_transformation is not None:
-                PtsUtil.transform_point_cloud(sampled_world_pts, new_pose),
+            current_transformation = init_transformation
-                sampled_model_pts
+        else:
+            current_transformation = np.identity(4)
+        cnt = 0
+        best_score = 1e10
+        best_reg = None
+        voxel_sizes = []
+        for i in range(steps):
+            voxel_sizes.append(
+                voxel_size_range[0]
+                + i * (voxel_size_range[1] - voxel_size_range[0]) / steps
             )
 
-            if distance < best_distance:
+        for voxel_size in voxel_sizes:
-                best_pose, best_distance = new_pose, distance
+            radius_normal = voxel_size * 2
-                cnt_unchange = 0
+            source_downsampled = source.voxel_down_sample(voxel_size)
+            source_downsampled.estimate_normals(
+                search_param=o3d.geometry.KDTreeSearchParamHybrid(
+                    radius=radius_normal, max_nn=30
+                )
+            )
+            target_downsampled = target.voxel_down_sample(voxel_size)
+            target_downsampled.estimate_normals(
+                search_param=o3d.geometry.KDTreeSearchParamHybrid(
+                    radius=radius_normal, max_nn=30
+                )
+            )
+
+            reg_icp = pipreg.registration_icp(
+                source_downsampled,
+                target_downsampled,
+                voxel_size * 2,
+                current_transformation,
+                pipreg.TransformationEstimationPointToPlane(),
+                pipreg.ICPConvergenceCriteria(max_iteration=500),
+            )
+            cnt += 1
+            if reg_icp.fitness == 0:
+                score = 1e10
             else:
-                cnt_unchange += 1
+                score = reg_icp.inlier_rmse / reg_icp.fitness
-                if cnt_unchange > 11110:
-                    break
             
-            temperature *= 0.999
+            if score < best_score:
-            print(temperature)
+                best_score = score
+                best_reg = reg_icp
 
-        return best_pose
+        return best_reg, best_score
+
+    @staticmethod
+    def multi_scale_ransac(source_downsampled, target_downsampled, source_fpfh, model_fpfh, voxel_size_range, steps=20):
+        pipreg = o3d.pipelines.registration
+        cnt = 0
+        best_score = 1e10
+        best_reg = None
+        voxel_sizes = []
+        for i in range(steps):
+            voxel_sizes.append(
+                voxel_size_range[0]
+                + i * (voxel_size_range[1] - voxel_size_range[0]) / steps
+            )
+
+        for voxel_size in voxel_sizes:
+            reg_ransac = pipreg.registration_ransac_based_on_feature_matching(
+                source_downsampled,
+                target_downsampled,
+                source_fpfh,
+                model_fpfh,
+                mutual_filter=True,
+                max_correspondence_distance=voxel_size*2,
+                estimation_method=pipreg.TransformationEstimationPointToPoint(False),
+                ransac_n=4,
+                checkers=[pipreg.CorrespondenceCheckerBasedOnEdgeLength(0.9)],
+                criteria=pipreg.RANSACConvergenceCriteria(8000000, 500),
+            )
+            cnt += 1
+            if reg_ransac.fitness == 0:
+                score = 1e10
+            else:
+                score = reg_ransac.inlier_rmse / reg_ransac.fitness
+            if score < best_score:
+                best_score = score
+                best_reg = reg_ransac
+
+        return best_reg, best_score
+
+    @staticmethod
+    def register(pcl: np.ndarray, model: trimesh.Trimesh, voxel_size=0.01):
+        radius_normal = voxel_size * 2
+        pipreg = o3d.pipelines.registration
+        model_pcd = o3d.geometry.PointCloud()
+        model_pcd.points = o3d.utility.Vector3dVector(model.vertices)
+        model_downsampled = model_pcd.voxel_down_sample(voxel_size)
+        model_downsampled.estimate_normals(
+            search_param=o3d.geometry.KDTreeSearchParamHybrid(
+                radius=radius_normal, max_nn=30
+            )
+        )
+        model_fpfh = pipreg.compute_fpfh_feature(
+            model_downsampled,
+            o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=100),
+        )
+
+        source_pcd = o3d.geometry.PointCloud()
+        source_pcd.points = o3d.utility.Vector3dVector(pcl)
+        source_downsampled = source_pcd.voxel_down_sample(voxel_size)
+        source_downsampled.estimate_normals(
+            search_param=o3d.geometry.KDTreeSearchParamHybrid(
+                radius=radius_normal, max_nn=30
+            )
+        )
+        source_fpfh = pipreg.compute_fpfh_feature(
+            source_downsampled,
+            o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=100),
+        )
+
+        reg_ransac, ransac_best_score = PtsUtil.multi_scale_ransac(
+            source_downsampled,
+            model_downsampled,
+            source_fpfh,
+            model_fpfh,
+            voxel_size_range=(0.03, 0.005),
+            steps=3,
+        )
+        reg_icp, icp_best_score = PtsUtil.multi_scale_icp(
+            source_downsampled,
+            model_downsampled,
+            voxel_size_range=(0.02, 0.001),
+            init_transformation=reg_ransac.transformation,
+            steps=50,
+        )
+        return reg_icp.transformation
     
     @staticmethod
     def get_pts_from_depth(depth, cam_intrinsic, cam_extrinsic):