def icp_registration_error(sourcepc, targetpc, threshold=0.02):
source_o3dpc = o3d.geometry.PointCloud()
source_o3dpc.points = o3d.utility.Vector3dVector(sourcepc.position)
target_o3dpc = o3d.geometry.PointCloud()
target_o3dpc.points = o3d.utility.Vector3dVector(targetpc.position)
threshold = 0.02
draw_registration_result(source, target, trans_init)
print("Initial alignment")
evaluation = o3d.evaluate_registration(source, target, threshold,
trans_init)
print('evaluation:', evaluation)
print("Apply point-to-point ICP")
reg_p2p = o3d.registration_icp(
source,
target,
threshold,
init=np.eye(4),
estimation_method=o3d.TransformationEstimationPointToPoint())
print(reg_p2p)
print("reg_p2p Transformation is:")
print(reg_p2p.transformation)
print("transformation type is :", type(reg_p2p.transformation))
target.transform(trans_init)
draw_registration_result(source, target, reg_p2p.transformation)
def icp(self, source, target):
source = source.astype(np.float32)
target = target.astype(np.float32)
threshold = self.threshold
trans_init = np.asarray([[1.0, 0.0, 0.0, 0.2], [0.0, 1.0, 0.0, 0.1],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
pc_source = open3d.PointCloud()
pc_source.points = open3d.Vector3dVector(source)
pc_target = open3d.PointCloud()
pc_target.points = open3d.Vector3dVector(target)
evaluation = open3d.evaluate_registration(pc_source, pc_target,
threshold, trans_init)
print(evaluation)
print("Apply point-to-point ICP")
reg_p2p = open3d.registration_icp(
pc_source, pc_target, threshold, trans_init,
open3d.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
pc_source.transform(reg_p2p.transformation)
source = np.asarray(pc_source.points)
return reg_p2p.transformation
def stitch_pointclouds_open3D(self, target, source, threshold = 1, trans_init = np.eye(4), max_iteration = 2000, with_plot = True):
source = self.check_pointcloud_type(np.array(source))
target = self.check_pointcloud_type(np.array(target))
target.paint_uniform_color([0.1, 0.1, 0.7])
print dir(open3d)
if with_plot:
self.draw_registration_result(source, target, trans_init)
print("Initial alignment")
evaluation = open3d.evaluate_registration(source, target, threshold, trans_init)
print(evaluation)
print("Apply point-to-point ICP")
reg_p2p = open3d.registration_icp(source, target, threshold, trans_init,
open3d.TransformationEstimationPointToPoint(),
open3d.ICPConvergenceCriteria(max_iteration = max_iteration))
print("Transformation is:")
print(reg_p2p.transformation)
print("")
if with_plot:
self.draw_registration_result(source, target, reg_p2p.transformation)
xyz_source = np.asarray(source.points)
xyz_target = np.asarray(target.points)
xyz_global = np.concatenate([xyz_source, xyz_target], axis = 0)
return xyz_global
sfile = os.path.join(dirs.inpdir, 'open3d/mean_face.ply')
mesh = open3d.read_triangle_mesh(sfile)
source = mesh2pcd(mesh)
open3d.draw_geometries([target])
open3d.draw_geometries([source])
trans_init = np.asarray([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
draw_registration_result(source, target, trans_init)
print("Initial alignment")
evaluation = open3d.evaluate_registration(source, target, np.Inf, trans_init)
print(evaluation)
print("Apply point-to-point ICP")
reg_p2p = open3d.registration_icp(source, target, np.Inf, trans_init, open3d.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
print("")
draw_registration_result(source, target, reg_p2p.transformation)
print("Apply point-to-plane ICP")
reg_p2l = open3d.registration_icp(source, target, np.Inf, trans_init, open3d.TransformationEstimationPointToPlane())
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation)
source, target, source_down, target_down, source_fpfh, target_fpfh = fgr.prepare_dataset(
voxel_size, pointclouds[0], pointclouds[1])
start = time.time()
result_fast = fgr.execute_fast_global_registration(source_down, target_down,
source_fpfh, target_fpfh,
voxel_size)
print("Fast global registration took %.3f sec.\n" % (time.time() - start))
fgr.draw_registration_result(source_down, target_down,
result_fast.transformation)
# start accurate registration
threshold = 0.5
trans_init = result_fast.transformation
print("Initial alignment")
evaluation = open3d.evaluate_registration(source, target, threshold,
trans_init)
print(evaluation)
print("Apply point-to-point ICP")
reg_p2p = open3d.registration_icp(
source, target, threshold, trans_init,
open3d.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
print("")
fgr.draw_registration_result(source, target, reg_p2p.transformation)
print("Apply point-to-plane ICP")
reg_p2l = open3d.registration_icp(
source, target, threshold, trans_init,
pc.iloc[:, :3].to_csv(xyz_paths[idx], sep=",", header=None, index=None)
pc_prev = open3d.read_point_cloud(xyz_paths[0])
R_prev = np.eye(4)
cur_ego = pd.DataFrame(np.zeros((1, 6)), dtype=np.float)
cur_ego.iloc[0, :].T.to_csv(xyz_paths[0].replace('.xyz', '_egomotion.csv'),
sep=",",
header=None,
index=None)
print(cur_ego)
for xyz_path in tqdm(xyz_paths[1:]):
pc_cur = open3d.read_point_cloud(xyz_path)
# evaluation = open3d.evaluate_registration(pc_prev, pc_cur, threshold, trans_init) # was used until saturday
evaluation = open3d.evaluate_registration(pc_cur, pc_prev, threshold,
trans_init)
print("Apply point-to-point ICP to: \n{}".format(xyz_path))
# reg_p2p = open3d.registration_icp(pc_prev, pc_cur, threshold, trans_init, open3d.TransformationEstimationPointToPoint(), ) # was used until saturday
reg_p2p = open3d.registration_icp(
pc_cur,
pc_prev,
threshold,
trans_init,
open3d.TransformationEstimationPointToPoint(),
)
print(reg_p2p)
print("Transformation is:")
R_cur = reg_p2p.transformation
print(R_cur)
R = np.matmul(R_cur, R_prev)
rot = extract_rotation(R)
