From 41ee79db0cafee13863666849510641b44962ec4 Mon Sep 17 00:00:00 2001 From: hofee Date: Sun, 13 Oct 2024 15:24:41 +0800 Subject: [PATCH] update config --- app_cad.py | 17 ++- ...e_loop.yaml => cad_close_loop_config.yaml} | 0 ..._config.yaml => cad_open_loop_config.yaml} | 0 move_cam_to.py | 33 ------ register.py | 109 ------------------ ..._strategy.py => cad_open_loop_strategy.py} | 8 +- 6 files changed, 16 insertions(+), 151 deletions(-) rename configs/{cad_config_close_loop.yaml => cad_close_loop_config.yaml} (100%) rename configs/{cad_config.yaml => cad_open_loop_config.yaml} (100%) delete mode 100644 move_cam_to.py delete mode 100644 register.py rename runners/{cad_strategy.py => cad_open_loop_strategy.py} (98%) diff --git a/app_cad.py b/app_cad.py index 70ab16c..904b46a 100644 --- a/app_cad.py +++ b/app_cad.py @@ -1,9 +1,16 @@ from PytorchBoot.application import PytorchBootApplication -from runners.cad_strategy import CADStrategyRunner +from runners.cad_open_loop_strategy import CADOpenLoopStrategyRunner +from runners.cad_close_loop_strategy import CADCloseLoopStrategyRunner -@PytorchBootApplication("cad") -class AppCAD: + +@PytorchBootApplication("cad_ol") +class AppCADOpenLoopStrategy: @staticmethod def start(): - CADStrategyRunner("configs/cad_config.yaml").run() - \ No newline at end of file + CADOpenLoopStrategyRunner("configs/cad_open_loop_config.yaml").run() + +@PytorchBootApplication("cad_cl") +class AppCADCloseLoopStrategy: + @staticmethod + def start(): + CADCloseLoopStrategyRunner("configs/cad_close_loop_config.yaml").run() \ No newline at end of file diff --git a/configs/cad_config_close_loop.yaml b/configs/cad_close_loop_config.yaml similarity index 100% rename from configs/cad_config_close_loop.yaml rename to configs/cad_close_loop_config.yaml diff --git a/configs/cad_config.yaml b/configs/cad_open_loop_config.yaml similarity index 100% rename from configs/cad_config.yaml rename to configs/cad_open_loop_config.yaml diff --git a/move_cam_to.py b/move_cam_to.py deleted file mode 100644 index d83c53f..0000000 --- a/move_cam_to.py +++ /dev/null @@ -1,33 +0,0 @@ -from utils.data_load import DataLoadUtil -from utils.control_util import ControlUtil -from utils.view_util import ViewUtil -from utils.pts_util import PtsUtil -from utils.communicate_util import CommunicateUtil -import numpy as np -import os -if __name__ == "__main__": - idx = "2" - ControlUtil.connect_robot() - ControlUtil.franka_reset() - root_path = "/home/yan20/nbv_rec/project/franka_control/temp_output/cad_model_world" - frame_path = os.path.join(root_path, idx) - cam_info = DataLoadUtil.load_cam_info(frame_path, binocular=True) - render_camL_to_world = cam_info["cam_to_world"] - render_camO_to_world = cam_info["cam_to_world_O"] - L_to_O = np.dot(np.linalg.inv(render_camO_to_world), render_camL_to_world) - - ControlUtil.set_pose(render_camO_to_world) - - real_camO_to_world = ControlUtil.get_pose() - real_camL_to_world = np.dot(real_camO_to_world,L_to_O) - view_data = CommunicateUtil.get_view_data(init=True) - cam_pts = ViewUtil.get_pts(view_data) - np.savetxt(f"cam_pts_{idx}.txt", cam_pts) - world_pts = PtsUtil.transform_point_cloud(cam_pts, render_camL_to_world) - print(real_camL_to_world) - np.savetxt(f"world_pts_{idx}.txt", world_pts) - # import ipdb;ipdb.set_trace() - - # view_data = CommunicateUtil.get_view_data() - # cam_pts = ViewUtil.get_pts(view_data) - # np.savetxt(f"cam_pts_{idx}.txt", cam_pts) \ No newline at end of file diff --git a/register.py b/register.py deleted file mode 100644 index 5640c96..0000000 --- a/register.py +++ /dev/null @@ -1,109 +0,0 @@ -import os -import numpy as np -import trimesh -from utils.pts_util import PtsUtil -import numpy as np -import open3d as o3d -import torch -import trimesh -from scipy.spatial import cKDTree - -def register(pcl: np.ndarray, model: trimesh.Trimesh, voxel_size=0.002): - 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 = pipreg.registration_ransac_based_on_feature_matching( - # source_downsampled, - # model_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(4000000, 500), - # ) - - reg_icp2 = pipreg.registration_icp( - source_downsampled, - model_downsampled, - voxel_size*10, - np.eye(4), - pipreg.TransformationEstimationPointToPlane(), - pipreg.ICPConvergenceCriteria(max_iteration=2000), - ) - - reg_icp = pipreg.registration_icp( - source_downsampled, - model_downsampled, - voxel_size, - reg_icp2.transformation, - pipreg.TransformationEstimationPointToPlane(), - pipreg.ICPConvergenceCriteria(max_iteration=2000), - ) - - return reg_icp2.transformation, reg_icp.transformation - -if __name__ == "__main__": - - model_dir = "/home/yan20/Desktop/nbv_rec/data/models/bear" - model_path = os.path.join(model_dir,"mesh.ply") - temp_name = "cad_model_world" - cad_model = trimesh.load(model_path) - - pts_path = "/home/yan20/nbv_rec/project/franka_control/first_real_pts_bear.txt" - pts = np.loadtxt(pts_path) - very_coarse_real_to_cad = np.eye(4) - cad_model_pts = cad_model.vertices - very_coarse_real_to_cad[:3,3] = np.mean(cad_model_pts, axis=0) - np.mean(pts, axis=0) - very_coarse_cad_pts = PtsUtil.transform_point_cloud(pts, very_coarse_real_to_cad) - - target_point = np.array([-3.422540776542676300e-02, -2.412379452948226755e-02, 1.123609126159126337e-01]) - - # 设置一个容忍度 - tolerance = 1e-5 - - # 计算每个点与目标点之间的距离 - distances = np.linalg.norm(very_coarse_cad_pts - target_point, axis=1) - - # 统计距离小于容忍度的点的数量 - count = np.sum(distances < tolerance) - print(count) - print(very_coarse_cad_pts.shape) - print(np.mean(pts, axis=0), np.mean(cad_model_pts, axis=0), np.mean(very_coarse_cad_pts, axis=0)) - pts = pts[distances > tolerance] - np.savetxt(os.path.join(temp_name + "_filtered.txt"), pts) - # very_coarse_real_to_cad[:3,3] = np.mean(cad_model_pts, axis=0) - np.mean(pts, axis=0) - # very_coarse_cad_pts = PtsUtil.transform_point_cloud(pts, very_coarse_real_to_cad) - # np.savetxt(os.path.join(temp_name + "_very_coarse_reg.txt"), very_coarse_cad_pts) - - - # real_to_cad = PtsUtil.register(very_coarse_cad_pts, cad_model) - # cad_pts = PtsUtil.transform_point_cloud(very_coarse_cad_pts, real_to_cad) - # np.savetxt(os.path.join(temp_name + "_reg.txt"), cad_pts) diff --git a/runners/cad_strategy.py b/runners/cad_open_loop_strategy.py similarity index 98% rename from runners/cad_strategy.py rename to runners/cad_open_loop_strategy.py index 515ea1d..a5bb618 100644 --- a/runners/cad_strategy.py +++ b/runners/cad_open_loop_strategy.py @@ -19,16 +19,16 @@ from utils.data_load import DataLoadUtil from utils.view_util import ViewUtil -@stereotype.runner("CAD_strategy_runner") -class CADStrategyRunner(Runner): +@stereotype.runner("CAD_open_loop_strategy_runner") +class CADOpenLoopStrategyRunner(Runner): def __init__(self, config_path: str): super().__init__(config_path) - self.load_experiment("cad_strategy") + self.load_experiment("cad_open_loop_strategy") self.status_info = { "status_manager": status_manager, "app_name": "cad", - "runner_name": "CAD_strategy_runner" + "runner_name": "CAD_open_loop_strategy_runner" } self.generate_config = ConfigManager.get("runner", "generate") self.reconstruct_config = ConfigManager.get("runner", "reconstruct")