Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def preprocess_point_cloud(pcd, config):
voxel_size = config["voxel_size"]
pcd_down = pcd.voxel_down_sample(voxel_size)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
max_nn=30))
pcd_fpfh = o3d.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
max_nn=100))
return (pcd_down, pcd_fpfh)
def preprocess_point_cloud(pcd, voxel_size):
print(":: Downsample with a voxel size %.3f." % voxel_size)
pcd_down = pcd.voxel_down_sample(voxel_size)
radius_normal = voxel_size * 2
print(":: Estimate normal with search radius %.3f." % radius_normal)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxel_size * 5
print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
pcd_fpfh = o3d.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
return pcd_down, pcd_fpfh
result_icp = o3d.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.registration.TransformationEstimationPointToPlane(),
o3d.registration.ICPConvergenceCriteria(max_iteration=iter))
if config["icp_method"] == "color":
result_icp = o3d.registration.registration_colored_icp(
source_down, target_down, voxel_size[scale],
current_transformation,
o3d.registration.ICPConvergenceCriteria(
relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
current_transformation = result_icp.transformation
if i == len(max_iter) - 1:
information_matrix = o3d.registration.get_information_matrix_from_point_clouds(
source_down, target_down, voxel_size[scale] * 1.4,
result_icp.transformation)
if config["debug_mode"]:
draw_registration_result_original_color(source, target,
result_icp.transformation)
return (result_icp.transformation, information_matrix)
max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
if config["icp_method"] == "point_to_plane":
result_icp = o3d.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.registration.TransformationEstimationPointToPlane(),
o3d.registration.ICPConvergenceCriteria(max_iteration=iter))
if config["icp_method"] == "color":
result_icp = o3d.registration.registration_colored_icp(
source_down, target_down, voxel_size[scale],
current_transformation,
o3d.registration.ICPConvergenceCriteria(
relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
current_transformation = result_icp.transformation
if i == len(max_iter) - 1:
information_matrix = o3d.registration.get_information_matrix_from_point_clouds(
source_down, target_down, voxel_size[scale] * 1.4,
result_icp.transformation)
if config["debug_mode"]:
draw_registration_result_original_color(source, target,
result_icp.transformation)
return (result_icp.transformation, information_matrix)
def compute(self, data):
pcd = o3.geometry.PointCloud()
pcd.points = o3.utility.Vector3dVector(data)
self.estimate_normals(pcd)
fpfh = o3.registration.compute_fpfh_feature(pcd, self._param_feature)
return fpfh.data.T
def icp_registration(source, target, voxel_size, transform_init, max_iterations):
# See http://www.open3d.org/docs/release/tutorial/Basic/icp_registration.html#icp-registration
threshold = 8.0 * voxel_size
reg_p2p = o3d.registration.registration_icp(
source, target, threshold, transform_init,
o3d.registration.TransformationEstimationPointToPoint(),
o3d.registration.ICPConvergenceCriteria(max_iteration=max_iterations))
return reg_p2p
def register_point_cloud_fpfh(source, target, source_fpfh, target_fpfh, config):
distance_threshold = config["voxel_size"] * 1.4
if config["global_registration"] == "fgr":
result = o3d.registration.registration_fast_based_on_feature_matching(
source, target, source_fpfh, target_fpfh,
o3d.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
if config["global_registration"] == "ransac":
result = o3d.registration.registration_ransac_based_on_feature_matching(
source, target, source_fpfh, target_fpfh, distance_threshold,
o3d.registration.TransformationEstimationPointToPoint(False), 4, [
o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.registration.CorrespondenceCheckerBasedOnDistance(
distance_threshold)
], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
if (result.transformation.trace() == 4.0):
return (False, np.identity(4), np.zeros((6, 6)))
information = o3d.registration.get_information_matrix_from_point_clouds(
source, target, distance_threshold, result.transformation)
if information[5, 5] / min(len(source.points), len(target.points)) < 0.3:
return (False, np.identity(4), np.zeros((6, 6)))
return (True, result.transformation, information)
def update_posegrph_for_scene(s, t, transformation, information, odometry,
pose_graph):
if t == s + 1: # odometry case
odometry = np.dot(transformation, odometry)
odometry_inv = np.linalg.inv(odometry)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry_inv))
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s,
t,
transformation,
information,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s,
t,
transformation,
information,
uncertain=True))
return (odometry, pose_graph)
def execute_fast_global_registration(source_down, target_down, source_fpfh,
target_fpfh, voxel_size):
distance_threshold = voxel_size * 0.5
print(":: Apply fast global registration with distance threshold %.3f" \
% distance_threshold)
result = o3d.registration.registration_fast_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh,
o3d.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
return result