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