def test_cpd_registration(self):
res = cpd.registration_cpd(self._source, self._target)
res_rot = trans.identity_matrix()
res_rot[:3, :3] = res.transformation.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-2, rtol=1.0e-2))
self.assertTrue(np.allclose(res.transformation.t, self._tf.t, atol=1.0e-4, rtol=1.0e-4))
def FAFB_to_FlyEM(object, source, target, tftype='affine'):
object = cp.asarray(object, dtype=cp.float32)
print('object',object[0])
source = cp.asarray(source, dtype=cp.float32)
print('source', source[0])
target = cp.asarray(target, dtype=cp.float32)
print('target', target[0])
tf_param, _, _ = cpd.registration_cpd(source, target, tf_type_name=tftype, use_cuda=use_cuda)
# tf_param = bcpd.registration_bcpd(source, target)
result = tf_param.transform(object)
return to_cpu(result), tf_param
def test_cpd_registration(self):
res = cpd.registration_cpd(self._source, self._target)
self.assertTrue(
np.allclose(res.transformation.rot,
self._tf.rot,
atol=1.0e-4,
rtol=1.0e-4))
self.assertTrue(
np.allclose(res.transformation.t,
self._tf.t,
atol=1.0e-4,
rtol=1.0e-4))
def FCWB_to_FC(object, source, target, tftype='affine'):
object = [[xyz[0]*1.7-475, xyz[1]*-2 + 185, xyz[2]*-2.5 + 125] for xyz in object]
# object = [[xyz[0] * 1, xyz[1] * -1, xyz[2] * -1] for xyz in object]
object = cp.asarray(object, dtype=cp.float32)
source = [[xyz[0]*1.7-475, xyz[1]*-2 + 185, xyz[2]*-2.5 + 125] for xyz in source]
# source = [[xyz[0] * 1, xyz[1] * -1, xyz[2] * -1] for xyz in source]
source = cp.asarray(source, dtype=cp.float32)
target = cp.asarray(target, dtype=cp.float32)
# print(source)
# print(target)
tf_param, _, _ = cpd.registration_cpd(source, target, tf_type_name=tftype, use_cuda=use_cuda)
result = tf_param.transform(object)
return to_cpu(result), tf_param
def affine_cpd(source, target, threshold=0.5):
# Affine CPD using probreg
#### NOTE:
#### Increase threshold value to increase processing speed
#### Results in more downsampled point cloud
source = source.voxel_down_sample(voxel_size=threshold)
target = target.voxel_down_sample(voxel_size=threshold)
print("Starting CPD")
#o3d.visualization.draw_geometries([source, target])
tf_param, _, _ = cpd.registration_cpd(source, target, 'affine')
#tf_param = l2dist_regs.registration_svr(source, target, 'nonrigid')
print("Done!")
return tf_param
def cpd_rigid(self):
output_every_tf = 'cpd_every_tf.txt'
output_seq_tf = 'cpd_seq_tf.txt'
with open(output_every_tf, 'a+') as f:
f.write(original_tf_03)
f.close()
with open(output_seq_tf, 'a+') as f:
f.write(original_tf_03)
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/03/pcd/' + str(
i + 1) + '.pcd'
target_filename = '/media/chenxin/我不是硬盘/kitti_dataset/sequences/03/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, _, _ = cpd.registration_cpd(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 open3d as o3
import transformations as trans
from probreg import cpd
from probreg import callbacks
import utils
import logging
log = logging.getLogger('probreg')
log.setLevel(logging.DEBUG)
source, target = utils.prepare_source_and_target_rigid_3d('bunny.pcd')
cbs = [callbacks.Open3dVisualizerCallback(source, target)]
tf_param, _, _ = cpd.registration_cpd(source, target, callbacks=cbs)
rot = trans.identity_matrix()
rot[:3, :3] = tf_param.rot
print("result: ", np.rad2deg(trans.euler_from_matrix(rot)), tf_param.scale,
tf_param.t)
def register_cpd(source, target):
tf_param, _, _ = cpd.registration_cpd(source, target, update_scale=False)
result = copy.deepcopy(source)
result.points = tf_param.transform(result.points)
return result, tf_param
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.pipelines.registration.registration_icp(
source, target, 0.5, np.identity(4),
o3.pipelines.registration.TransformationEstimationPointToPoint(),
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,
import copy
import numpy as np
import open3d as o3
from probreg import cpd
# load source and target point cloud
source = o3.io.read_point_cloud(
'/home/eddyod/programming/probreg/examples/bunny.pcd')
target = copy.deepcopy(source)
# transform target point cloud
th = np.deg2rad(30.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]]))
source = source.voxel_down_sample(voxel_size=0.005)
target = target.voxel_down_sample(voxel_size=0.005)
# compute cpd registration
tf_param, _, _ = cpd.registration_cpd(source, target)
result = copy.deepcopy(source)
result.points = tf_param.transform(result.points)
# draw result
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 1, 0])
result.paint_uniform_color([0, 0, 1])
o3.visualization.draw_geometries([source, target, result])
def register(self, pcd1, pcd2):
reg = cpd.registration_cpd(pcd1, pcd2)
return reg.transformation
def FC_to_FCWB(source, target, tftype='affine'):
source = [[(xyz[0] * + 475)/1.7, (xyz[1] -185) / -2, (xyz[2]-125) / -2.5] for xyz in source]
source = cp.asarray(source, dtype=cp.float32)
target = cp.asarray(target, dtype=cp.float32)
tf_param, _, _ = cpd.registration_cpd(source, target, tf_type_name=tftype, use_cuda=use_cuda)
return tf_param
