update
This commit is contained in:
parent
41ee79db0c
commit
2f87a2626c
15
combine_all_pts.py
Normal file
15
combine_all_pts.py
Normal file
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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")
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user