def icp_o3_gicp(file_idx,
cfg,
refine=None,
refine_radius=0.05,
precomputed_results=None):
pc1, pc2, pc1_centroid = load_pountclouds(file_idx, cfg)
voxel_size = 0.05
start = time.time()
if precomputed_results is None:
distance_threshold = voxel_size * 1.5
source_down, target_down, source_fpfh, target_fpfh = ICP._icp_global_prepare_dataset(
pc1, pc2, voxel_size)
reg_res = o3.registration_ransac_based_on_feature_matching(
source_down,
target_down,
source_fpfh,
target_fpfh,
distance_threshold,
o3.TransformationEstimationPointToPoint(
with_constraint=cfg.evaluation.special.icp.with_constraint,
with_scaling=False),
4, # scaling=False
[
o3.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3.CorrespondenceCheckerBasedOnDistance(distance_threshold)
],
o3.RANSACConvergenceCriteria(4000000, 500))
transformation = reg_res.transformation
else:
precomp_pred_translation, precomp_pred_angle, precomp_pred_center = precomputed_results
transformation = get_mat_angle(precomp_pred_translation,
precomp_pred_angle, precomp_pred_center)
if refine is None:
time_elapsed = time.time() - start
return transformation, pc1_centroid, time_elapsed
else:
if refine == 'p2p':
reg_p2p = o3.registration_icp(
pc1, pc2, refine_radius, transformation,
o3.TransformationEstimationPointToPoint(
with_constraint=cfg.evaluation.special.icp.with_constraint,
with_scaling=False))
# if file_idx == 8019:
# print('->', reg_p2p.transformation)
time_elapsed = time.time() - start
return reg_p2p.transformation, pc1_centroid, time_elapsed
else:
assert False
def _perform_registration(self):
import open3d
def array2pcd(xyz):
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(xyz)
return pcd
fixed = array2pcd(self.fixed_points)
moving = array2pcd(self.moving_points)
init = np.asarray([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
regp2p = open3d.registration_icp(
moving, fixed, np.Inf, init,
open3d.TransformationEstimationPointToPoint())
self.transform = regp2p.transformation
print(regp2p)
moving_points = np.concatenate(
(self.moving_points, np.ones((self.number_of_moving_points, 1))),
axis=1)
self.moving_points = moving_points @ np.transpose(
self.transform)[:, :3]
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 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
def execute_global_registration(source_down: op3.PointCloud,
target_down: op3.PointCloud,
source_fpfh: op3.PointCloud,
target_fpfh: op3.PointCloud,
voxel_size: float,
verbose=False):
"""
Do global matching, find gross transformation form source to target
:param source_down, target_down: 2 objects of Open3D, that are point clouds of source and target after down-sampling
:param source_fpfh, target_fpfh: 2 objects of Open3D, that are point cloud features of source and target
:param voxel_size: a float value, that is how sparse you want the sample points is
:param verbose: a boolean value, display notification and visualization when True and no notification when False
:return: a transformation object
"""
distance_threshold = voxel_size * 1.5
if verbose:
print(":: RANSAC registration on downsampled point clouds.")
print(" Since the downsampling voxel size is %.3f," % voxel_size)
print(" we use a liberal distance threshold %.3f." %
distance_threshold)
result = op3.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh, distance_threshold,
op3.TransformationEstimationPointToPoint(False), 4, [
op3.CorrespondenceCheckerBasedOnEdgeLength(0.9),
op3.CorrespondenceCheckerBasedOnDistance(distance_threshold)
], op3.RANSACConvergenceCriteria(4000000, 500))
return result
def solve(datArray, prm):
global scnFtArray, scnPcArray, Param
Param.update(prm)
scnFtArray = []
scnPcArray = []
for dat in datArray:
pc = fromNumpy(dat)
scnPcArray.append(pc)
scnFtArray.append(_get_features(pc))
t1 = time.time()
resft = o3.registration_ransac_based_on_feature_matching(
modPcArray[0], scnPcArray[0], modFtArray[0], scnFtArray[0],
Param["distance_threshold"],
o3.TransformationEstimationPointToPoint(False), 4, [
o3.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3.CorrespondenceCheckerBasedOnDistance(
Param["distance_threshold"])
], o3.RANSACConvergenceCriteria(2000000, 500))
print "time for feature matching", time.time() - t1
print "feature matching", resft.transformation, resft.fitness
resicp = o3.registration_icp(modPcArray[0], scnPcArray[0],
Param["icp_threshold"], resft.transformation,
o3.TransformationEstimationPointToPlane())
return {
"transform": [resicp.transformation],
"fitness": [resicp.fitness],
"rmse": [resicp.inlier_rmse]
}
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 refine_registration(scene1, scene2, trans, voxel_size, max_iteration):
distance_threshold = voxel_size * 0.4
result = open3d.registration_icp(
scene1, scene2, distance_threshold, trans,
open3d.TransformationEstimationPointToPoint(),
open3d.ICPConvergenceCriteria(max_iteration=100))
return result
def getImmovTf(T,p0): p1=transform(T,p0) pc0=fromNumpy(p0) pc1=fromNumpy(p1) reg=o3d.registration_icp(pc1,pc0,param['icp_threshold'],np.eye(4,dtype=float),o3d.TransformationEstimationPointToPoint()) # print reg.fitness # print reg.transformation return np.dot(reg.transformation,T),p1
def execute_global_registration(src_keypts, tgt_keypts, src_desc, tgt_desc, distance_threshold):
result = open3d.registration_ransac_based_on_feature_matching(
src_keypts, tgt_keypts, src_desc, tgt_desc,
distance_threshold,
open3d.TransformationEstimationPointToPoint(False), 4,
[open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9),
open3d.CorrespondenceCheckerBasedOnDistance(distance_threshold)],
open3d.RANSACConvergenceCriteria(4000000, 500))
return result
def execute_global_registration(kp1, kp2, fpfh1, fpfh2, voxel_size):
distance_threshold = voxel_size * 2.5
result = open3d.registration_ransac_based_on_feature_matching(
kp1, kp2, fpfh1, fpfh2, distance_threshold,
open3d.TransformationEstimationPointToPoint(False), 4, [
open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9),
open3d.CorrespondenceCheckerBasedOnDistance(distance_threshold)
], open3d.RANSACConvergenceCriteria(500000, 1000))
return result
def registration_point_to_point(scene1, scene2, voxel_size):
# scene1 and 2 are point cloud data
# voxel_size is grid size
#draw_registration_result(scene1,scene2,np.identity(4))
# voxel down sampling
scene1_down = open3d.voxel_down_sample(scene1, voxel_size)
scene2_down = open3d.voxel_down_sample(scene2, voxel_size)
#draw_registration_result(scene1_down,scene2_down,np.identity(4))
# estimate normal with search radius voxel_size*2.0
radius_normal = voxel_size * 2.0
open3d.estimate_normals(
scene1, open3d.KDTreeSearchParamHybrid(radius=radius_normal,
max_nn=30))
open3d.estimate_normals(
scene2, open3d.KDTreeSearchParamHybrid(radius=radius_normal,
max_nn=30))
open3d.estimate_normals(
scene1_down,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
open3d.estimate_normals(
scene2_down,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
# compute fpfh feature with search radius voxel_size/2.0
radius_feature = voxel_size * 2.0
scene1_fpfh = open3d.compute_fpfh_feature(
scene1_down,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
scene2_fpfh = open3d.compute_fpfh_feature(
scene2_down,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
# compute ransac registration
ransac_result = execute_global_registration(scene1_down, scene2_down,
scene1_fpfh, scene2_fpfh,
voxel_size)
#draw_registration_result(scene1,scene2,ransac_result.transformation)
# point to point ICP resigtration
distance_threshold = voxel_size * 0.4
result = open3d.registration_icp(
scene1, scene2, distance_threshold, ransac_result.transformation,
open3d.TransformationEstimationPointToPoint(),
open3d.ICPConvergenceCriteria(max_iteration=1000))
#draw_registration_result(scene1,scene2,result.transformation)
print(result)
return result
def refine_registration(source, target, trans, voxel_size):
global all_transformation
distance_threshold = voxel_size * 0.4
print(":: Point-to-point ICP registration is applied on original point")
print(" clouds to refine the alignment. This time we use a strict")
print(" distance threshold %.3f." % distance_threshold)
result = o3.registration_icp(source, target, distance_threshold, trans,
o3.TransformationEstimationPointToPoint())
return result.transformation
def registration_icp(source,
target,
threshold,
transformation=np.eye(4),
**kwargs):
# call the open3d registration_icp
reg_p2p = open3d.registration_icp(
source, target, threshold, transformation,
open3d.TransformationEstimationPointToPoint(),
open3d.ICPConvergenceCriteria(max_iteration=30))
return reg_p2p.transformation
def refine_registration(source, target, corrs):
start = time.time()
p2p = o3d.TransformationEstimationPointToPoint()
result = p2p.compute_transformation(source, target,
o3d.Vector2iVector(corrs))
elapsed_time = time.time() - start
return result, elapsed_time
def icp_refine(source_pc, target_pc, trans, dis):
"""
use icp to refine the rigid transformation.
"""
result = open3d.registration_icp(
source=source_pc,
target=target_pc,
max_correspondence_distance=dis,
init=trans,
estimation_method=open3d.TransformationEstimationPointToPoint(False)
)
return result
def register(self, iteration=None, voxel_size=None):
iteration = 100 if iteration is None else iteration
voxel_size = 0.01 if voxel_size is None else voxel_size
source, target = self._prepare(voxel_size=voxel_size)
result = open3d.registration_icp(
source, # points_from_depth
target, # points_from_cad
2 * voxel_size,
tf.inverse_matrix(self._transform),
open3d.TransformationEstimationPointToPoint(False),
open3d.ICPConvergenceCriteria(max_iteration=iteration),
)
return tf.inverse_matrix(result.transformation)
def match_():
source = copy.deepcopy(modPcArray[0])
source_fpfh = modFtArray[0]
target = scnPcArray[0]
target_fpfh = scnFtArray[0]
reg_global = o3d.registration_ransac_based_on_feature_matching(
source, target, source_fpfh, target_fpfh, param['distance_threshold'],
o3d.TransformationEstimationPointToPoint(False), 4, [
o3d.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.CorrespondenceCheckerBasedOnDistance(
param['distance_threshold'])
], o3d.RANSACConvergenceCriteria(4000000, 500))
source.transform(reg_global.transformation)
return reg_global, [source]
def open3d_icp(src, trgt, init_rotation=np.eye(3, 3)):
source = open3d.PointCloud()
source.points = open3d.Vector3dVector(src)
target = open3d.PointCloud()
target.points = open3d.Vector3dVector(trgt)
init_rotation_4x4 = np.eye(4, 4)
init_rotation_4x4[0:3, 0:3] = init_rotation
threshold = 0.2
reg_p2p = open3d.registration_icp(source, target, threshold, init_rotation_4x4,
open3d.TransformationEstimationPointToPoint())
return reg_p2p
def icp_two_pc(source, target, draw_result=False, point2plane=False):
"""
return target * result = source
:param source:
:param target:
:param draw_result:
:param point2plane:
:return:
"""
if isinstance(source, PointCloud):
tmp = source
source = o3d.PointCloud()
source.points = o3d.Vector3dVector(tmp.position)
elif isinstance(source, np.ndarray):
tmp = source
source = o3d.PointCloud()
source.points = o3d.Vector3dVector(tmp)
if isinstance(target, PointCloud):
tmp = target
target = o3d.PointCloud()
target.points = o3d.Vector3dVector(tmp.position)
elif isinstance(target, np.ndarray):
tmp = target
target = o3d.PointCloud()
target.points = o3d.Vector3dVector(tmp)
if point2plane:
result = o3d.registration_icp(
source,
target,
0.002,
estimation_method=o3d.TransformationEstimationPointToPlane())
else:
print('using point to point icp method')
result = o3d.registration_icp(
source,
target,
0.002,
estimation_method=o3d.TransformationEstimationPointToPoint())
transformation = result.transformation
print('result transformation:', transformation)
if draw_result:
source.transform(
transformation
) # source point cloud apply the transformation to get target point cloud
draw_registration_result(source, target)
return transformation
def execute_global_registration(source_down, target_down, source_fpfh,
target_fpfh, voxel_size):
distance_threshold = voxel_size * 1.5
print(":: RANSAC registration on downsampled point clouds.")
print(" Since the downsampling voxel size is %.3f," % voxel_size)
print(" we use a liberal distance threshold %.3f." % distance_threshold)
result = op3.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh, distance_threshold,
op3.TransformationEstimationPointToPoint(False), 4, [
op3.CorrespondenceCheckerBasedOnEdgeLength(0.9),
op3.CorrespondenceCheckerBasedOnDistance(distance_threshold)
],
op3.RANSACConvergenceCriteria(4000000, 500)
# op3.RANSACConvergenceCriteria(10000000, 20000)
)
return result
def update_source_to_target_transformation(self):
"""
Function to update the source to target transformation matrix
"""
source = copy.deepcopy(self.source_pcl)
target = pn.voxel_down_sample(self.target_pcl, target_voxel_size)
if self.source_to_target_transformation.size > 0:
# if the global registration have already been done
source.transform(self.source_to_target_transformation)
icp_result = pn.registration_icp(
source, target, distance_threshold_icp, np.eye(4),
pn.TransformationEstimationPointToPlane())
if icp_result.fitness > 0.8:
self.source_to_target_transformation =\
icp_result.transformation.dot(self.source_to_target_transformation)
return
# we fail to align source & target with ICP
# GLOBAL REGISTRATION
# PREPROCESSING POINT CLOUD
source = copy.deepcopy(self.source_pcl)
source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)
# GLOBAL REGISTRATION
ransac_result = pn.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh,
distance_threshold_ransac,
pn.TransformationEstimationPointToPoint(False), 4, [
pn.CorrespondenceCheckerBasedOnDistance(
distance_threshold_ransac)
], pn.RANSACConvergenceCriteria(10000000, 10000))
source.transform(ransac_result.transformation)
icp_result = pn.registration_icp(
source, target, distance_threshold_icp, np.eye(4),
pn.TransformationEstimationPointToPlane())
self.source_to_target_transformation = \
icp_result.transformation.dot(ransac_result.transformation)
return
def register(self):
start = time.time()
result = open3d.registration_ransac_based_on_feature_matching(
self.source_open3d, self.target_open3d, self.source_fpfh,
self.target_fpfh, self.epsilon,
open3d.TransformationEstimationPointToPoint(False), 4, [
open3d.CorrespondenceCheckerBasedOnEdgeLength(0.95),
open3d.CorrespondenceCheckerBasedOnDistance(self.epsilon)
],
open3d.RANSACConvergenceCriteria(self.max_iteration,
self.max_validation))
end = time.time()
self.time = end - start
self.transformation = result.transformation
return
def get_evaluation(pcd_temp_,
pcd_scene_,
inlier_thres,
tf,
final_th=0,
n_iter=5): #queue
tf_pcd = np.eye(4)
reg_p2p = open3d.registration_icp(
pcd_temp_, pcd_scene_, inlier_thres, np.eye(4),
open3d.TransformationEstimationPointToPoint(),
open3d.ICPConvergenceCriteria(max_iteration=1)) #5?
tf = np.matmul(reg_p2p.transformation, tf)
tf_pcd = np.matmul(reg_p2p.transformation, tf_pcd)
pcd_temp_.transform(reg_p2p.transformation)
for i in range(4):
inlier_thres = reg_p2p.inlier_rmse * 3
if inlier_thres == 0:
continue
reg_p2p = open3d.registration_icp(
pcd_temp_, pcd_scene_, inlier_thres, np.eye(4),
open3d.TransformationEstimationPointToPlane(),
open3d.ICPConvergenceCriteria(max_iteration=1)) #5?
tf = np.matmul(reg_p2p.transformation, tf)
tf_pcd = np.matmul(reg_p2p.transformation, tf_pcd)
pcd_temp_.transform(reg_p2p.transformation)
inlier_rmse = reg_p2p.inlier_rmse
##Calculate fitness with depth_inlier_th
if (final_th > 0):
inlier_thres = final_th #depth_inlier_th*2 #reg_p2p.inlier_rmse*3
reg_p2p = registration_icp(
pcd_temp_, pcd_scene_, inlier_thres, np.eye(4),
TransformationEstimationPointToPlane(),
ICPConvergenceCriteria(max_iteration=1)) #5?
if (np.abs(np.linalg.det(tf[:3, :3]) - 1) > 0.001):
tf[:3, 0] = tf[:3, 0] / np.linalg.norm(tf[:3, 0])
tf[:3, 1] = tf[:3, 1] / np.linalg.norm(tf[:3, 1])
tf[:3, 2] = tf[:3, 2] / np.linalg.norm(tf[:3, 2])
if (np.linalg.det(tf) < 0):
tf[:3, 2] = -tf[:3, 2]
return tf, inlier_rmse, tf_pcd, reg_p2p.fitness
def icp_p2point(file_idx,
cfg,
radius=0.2,
its=30,
init=None,
with_constraint=None):
with_constraint = with_constraint if with_constraint is not None else cfg.evaluation.special.icp.with_constraint
pc1, pc2, pc1_centroid = load_pountclouds(file_idx, cfg)
if init is None:
# init = get_median_init(pc1, pc2)
init = get_centroid_init(pc1, pc2)
start = time.time()
reg_p2p = o3.registration_icp(pc1, pc2, radius, init,
o3.TransformationEstimationPointToPoint(
with_constraint=with_constraint,
with_scaling=False),
o3.registration.ICPConvergenceCriteria(
max_iteration=its)) # Default: 30
time_elapsed = time.time() - start
return reg_p2p.transformation, pc1_centroid, time_elapsed
def register_iterative(self, iteration=None, voxel_size=None):
iteration = 100 if iteration is None else iteration
voxel_size = 0.01 if voxel_size is None else voxel_size
yield self._transform
source, target = self._prepare(voxel_size=voxel_size)
for i in range(iteration):
result = open3d.registration_icp(
source, # points_from_depth
target, # points_from_cad
2 * voxel_size,
tf.inverse_matrix(self._transform),
open3d.TransformationEstimationPointToPoint(False),
open3d.ICPConvergenceCriteria(max_iteration=1),
)
print(f"[{i:08d}] fitness={result.fitness:.2g} "
f"inlier_rmse={result.inlier_rmse:.2g}")
self._transform = tf.inverse_matrix(result.transformation)
yield self._transform
def _perform_registration(self):
if self.registration == 0:
self.registration_name = 'None'
return
fixed = open3dmeshopen3dpcd(self.fixed)
moving = open3dmeshopen3dpcd(self.moving)
if self.registration == 1:
estimator = open3d.TransformationEstimationPointToPoint()
else:
estimator = open3d.TransformationEstimationPointToPlane()
init = np.asarray([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
registration = open3d.registration_icp(moving, fixed, np.Inf, init,
estimator)
self.registration_name = type(estimator)
self.transform = registration.transformation
self.moving.transform(self.transform)
def transformation_matrix(source_points, target_points):
"""
Compute the transformation matrix between to set of matching points.
Parameters:
source_points (numpy.arrays): the source points array
target_points (numpy.arrays): the target points array matching the
source
Returns:
transformation_matrix (numpy.arrays) : The 4D transformation matrix from source to target
"""
assert (len(source_points) >= 3
and len(target_points) == len(source_points))
source = pn.PointCloud()
source.points = pn.Vector3dVector(source_points)
target = pn.PointCloud()
target.points = pn.Vector3dVector(target_points)
corr = np.array(2 * [range(len(source_points))]).T
p2p = pn.TransformationEstimationPointToPoint()
trans = p2p.compute_transformation(source, target, pn.Vector2iVector(corr))
return trans
def icp_o3_gicp_fast(file_idx,
cfg,
refine=None,
refine_radius=0.05,
precomputed_results=None):
pc1, pc2, pc1_centroid = load_pountclouds(file_idx, cfg)
voxel_size = 0.05
distance_threshold = voxel_size * 0.5
start = time.time()
if precomputed_results is None:
source_down, target_down, source_fpfh, target_fpfh = ICP._icp_global_prepare_dataset(
pc1, pc2, voxel_size)
reg_res = o3.registration_fast_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh,
o3.FastGlobalRegistrationOption(
with_constraint=cfg.evaluation.special.icp.with_constraint,
maximum_correspondence_distance=distance_threshold))
transformation = reg_res.transformation
else:
precomp_pred_translation, precomp_pred_angle, precomp_pred_center = precomputed_results
transformation = get_mat_angle(precomp_pred_translation,
precomp_pred_angle, precomp_pred_center)
if refine is None:
time_elapsed = time.time() - start
return transformation, pc1_centroid, time_elapsed
else:
if refine == 'p2p':
reg_p2p = o3.registration_icp(
pc1, pc2, refine_radius, transformation,
o3.TransformationEstimationPointToPoint(
with_constraint=cfg.evaluation.special.icp.with_constraint,
with_scaling=False))
time_elapsed = time.time() - start
return reg_p2p.transformation, pc1_centroid, time_elapsed
else:
assert False
# transform target point cloud
th = np.deg2rad(10.0)
target.transform(np.array([[np.cos(th), -np.sin(th), 0.0, 0.0],
[np.sin(th), np.cos(th), 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]))
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_p2p.transformation)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
if save_image:
vis.capture_screen_image("image_%04d.jpg" % i)
vis.run()
