def main(): argvs = sys.argv model_num = str(argvs[1]) scene_num = str(argvs[2]) cloud_dir = "/mnt/container-data/remove_plane/" model_path = cloud_dir + model_num + ".ply" model_cloud = o3.read_point_cloud(model_path) scene_path = cloud_dir + scene_num + ".ply" scene_cloud = o3.read_point_cloud(scene_path) rot_in = read_rotation(model_num) model_cloud.transform(rot_in) model_cloud = o3.voxel_down_sample(model_cloud, voxel_size=0.020) scene_cloud = o3.voxel_down_sample(scene_cloud, voxel_size=0.020) # 基準とするpointcloud, 回転させたいpointcloud の順番 cbs = [callbacks.Open3dVisualizerCallback(model_cloud, scene_cloud)] objective_type = 'pt2pt' # 基準とするpointcloud, 回転させたいpointcloud の順番 tf_param, _, _ = filterreg.registration_filterreg( model_cloud, scene_cloud, scene_cloud.normals, objective_type=objective_type, sigma2=sig, callbacks=cbs, maxiter=ter, tol=ol) write_rotation(tf_param, scene_num)
def test_filterreg_registration_pt2pl(self): res = filterreg.registration_filterreg(self._source, self._target, self._target_normals) 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=2.0e-1, rtol=1.0e-1)) self.assertTrue(np.allclose(res.transformation.t, self._tf.t, atol=1.0e-2, rtol=1.0e-3))
def register(self, pcd1, pcd2): f_reg = filterreg.registration_filterreg(pcd1, pcd2) trans = f_reg.transformation scale_matrix = np.identity(4) * trans.scale transformation_matrix = np.identity(4) transformation_matrix[0:3, 0:3] = trans.rot transformation_matrix[0:3, 3] = trans.t transformation_matrix *= trans.scale return transformation_matrix
def filterreg_pcd(self): output_every_tf = 'filterreg_every_tf.txt' output_seq_tf = 'filterreg_seq_tf.txt' with open(output_every_tf, 'a+') as f: f.write(original_tf_08) f.close() with open(output_seq_tf, 'a+') as f: f.write(original_tf_08) 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/08/pcd/' + str( i + 1) + '.pcd' target_filename = '/media/chenxin/我不是硬盘/kitti_dataset/sequences/08/pcd/' + str( i) + '.pcd' source = o3.io.read_point_cloud(source_filename) source = source.voxel_down_sample( voxel_size=VOXEL) #voxel 0.47/0.48 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() objective_type = 'pt2pt' ''' tf_param, _, _ = filterreg.registration_filterreg(source, target, objective_type=objective_type, maxiter=MAX_ITER, tol=TOL, sigma2=None) ''' # default: maxiter = 50, tol = 1e-3 tf_param, _, _ = filterreg.registration_filterreg( source, target, objective_type=objective_type, sigma2=0.16, w=0.1) # sigma = 0.2, w = 0.1 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)
def register_filterreg_feature(source, target, objective_type='pt2pt'): tf_param, _, _ = filterreg.registration_filterreg( source, target, objective_type=objective_type, sigma2=1000, feature_fn=features.FPFH()) result = copy.deepcopy(source) result.points = tf_param.transform(result.points) return result, tf_param
def test_filterreg_registration_pt2pt(self): res = filterreg.registration_filterreg(self._source, self._target) self.assertTrue( np.allclose(t3d.euler.mat2euler(res.transformation.rot), t3d.euler.mat2euler(self._tf.rot), atol=2.0e-1, rtol=1.0e-1)) self.assertTrue( np.allclose(res.transformation.t, self._tf.t, atol=1.0e-2, rtol=1.0e-3))
import numpy as np import transforms3d as t3d from probreg import filterreg from probreg import callbacks import utils source, target = utils.prepare_source_and_target_rigid_3d('cloud_0.pcd', n_random=0, normals=True) cbs = [callbacks.Open3dVisualizerCallback(source, target)] objective_type = 'pt2pl' tf_param, _, _ = filterreg.registration_filterreg( source, target, target.normals, objective_type=objective_type, sigma2=0.01, callbacks=cbs) print("result: ", np.rad2deg(t3d.euler.mat2euler(tf_param.rot)), tf_param.scale, tf_param.t)
import numpy as np import transformations as trans from probreg import filterreg from probreg import callbacks import utils source, target = utils.prepare_source_and_target_rigid_3d('bunny.pcd') cbs = [callbacks.Open3dVisualizerCallback(source, target)] tf_param, _, _ = filterreg.registration_filterreg(source, target, sigma2=None, 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)
import numpy as np import transformations as trans from probreg import filterreg from probreg import features from probreg import callbacks import utils source, target = utils.prepare_source_and_target_rigid_3d( 'bunny.pcd', orientation=np.deg2rad([0.0, 0.0, 80.0]), translation=np.array([0.5, 0.0, 0.0]), n_random=0) cbs = [callbacks.Open3dVisualizerCallback(source, target)] objective_type = 'pt2pt' tf_param, _, _ = filterreg.registration_filterreg( source, target, objective_type=objective_type, sigma2=1000, feature_fn=features.FPFH(), 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)
#waymopair = WaymoLIDARPair(max_frames=150, as_pc = True, voxel_size = 0.5, filename='../../waymodata/segment-10206293520369375008_2796_800_2816_800_with_camera_labels.tfrecord') waymopair = WaymoLIDARPairReg(max_frames=100, filename='../data/waymo_pcs.npy') done = False all_trans = [] vis = WaymoLIDARVisCallback() idx = 0 while True: 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) objective_type = 'pt2pt' tf_param, _, _ = filterreg.registration_filterreg( prev_pc, curr_pc, objective_type=objective_type, sigma2=None) 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)
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)
from probreg import filterreg from probreg import callbacks import matplotlib.pyplot as plt import utils import numpy as np source, target = utils.prepare_source_and_target_nonrigid_2d( 'fish_source.txt', 'fish_target.txt') cbs = [callbacks.Plot2DCallback(source, target)] tf_param, _, _ = filterreg.registration_filterreg(source, target, objective_type="pt2pt", callbacks=cbs, tf_init_params={ "rot": np.identity(2), "t": np.zeros(2) }) plt.show()
import numpy as np import transforms3d as t3d from probreg import filterreg from probreg import callbacks import utils source, target = utils.prepare_source_and_target_rigid_3d('bunny.pcd') cbs = [callbacks.Open3dVisualizerCallback(source, target)] objective_type = 'pt2pt' tf_param, _, _ = filterreg.registration_filterreg( source, target, objective_type=objective_type, sigma2=None, update_sigma2=True, callbacks=cbs) print("result: ", np.rad2deg(t3d.euler.mat2euler(tf_param.rot)), tf_param.scale, tf_param.t)
''' ''' # cpd print('Registration method: cpd') start = timer() tf_param, _, _ = cpd.registration_cpd(source, target, maxiter=MAX_ITER, tol=TOL) end = timer() ''' # filterreg print('Registration method: filterreg') print('init sigma = 0.4, init w = 0.1') start = timer() objective_type = 'pt2pt' tf_param, _, _ = filterreg.registration_filterreg( source, target, objective_type=objective_type, sigma2=0.16, w=0.1) #, maxiter=MAX_ITER, tol=TOL) end = timer() ''' # svr print('Registration method: svr') start = timer() tf_param = l2dist_regs.registration_svr(source, target) end = timer() ''' ''' # gmmreg print('Registration method: gmmreg') start = timer() tf_param = l2dist_regs.registration_gmmreg(source, target) end = timer()