diff --git a/runners/cad_strategy.py b/runners/cad_strategy.py index ab06740..c2f50db 100644 --- a/runners/cad_strategy.py +++ b/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") @@ -223,24 +221,4 @@ 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) - \ No newline at end of file diff --git a/utils/control_util.py b/utils/control_util.py index 9d76179..e3eea70 100644 --- a/utils/control_util.py +++ b/utils/control_util.py @@ -61,8 +61,8 @@ class ControlUtil: @staticmethod def rotate_display_table(degree): turn_directions = { - "left": 0, - "right": 1 + "left": 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)) @@ -159,7 +160,7 @@ class ControlUtil: if __name__ == "__main__": ControlUtil.connect_robot() - #ControlUtil.franka_reset() + # ControlUtil.franka_reset() def main_test(): print(ControlUtil.get_curr_gripper_to_base_pose()) ControlUtil.init() @@ -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()) - \ No newline at end of file + 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() \ No newline at end of file diff --git a/utils/pts_util.py b/utils/pts_util.py index 13bb273..ffe58d9 100644 --- a/utils/pts_util.py +++ b/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): - model_pts = model.vertices - sampled_world_pts = PtsUtil.voxel_downsample_point_cloud(pcl, voxel_size) - sampled_model_pts = PtsUtil.voxel_downsample_point_cloud(model_pts, voxel_size) - 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( - PtsUtil.transform_point_cloud(sampled_world_pts, new_pose), - sampled_model_pts - ) - - if distance < best_distance: - best_pose, best_distance = new_pose, distance - cnt_unchange = 0 - else: - cnt_unchange += 1 - if cnt_unchange > 11110: - break - - temperature *= 0.999 - print(temperature) + def multi_scale_icp( + source, target, voxel_size_range, init_transformation=None, steps=20 + ): + pipreg = o3d.pipelines.registration - return best_pose + if init_transformation is not None: + current_transformation = init_transformation + 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 + ) + + for voxel_size in voxel_sizes: + radius_normal = voxel_size * 2 + 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: + score = reg_icp.inlier_rmse / reg_icp.fitness + + if score < best_score: + best_score = score + best_reg = reg_icp + + 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):