diff --git a/combine_all_pts.py b/combine_all_pts.py new file mode 100644 index 0000000..e38c63e --- /dev/null +++ b/combine_all_pts.py @@ -0,0 +1,15 @@ +import numpy as np +import os + +if __name__ == "__main__": + pts_dir_path = "/home/yan20/nbv_rec/project/franka_control/temp_output/cad_model_world/pts" + pts_dir = os.listdir(pts_dir_path) + pts_list = [] + for i in range(len(pts_dir)): + pts_path = os.path.join(pts_dir_path, pts_dir[i]) + pts = np.loadtxt(pts_path) + pts_list.append(pts) + combined_pts = np.vstack(pts_list) + path = "/home/yan20/nbv_rec/project/franka_control" + np.savetxt(os.path.join(path, "combined_pts.txt"), combined_pts) + \ No newline at end of file diff --git a/configs/cad_close_loop_config.yaml b/configs/cad_close_loop_config.yaml index 6641e31..98ffd61 100644 --- a/configs/cad_close_loop_config.yaml +++ b/configs/cad_close_loop_config.yaml @@ -19,8 +19,8 @@ runner: max_shot_view_num: 50 min_shot_new_pts_num: 10 min_coverage_increase: 0.001 - max_view: 512 - min_view: 128 + max_view: 64 + min_view: 32 max_diag: 0.7 min_diag: 0.01 random_view_ratio: 0 diff --git a/runners/cad_close_loop_strategy.py b/runners/cad_close_loop_strategy.py index e853ae8..ef0f447 100644 --- a/runners/cad_close_loop_strategy.py +++ b/runners/cad_close_loop_strategy.py @@ -44,7 +44,7 @@ class CADCloseLoopStrategyRunner(Runner): "min_cam_table_included_degree" ] self.max_shot_view_num = self.generate_config["max_shot_view_num"] - self.min_shot_new_pts_num = self.generate_config["max_shot_new_pts_num"] + self.min_shot_new_pts_num = self.generate_config["min_shot_new_pts_num"] self.min_coverage_increase = self.generate_config["min_coverage_increase"] self.random_view_ratio = self.generate_config["random_view_ratio"] @@ -151,12 +151,16 @@ class CADCloseLoopStrategyRunner(Runner): scan_points_idx_list.append(indices) """ close-loop strategy """ - scanned_pts = [first_real_world_pts] + scanned_pts = PtsUtil.voxel_downsample_point_cloud( + first_splitted_real_world_pts, self.voxel_size + ) + shot_pts_list = [first_splitted_real_world_pts] history_indices = [] last_coverage = 0 - last_covered_num = 0 Log.info("[Part 4/4] start close-loop control") + cnt = 0 while True: + #import ipdb; ipdb.set_trace() next_best_view, next_best_coverage, next_best_covered_num = ( ReconstructionUtil.compute_next_best_view_with_overlap( scanned_pts, @@ -164,8 +168,7 @@ class CADCloseLoopStrategyRunner(Runner): history_indices, scan_points_idx_list, threshold=self.voxel_size, - soft_overlap_threshold=self.soft_overlap_threshold, - hard_overlap_threshold=self.hard_overlap_threshold, + overlap_area_threshold=25, scan_points_threshold=self.scan_points_threshold, ) ) @@ -176,28 +179,50 @@ class CADCloseLoopStrategyRunner(Runner): ''' get world pts ''' time.sleep(0.5) view_data = CommunicateUtil.get_view_data() - world_shot_pts = ViewUtil.get_pts(view_data) - scanned_pts.append(world_shot_pts) + if view_data is None: + Log.error("No view data received") + continue + cam_shot_pts = ViewUtil.get_pts(view_data) + world_shot_pts = PtsUtil.transform_point_cloud( + cam_shot_pts, first_cam_to_real_world + ) + _, world_splitted_shot_pts = self.split_scan_pts_and_obj_pts( + world_shot_pts + ) + shot_pts_list.append(world_splitted_shot_pts) + + debug_dir = os.path.join(temp_dir, "debug") + if not os.path.exists(debug_dir): + os.makedirs(debug_dir) + np.savetxt(os.path.join(debug_dir, f"shot_pts_{cnt}.txt"), world_splitted_shot_pts) + np.savetxt(os.path.join(debug_dir, f"render_pts_{cnt}.txt"), sample_view_pts_list[next_best_view]) + #real_world_to_cad = PtsUtil.register(first_splitted_real_world_pts, cad_model) + #import ipdb; ipdb.set_trace() + last_scanned_pts_num = scanned_pts.shape[0] + new_scanned_pts = PtsUtil.voxel_downsample_point_cloud( + np.vstack([scanned_pts, world_splitted_shot_pts]), self.voxel_size + ) + new_scanned_pts_num = new_scanned_pts.shape[0] history_indices.append(scan_points_idx_list[next_best_view]) + scanned_pts = new_scanned_pts Log.info( - f"Current rec pts num: {len(scanned_pts)}, Best cover pts: {next_best_covered_num}, Best coverage: {next_best_coverage}" + f"Next Best cover pts: {next_best_covered_num}, Best coverage: {next_best_coverage}" ) coverage_rate_increase = next_best_coverage - last_coverage if coverage_rate_increase < self.min_coverage_increase: Log.info(f"Coverage rate = {coverage_rate_increase} < {self.min_coverage_increase}, stop scanning") - break + # break last_coverage = next_best_coverage - new_added_pts_num = next_best_covered_num - last_covered_num + new_added_pts_num = new_scanned_pts_num - last_scanned_pts_num if new_added_pts_num < self.min_shot_new_pts_num: - Log.info(f"New added pts num = {new_added_pts_num} < {self.min_shot_new_pts_num}, stop scanning") - break - last_covered_num = next_best_covered_num - - if len(scanned_pts) >= self.max_shot_view_num: - Log.info(f"Scanned view num = {len(scanned_pts)} >= {self.max_shot_view_num}, stop scanning") - break + Log.info(f"New added pts num = {new_added_pts_num} < {self.min_shot_new_pts_num}") + #ipdb.set_trace() + if len(shot_pts_list) >= self.max_shot_view_num: + Log.info(f"Scanned view num = {len(shot_pts_list)} >= {self.max_shot_view_num}, stop scanning") + #break + cnt += 1 Log.success("[Part 4/4] finish close-loop control") diff --git a/utils/pts_util.py b/utils/pts_util.py index ffe58d9..ba7c10d 100644 --- a/utils/pts_util.py +++ b/utils/pts_util.py @@ -117,66 +117,6 @@ class PtsUtil: filtered_sampled_points = sampled_points[idx] return filtered_sampled_points[:, :3] - @staticmethod - def old_register(pcl: np.ndarray, model: trimesh.Trimesh, voxel_size=0.002): - radius_normal = voxel_size * 3 - 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_icp = pipreg.registration_icp( - source_downsampled, - model_downsampled, - voxel_size/2, - reg_ransac.transformation, - pipreg.TransformationEstimationPointToPlane(), - pipreg.ICPConvergenceCriteria(max_iteration=2000), - ) - - return reg_icp.transformation - - @staticmethod - def chamfer_distance(pcl_a, pcl_b): - distances = np.linalg.norm(pcl_a[:, None] - pcl_b, axis=2) - min_distances = np.min(distances, axis=1) - return np.sum(min_distances) - @staticmethod def multi_scale_icp( source, target, voxel_size_range, init_transformation=None, steps=20 diff --git a/utils/reconstruction_util.py b/utils/reconstruction_util.py index 5a4868c..7a1d16e 100644 --- a/utils/reconstruction_util.py +++ b/utils/reconstruction_util.py @@ -13,15 +13,14 @@ class ReconstructionUtil: return coverage_rate, covered_points_num @staticmethod - def compute_overlap_rate(new_point_cloud, combined_point_cloud, threshold=0.01): + def check_overlap(new_point_cloud, combined_point_cloud, overlap_area_threshold=25, voxel_size=0.01): kdtree = cKDTree(combined_point_cloud) distances, _ = kdtree.query(new_point_cloud) - overlapping_points = np.sum(distances < threshold*2) - if new_point_cloud.shape[0] == 0: - overlap_rate = 0 - else: - overlap_rate = overlapping_points / new_point_cloud.shape[0] - return overlap_rate + overlapping_points = np.sum(distances < voxel_size*2) + cm = 0.01 + voxel_size_cm = voxel_size / cm + overlap_area = overlapping_points * voxel_size_cm * voxel_size_cm + return overlap_area > overlap_area_threshold @staticmethod @@ -122,7 +121,7 @@ class ReconstructionUtil: return view_sequence, remaining_views, combined_point_cloud @staticmethod - def compute_next_best_view_with_overlap(scanned_pts:list, point_cloud_list, history_indices, scan_points_indices_list, threshold=0.01, soft_overlap_threshold=0.5, hard_overlap_threshold=0.7, scan_points_threshold=5): + def compute_next_best_view_with_overlap(scanned_pts, point_cloud_list, history_indices, scan_points_indices_list, threshold=0.01, overlap_area_threshold=25, scan_points_threshold=5): max_rec_pts = np.vstack(point_cloud_list) downsampled_max_rec_pts = PtsUtil.voxel_downsample_point_cloud(max_rec_pts, threshold) best_view = None @@ -132,14 +131,17 @@ class ReconstructionUtil: if point_cloud_list[view].shape[0] == 0: continue new_scan_points_indices = scan_points_indices_list[view] + if not ReconstructionUtil.check_scan_points_overlap(history_indices, new_scan_points_indices, scan_points_threshold): - overlap_threshold = hard_overlap_threshold + curr_overlap_area_threshold = overlap_area_threshold else: - overlap_threshold = soft_overlap_threshold - overlap_rate = ReconstructionUtil.compute_overlap_rate(point_cloud_list[view], scanned_pts, threshold) - if overlap_rate < overlap_threshold: + curr_overlap_area_threshold = overlap_area_threshold * 0.5 + + if not ReconstructionUtil.check_overlap(point_cloud_list[view], scanned_pts, overlap_area_threshold = curr_overlap_area_threshold, voxel_size=threshold): continue - new_combined_point_cloud = np.vstack(scanned_pts + [point_cloud_list[view]]) + + + new_combined_point_cloud = np.vstack([scanned_pts ,point_cloud_list[view]]) new_downsampled_combined_point_cloud = PtsUtil.voxel_downsample_point_cloud(new_combined_point_cloud,threshold) new_coverage, new_covered_num = ReconstructionUtil.compute_coverage_rate(downsampled_max_rec_pts, new_downsampled_combined_point_cloud, threshold) if new_coverage > best_coverage: diff --git a/utils/view_util.py b/utils/view_util.py index 63df3ae..585681d 100644 --- a/utils/view_util.py +++ b/utils/view_util.py @@ -91,6 +91,7 @@ class CameraData: points = np.linalg.inv(self.intrinsics) @ points # 3xN points = points.T # Nx3 points = points * self.depth_image.flatten()[:, np.newaxis] # Nx3 + points = points[points[:, 2] > 0] # Nx3 return points @property