267 lines
15 KiB
Python
267 lines
15 KiB
Python
import numpy as np
|
|
from scipy.spatial import cKDTree
|
|
from utils.pts import PtsUtil
|
|
|
|
class ReconstructionUtil:
|
|
|
|
@staticmethod
|
|
def compute_coverage_rate(target_point_cloud, combined_point_cloud, threshold=0.01):
|
|
kdtree = cKDTree(combined_point_cloud)
|
|
distances, _ = kdtree.query(target_point_cloud)
|
|
covered_points_num = np.sum(distances < threshold*2)
|
|
coverage_rate = covered_points_num / target_point_cloud.shape[0]
|
|
return coverage_rate, covered_points_num
|
|
|
|
@staticmethod
|
|
def compute_coverage_rate_with_normal(target_point_cloud, combined_point_cloud, target_normal, combined_normal, threshold=0.01, normal_threshold=0.1):
|
|
kdtree = cKDTree(combined_point_cloud)
|
|
distances, indices = kdtree.query(target_point_cloud)
|
|
is_covered_by_distance = distances < threshold*2
|
|
normal_dots = np.einsum('ij,ij->i', target_normal, combined_normal[indices])
|
|
is_covered_by_normal = normal_dots > normal_threshold
|
|
|
|
pts_nrm_target = np.hstack([target_point_cloud, target_normal])
|
|
np.savetxt("pts_nrm_target.txt", pts_nrm_target)
|
|
pts_nrm_combined = np.hstack([combined_point_cloud, combined_normal])
|
|
np.savetxt("pts_nrm_combined.txt", pts_nrm_combined)
|
|
import ipdb; ipdb.set_trace()
|
|
covered_points_num = np.sum(is_covered_by_distance & is_covered_by_normal)
|
|
coverage_rate = covered_points_num / target_point_cloud.shape[0]
|
|
|
|
return coverage_rate, covered_points_num
|
|
|
|
|
|
@staticmethod
|
|
def check_overlap(new_point_cloud, combined_point_cloud, overlap_area_threshold=25, voxel_size=0.01, require_new_added_pts_num=False):
|
|
kdtree = cKDTree(combined_point_cloud)
|
|
distances, _ = kdtree.query(new_point_cloud)
|
|
overlapping_points_num = np.sum(distances < voxel_size*2)
|
|
cm = 0.01
|
|
voxel_size_cm = voxel_size / cm
|
|
overlap_area = overlapping_points_num * voxel_size_cm * voxel_size_cm
|
|
if require_new_added_pts_num:
|
|
return overlap_area > overlap_area_threshold, len(new_point_cloud)-np.sum(distances < voxel_size*1.2)
|
|
return overlap_area > overlap_area_threshold
|
|
|
|
|
|
@staticmethod
|
|
def get_new_added_points(old_combined_pts, new_pts, threshold=0.005):
|
|
if old_combined_pts.size == 0:
|
|
return new_pts
|
|
if new_pts.size == 0:
|
|
return np.array([])
|
|
|
|
tree = cKDTree(old_combined_pts)
|
|
distances, _ = tree.query(new_pts, k=1)
|
|
new_added_points = new_pts[distances > threshold]
|
|
return new_added_points
|
|
|
|
@staticmethod
|
|
def compute_next_best_view_sequence(target_point_cloud, point_cloud_list, scan_points_indices_list, threshold=0.01, overlap_area_threshold=25, init_view = 0, scan_points_threshold=5, status_info=None):
|
|
selected_views = [init_view]
|
|
combined_point_cloud = point_cloud_list[init_view]
|
|
history_indices = [scan_points_indices_list[init_view]]
|
|
|
|
max_rec_pts = np.vstack(point_cloud_list)
|
|
downsampled_max_rec_pts = PtsUtil.voxel_downsample_point_cloud(max_rec_pts, threshold)
|
|
combined_point_cloud = PtsUtil.voxel_downsample_point_cloud(combined_point_cloud, threshold)
|
|
max_rec_pts_num = downsampled_max_rec_pts.shape[0]
|
|
max_real_rec_pts_coverage, _ = ReconstructionUtil.compute_coverage_rate(target_point_cloud, downsampled_max_rec_pts, threshold)
|
|
|
|
new_coverage, new_covered_num = ReconstructionUtil.compute_coverage_rate(downsampled_max_rec_pts, combined_point_cloud, threshold)
|
|
current_coverage = new_coverage
|
|
current_covered_num = new_covered_num
|
|
|
|
remaining_views = list(range(len(point_cloud_list)))
|
|
view_sequence = [(init_view, current_coverage)]
|
|
cnt_processed_view = 0
|
|
remaining_views.remove(init_view)
|
|
curr_rec_pts_num = combined_point_cloud.shape[0]
|
|
drop_output_ratio = 0.4
|
|
|
|
import time
|
|
while remaining_views:
|
|
best_view = None
|
|
best_coverage_increase = -1
|
|
best_combined_point_cloud = None
|
|
best_covered_num = 0
|
|
|
|
for view_index in remaining_views:
|
|
if np.random.rand() < drop_output_ratio:
|
|
continue
|
|
if point_cloud_list[view_index].shape[0] == 0:
|
|
continue
|
|
if selected_views:
|
|
new_scan_points_indices = scan_points_indices_list[view_index]
|
|
if not ReconstructionUtil.check_scan_points_overlap(history_indices, new_scan_points_indices, scan_points_threshold):
|
|
curr_overlap_area_threshold = overlap_area_threshold
|
|
else:
|
|
curr_overlap_area_threshold = overlap_area_threshold * 0.5
|
|
|
|
if not ReconstructionUtil.check_overlap(point_cloud_list[view_index], combined_point_cloud, overlap_area_threshold = curr_overlap_area_threshold, voxel_size=threshold):
|
|
continue
|
|
|
|
new_combined_point_cloud = np.vstack([combined_point_cloud, point_cloud_list[view_index]])
|
|
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)
|
|
coverage_increase = new_coverage - current_coverage
|
|
if coverage_increase > best_coverage_increase:
|
|
best_coverage_increase = coverage_increase
|
|
best_view = view_index
|
|
best_covered_num = new_covered_num
|
|
best_combined_point_cloud = new_downsampled_combined_point_cloud
|
|
|
|
|
|
if best_view is not None:
|
|
if best_coverage_increase <=1e-3 or best_covered_num - current_covered_num <= 5:
|
|
break
|
|
|
|
selected_views.append(best_view)
|
|
best_rec_pts_num = best_combined_point_cloud.shape[0]
|
|
print(f"Current rec pts num: {curr_rec_pts_num}, Best rec pts num: {best_rec_pts_num}, Best cover pts: {best_covered_num}, Max rec pts num: {max_rec_pts_num}")
|
|
print(f"Current coverage: {current_coverage+best_coverage_increase}, Best coverage increase: {best_coverage_increase}, Max Real coverage: {max_real_rec_pts_coverage}")
|
|
current_covered_num = best_covered_num
|
|
curr_rec_pts_num = best_rec_pts_num
|
|
combined_point_cloud = best_combined_point_cloud
|
|
remaining_views.remove(best_view)
|
|
history_indices.append(scan_points_indices_list[best_view])
|
|
current_coverage += best_coverage_increase
|
|
cnt_processed_view += 1
|
|
if status_info is not None:
|
|
sm = status_info["status_manager"]
|
|
app_name = status_info["app_name"]
|
|
runner_name = status_info["runner_name"]
|
|
sm.set_status(app_name, runner_name, "current coverage", current_coverage)
|
|
sm.set_progress(app_name, runner_name, "processed view", cnt_processed_view, len(point_cloud_list))
|
|
|
|
view_sequence.append((best_view, current_coverage))
|
|
|
|
else:
|
|
break
|
|
if status_info is not None:
|
|
sm = status_info["status_manager"]
|
|
app_name = status_info["app_name"]
|
|
runner_name = status_info["runner_name"]
|
|
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_sequence_with_normal(target_point_cloud, target_normal, point_cloud_list, normal_list, scan_points_indices_list, threshold=0.01, overlap_area_threshold=25, init_view = 0, scan_points_threshold=5, status_info=None):
|
|
selected_views = [init_view]
|
|
combined_point_cloud = point_cloud_list[init_view]
|
|
combined_normal = normal_list[init_view]
|
|
history_indices = [scan_points_indices_list[init_view]]
|
|
|
|
max_rec_pts = np.vstack(point_cloud_list)
|
|
max_rec_nrm = np.vstack(normal_list)
|
|
downsampled_max_rec_pts, idx = PtsUtil.voxel_downsample_point_cloud(max_rec_pts, threshold, require_idx=True)
|
|
downsampled_max_rec_nrm = max_rec_nrm[idx]
|
|
max_rec_pts_num = downsampled_max_rec_pts.shape[0]
|
|
try:
|
|
max_real_rec_pts_coverage, _ = ReconstructionUtil.compute_coverage_rate_with_normal(target_point_cloud, downsampled_max_rec_pts, target_normal, downsampled_max_rec_nrm, threshold)
|
|
except:
|
|
import ipdb; ipdb.set_trace()
|
|
|
|
new_coverage, new_covered_num = ReconstructionUtil.compute_coverage_rate_with_normal(downsampled_max_rec_pts, combined_point_cloud, downsampled_max_rec_nrm, combined_normal, threshold)
|
|
current_coverage = new_coverage
|
|
current_covered_num = new_covered_num
|
|
|
|
remaining_views = list(range(len(point_cloud_list)))
|
|
view_sequence = [(init_view, current_coverage)]
|
|
cnt_processed_view = 0
|
|
remaining_views.remove(init_view)
|
|
curr_rec_pts_num = combined_point_cloud.shape[0]
|
|
|
|
while remaining_views:
|
|
best_view = None
|
|
best_coverage_increase = -1
|
|
best_combined_point_cloud = None
|
|
best_combined_normal = None
|
|
best_covered_num = 0
|
|
|
|
for view_index in remaining_views:
|
|
if point_cloud_list[view_index].shape[0] == 0:
|
|
continue
|
|
if selected_views:
|
|
new_scan_points_indices = scan_points_indices_list[view_index]
|
|
if not ReconstructionUtil.check_scan_points_overlap(history_indices, new_scan_points_indices, scan_points_threshold):
|
|
curr_overlap_area_threshold = overlap_area_threshold
|
|
else:
|
|
curr_overlap_area_threshold = overlap_area_threshold * 0.5
|
|
|
|
if not ReconstructionUtil.check_overlap(point_cloud_list[view_index], combined_point_cloud, overlap_area_threshold = curr_overlap_area_threshold, voxel_size=threshold):
|
|
continue
|
|
|
|
new_combined_point_cloud = np.vstack([combined_point_cloud, point_cloud_list[view_index]])
|
|
new_combined_normal = np.vstack([combined_normal, normal_list[view_index]])
|
|
new_downsampled_combined_point_cloud, idx = PtsUtil.voxel_downsample_point_cloud(new_combined_point_cloud,threshold, require_idx=True)
|
|
new_downsampled_combined_normal = new_combined_normal[idx]
|
|
new_coverage, new_covered_num = ReconstructionUtil.compute_coverage_rate_with_normal(downsampled_max_rec_pts, new_downsampled_combined_point_cloud, downsampled_max_rec_nrm, new_downsampled_combined_normal, threshold)
|
|
coverage_increase = new_coverage - current_coverage
|
|
if coverage_increase > best_coverage_increase:
|
|
best_coverage_increase = coverage_increase
|
|
best_view = view_index
|
|
best_covered_num = new_covered_num
|
|
best_combined_point_cloud = new_downsampled_combined_point_cloud
|
|
best_combined_normal = new_downsampled_combined_normal
|
|
|
|
|
|
if best_view is not None:
|
|
if best_coverage_increase <=1e-3 or best_covered_num - current_covered_num <= 5:
|
|
break
|
|
|
|
selected_views.append(best_view)
|
|
best_rec_pts_num = best_combined_point_cloud.shape[0]
|
|
print(f"Current rec pts num: {curr_rec_pts_num}, Best rec pts num: {best_rec_pts_num}, Best cover pts: {best_covered_num}, Max rec pts num: {max_rec_pts_num}")
|
|
print(f"Current coverage: {current_coverage}, Best coverage increase: {best_coverage_increase}, Max Real coverage: {max_real_rec_pts_coverage}")
|
|
current_covered_num = best_covered_num
|
|
curr_rec_pts_num = best_rec_pts_num
|
|
combined_point_cloud = best_combined_point_cloud
|
|
combined_normal = best_combined_normal
|
|
remaining_views.remove(best_view)
|
|
history_indices.append(scan_points_indices_list[best_view])
|
|
current_coverage += best_coverage_increase
|
|
cnt_processed_view += 1
|
|
if status_info is not None:
|
|
sm = status_info["status_manager"]
|
|
app_name = status_info["app_name"]
|
|
runner_name = status_info["runner_name"]
|
|
sm.set_status(app_name, runner_name, "current coverage", current_coverage)
|
|
sm.set_progress(app_name, runner_name, "processed view", cnt_processed_view, len(point_cloud_list))
|
|
|
|
view_sequence.append((best_view, current_coverage))
|
|
|
|
else:
|
|
break
|
|
if status_info is not None:
|
|
sm = status_info["status_manager"]
|
|
app_name = status_info["app_name"]
|
|
runner_name = status_info["runner_name"]
|
|
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 generate_scan_points(display_table_top, display_table_radius, min_distance=0.03, max_points_num = 500, max_attempts = 1000):
|
|
points = []
|
|
attempts = 0
|
|
while len(points) < max_points_num and attempts < max_attempts:
|
|
angle = np.random.uniform(0, 2 * np.pi)
|
|
r = np.random.uniform(0, display_table_radius)
|
|
x = r * np.cos(angle)
|
|
y = r * np.sin(angle)
|
|
z = display_table_top
|
|
new_point = (x, y, z)
|
|
if all(np.linalg.norm(np.array(new_point) - np.array(existing_point)) >= min_distance for existing_point in points):
|
|
points.append(new_point)
|
|
attempts += 1
|
|
return points
|
|
|
|
@staticmethod
|
|
def check_scan_points_overlap(history_indices, indices2, threshold=5):
|
|
for indices1 in history_indices:
|
|
if len(set(indices1).intersection(set(indices2))) >= threshold:
|
|
return True
|
|
return False
|
|
|
|
|