update
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user