import copy
import numpy as np
import open3d as o3
import utils
source, target = utils.prepare_source_and_target_rigid_3d('bunny.pcd')
vis = o3.Visualizer()
vis.create_window()
result = copy.deepcopy(source)
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 1, 0])
result.paint_uniform_color([0, 0, 1])
vis.add_geometry(source)
vis.add_geometry(target)
vis.add_geometry(result)
threshold = 0.05
icp_iteration = 100
save_image = False
for i in range(icp_iteration):
reg_p2p = o3.registration_icp(result, target, threshold, np.identity(4),
o3.TransformationEstimationPointToPoint(),
o3.ICPConvergenceCriteria(max_iteration=1))
result.transform(reg_p2l.transformation)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
if save_image:
vis.capture_screen_image("image_%04d.jpg" % i)
vis.run()
import open3d as o3
import utils
from probreg import cpd
from probreg import l2dist_regs
from probreg import gmmtree
from probreg import filterreg
threshold = 0.001
max_iteration = 100
source, target = utils.prepare_source_and_target_rigid_3d(
'bunny.pcd', n_random=0, orientation=np.deg2rad([0.0, 0.0, 10.0]))
start = timer()
res = o3.registration_icp(
source, target, threshold, np.identity(4),
o3.TransformationEstimationPointToPoint(),
o3.ICPConvergenceCriteria(max_iteration=max_iteration))
end = timer()
print('ICP(Open3D): ', end - start)
start = timer()
res = cpd.registration_cpd(source,
target,
maxiter=max_iteration,
tol=threshold)
end = timer()
print('CPD: ', end - start)
start = timer()
res = l2dist_regs.registration_svr(source,
target,
result = pn.registration_fast_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh,
pn.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
print result
print time.time() - start_time
cam_pose_pcl.transform(result.transformation)
source.transform(result.transformation)
source_down.transform(result.transformation)
pn.draw_geometries([source_down, target_down])
print cam_pose_pcl.points[0]
#%%
result_icp = pn.registration_icp(source, target, distance_threshold_icp,
np.eye(4),
pn.TransformationEstimationPointToPlane())
print result_icp
cam_pose_pcl.transform(result_icp.transformation)
source.transform(result_icp.transformation)
pn.draw_geometries([target_mesh, source])
print cam_pose_pcl.points[0]
err = camera_position - cam_pose_pcl.points[0]
err = np.sqrt(err.dot(err))
print err
#%%
import pyrealsense2 as rs
pipeline = rs.pipeline()
pipeline.start()
pc = rs.pointcloud()
source = geometry.uniform_down_sample(source, round(cloudSize / 10000))'''
print(source)
print(target)
current_transformation = np.identity(4)
draw_registration_result_original_color(source, target,
current_transformation)
myCriteria = o3.ICPConvergenceCriteria(1e-8, 1e-8, 100)
# point to point ICP
t1 = time.time()
current_transformation = np.identity(4)
print("Point-to-point ICP")
result_icp = o3.registration_icp(source, target, 0.25,
current_transformation,
o3.TransformationEstimationPointToPoint(),
myCriteria)
t2 = time.time()
print("Time Consumed: ", t2 - t1)
print(result_icp)
print(result_icp.transformation)
err_t, err_R = eval_err(result_icp.transformation, Tg)
print("err_t: ", err_t)
print("err_R: ", err_R)
print()
draw_registration_result_original_color(source, target,
result_icp.transformation)
# point to plane ICP
current_transformation = np.identity(4)
print("Point-to-plane ICP")