def test_svr_registration(self):
res = l2dist_regs.registration_svr(self._source, self._target)
res_rot = trans.identity_matrix()
res_rot[:3, :3] = res.rot
ref_rot = trans.identity_matrix()
ref_rot[:3, :3] = self._tf.rot
self.assertTrue(np.allclose(trans.euler_from_matrix(res_rot),
trans.euler_from_matrix(ref_rot), atol=1.0e-1, rtol=1.0e-1))
self.assertTrue(np.allclose(res.t, self._tf.t, atol=1.0e-2, rtol=1.0e-3))
def svr_pcd(self):
output_every_tf = 'svr_every_tf.txt'
output_seq_tf = 'svr_seq_tf.txt'
with open(output_every_tf, 'a+') as f:
f.write(original_tf_09)
f.close()
with open(output_seq_tf, 'a+') as f:
f.write(original_tf_09)
f.close()
for i in range(self.number - 1):
print(i)
source_filename = '/media/chenxin/我不是硬盘/kitti_dataset/sequences/09/pcd/' + str(
i + 1) + '.pcd'
target_filename = '/media/chenxin/我不是硬盘/kitti_dataset/sequences/09/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 = l2dist_regs.registration_svr(source, target)
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)
waymopair = WaymoLIDARPair(
max_frames=150,
as_pc=True,
voxel_size=0.5,
filename=
'/home/mlab-dhruv/Desktop/pointclouds/waymodata/segment-10206293520369375008_2796_800_2816_800_with_camera_labels.tfrecord'
)
done = False
all_trans = []
vis = WaymoLIDARVisCallback()
idx = 0
while True:
prev_np, curr_np, prev_pc, curr_pc, done = waymopair.next_pair()
if done:
break
#1: Compute L2 Gaussian Registration & Transform chain
tf_param = l2dist_regs.registration_svr(prev_pc, curr_pc)
all_trans.append(copy.deepcopy(tf_param.inverse()))
if idx == 0:
result = copy.deepcopy(curr_pc)
else:
result = convert_np_to_pc(
np.linspace(-0.03, 0.01, num=20 * 3).reshape(-1, 3))
result.colors = o3.utility.Vector3dVector(
np.ones((20 * 3)).reshape(-1, 3) * np.array([1, 0, 0]))
# idx+=1
for tf in reversed(all_trans):
result.points = tf.transform(result.points)
vis(result, addpc=True)
[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]])
all_trans = []
#1: a)Load Source & Target Pointclouds b) Transform Target c) Downsample for speed
source = o3.read_point_cloud(filepath)
target = copy.deepcopy(source)
target.transform(TRANSLATE_02x)
target.transform(ROT_30z)
source = o3.voxel_down_sample(source, voxel_size=0.003)
target = o3.voxel_down_sample(target, voxel_size=0.003)
#2: Compute L2 Gaussian Registration
# cbs = [callbacks.Open3dVisualizerCallback(source, target)]
tf_param = l2dist_regs.registration_svr(source, target)
result = copy.deepcopy(source)
# result.points = tf_param.transform(result.points)
for i in range(4):
result.transform(TRANSLATE_005x)
result.transform(ROT_15z)
result.transform(ROT_15z)
print(tf_param.rot)
print(tf_param.t)
#3: draw result
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 1, 0])
result.paint_uniform_color([0, 0, 1])
import numpy as np
import transforms3d as t3d
from probreg import l2dist_regs
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 = l2dist_regs.registration_svr(source, target, callbacks=cbs)
print("result: ", np.rad2deg(t3d.euler.mat2euler(tf_param.rot)),
tf_param.scale, tf_param.t)
def register(self, pcd1, pcd2):
reg_svr = l2dist_regs.registration_svr(pcd1, pcd2)
return reg_svr.transformation
def test_svr_registration(self):
res = l2dist_regs.registration_svr(self._source, self._target)
self.assertTrue(np.allclose(t3d.euler.mat2euler(res.rot),
t3d.euler.mat2euler(self._tf.rot), atol=1.0e-1, rtol=1.0e-1))
self.assertTrue(np.allclose(res.t, self._tf.t, atol=1.0e-2, rtol=1.0e-3))
def register_svr(source, target):
tf_param = l2dist_regs.registration_svr(source, target)
result = copy.deepcopy(source)
result.points = tf_param.transform(result.points)
return result, tf_param
o3.pipelines.registration.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,
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,