def gmmtree_pcd(self):
output_every_tf = 'gmmtree_every_tf.txt'
output_seq_tf = 'gmmtree_seq_tf.txt'
with open(output_every_tf, 'a+') as f:
f.write(original_tf)
f.close()
with open(output_seq_tf, 'a+') as f:
f.write(original_tf)
f.close()
for i in range(self.number - 1):
print(i)
#source_filename = 'kitti_raw_05/' + str(i) + '.pcd'
#target_filename = 'kitti_raw_05/' + str(i + 1) + '.pcd'
source_filename = '/media/chenxin/我不是硬盘/kitti_dataset/sequences/04/pcd/' + str(
i + 1) + '.pcd'
target_filename = '/media/chenxin/我不是硬盘/kitti_dataset/sequences/04/pcd/' + str(
i) + '.pcd'
source = o3.io.read_point_cloud(source_filename)
source = source.voxel_down_sample(voxel_size=VOXEL)
target = o3.io.read_point_cloud(target_filename)
target = target.voxel_down_sample(voxel_size=VOXEL)
np_source = np.asarray(source.points)
#s_points_after_tf_basis = self.mul_tf_basis(np_source)
s_points_after_tf_basis = self.calib_velo2cam(np_source)
source.points = o3.utility.Vector3dVector(s_points_after_tf_basis)
np_target = np.asarray(target.points)
#t_points_after_tf_basis = self.mul_tf_basis(np_target)
t_points_after_tf_basis = self.calib_velo2cam(np_target)
target.points = o3.utility.Vector3dVector(t_points_after_tf_basis)
start = timer()
tf_param, _ = gmmtree.registration_gmmtree(
source, target) #, maxiter=MAX_ITER, tol=TOL)
end = timer()
self.time.append(end - start)
curr_tf = self.collect_tf(tf_param)
seq_tf = np.dot(curr_tf, self.seq_tf)
self.seq_tf = seq_tf
self.output(output_every_tf, curr_tf)
self.output(output_seq_tf, seq_tf)
import numpy as np
import transforms3d as t3d
from probreg import gmmtree
from probreg import callbacks
import utils
source, target = utils.prepare_source_and_target_rigid_3d('bunny.pcd',
n_random=0)
cbs = [callbacks.Open3dVisualizerCallback(source, target)]
tf_param, _ = gmmtree.registration_gmmtree(source, target, callbacks=cbs)
print("result: ", np.rad2deg(t3d.euler.mat2euler(tf_param.rot)),
tf_param.scale, tf_param.t)
def register_gmmtree(source, target):
# tf_param, _ = gmmtree.registration_gmmtree(source, target, maxiter=1000, tree_level=6)
tf_param, _ = gmmtree.registration_gmmtree(source, target)
result = copy.deepcopy(source)
result.points = tf_param.transform(result.points)
return result, tf_param
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,
opt_maxiter=max_iteration,
opt_tol=threshold)
end = timer()
print('SVR: ', end - start)
start = timer()
res = gmmtree.registration_gmmtree(source,
target,
maxiter=max_iteration,
tol=threshold)
end = timer()
print('GMMTree: ', end - start)
start = timer()
res = filterreg.registration_filterreg(source,
target,
sigma2=None,
maxiter=max_iteration,
tol=threshold)
end = timer()
print('FilterReg: ', end - start)