hofee/nbv_rec_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add close loop control

Browse Source
This commit is contained in:
hofee
2024-10-12 23:11:25 +08:00
parent 8d43d4de60
commit 07dcdb3452
4 changed files with 294 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
utils/reconstruction_util.py
View File

@@ -121,7 +121,34 @@ class ReconstructionUtil:
sm.set_progress(app_name, runner_name, "processed view", len(point_cloud_list), len(point_cloud_list))
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):
max_rec_pts = np.vstack(point_cloud_list)
downsampled_max_rec_pts = PtsUtil.voxel_downsample_point_cloud(max_rec_pts, threshold)
best_view = None
best_coverage = -1
best_covered_num = 0
for view in range(len(point_cloud_list)):
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
else:
overlap_threshold = soft_overlap_threshold
overlap_rate = ReconstructionUtil.compute_overlap_rate(point_cloud_list[view], scanned_pts, threshold)
if overlap_rate < overlap_threshold:
continue
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:
best_coverage = new_coverage
best_covered_num = new_covered_num
best_view = view
return best_view, best_coverage, best_covered_num
@staticmethod
def generate_scan_points(display_table_top, display_table_radius, min_distance=0.03, max_points_num = 500, max_attempts = 1000):
points = []