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()
