def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
if not source_temp.has_normals():
estimate_normal(source_temp)
estimate_normal(target_temp)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target_temp])
def extract_fpfh_features(pcd_path, downsample, device):
raw_src_pcd = o3d.io.read_point_cloud(pcd_path)
estimate_normal(raw_src_pcd, radius=downsample * 2)
src_pcd = raw_src_pcd.voxel_down_sample(downsample)
src_features = o3d.registration.compute_fpfh_feature(
src_pcd,
o3d.geometry.KDTreeSearchParamHybrid(radius=downsample * 5,
max_nn=100))
src_features = np.array(src_features.data).T
src_features = src_features / (
np.linalg.norm(src_features, axis=1, keepdims=True) + 1e-6)
return raw_src_pcd, np.array(src_pcd.points), src_features
def __getitem__(self, index):
# load meta data
filename = self.ids_list[index]
data = np.load(filename)
src_keypts = data['xyz0']
tgt_keypts = data['xyz1']
src_features = data['features0']
tgt_features = data['features1']
if self.descriptor == 'fpfh':
src_features = src_features / (
np.linalg.norm(src_features, axis=1, keepdims=True) + 1e-6)
tgt_features = tgt_features / (
np.linalg.norm(tgt_features, axis=1, keepdims=True) + 1e-6)
# compute ground truth transformation
orig_trans = data['gt_trans']
# data augmentation
if self.split == 'train':
src_keypts += np.random.rand(src_keypts.shape[0], 3) * 0.05
tgt_keypts += np.random.rand(tgt_keypts.shape[0], 3) * 0.05
aug_R = rotation_matrix(self.augment_axis, self.augment_rotation)
aug_T = translation_matrix(self.augment_translation)
aug_trans = integrate_trans(aug_R, aug_T)
tgt_keypts = transform(tgt_keypts, aug_trans)
gt_trans = concatenate(aug_trans, orig_trans)
# select {self.num_node} numbers of keypoints
N_src = src_features.shape[0]
N_tgt = tgt_features.shape[0]
src_sel_ind = np.arange(N_src)
tgt_sel_ind = np.arange(N_tgt)
if self.num_node != 'all' and N_src > self.num_node:
src_sel_ind = np.random.choice(N_src, self.num_node, replace=False)
if self.num_node != 'all' and N_tgt > self.num_node:
tgt_sel_ind = np.random.choice(N_tgt, self.num_node, replace=False)
src_desc = src_features[src_sel_ind, :]
tgt_desc = tgt_features[tgt_sel_ind, :]
src_keypts = src_keypts[src_sel_ind, :]
tgt_keypts = tgt_keypts[tgt_sel_ind, :]
# construct the correspondence set by mutual nn in feature space.
distance = np.sqrt(2 - 2 * (src_desc @ tgt_desc.T) + 1e-6)
source_idx = np.argmin(distance, axis=1)
if self.use_mutual:
target_idx = np.argmin(distance, axis=0)
mutual_nearest = (target_idx[source_idx] == np.arange(
source_idx.shape[0]))
corr = np.concatenate([
np.where(mutual_nearest == 1)[0][:, None],
source_idx[mutual_nearest][:, None]
],
axis=-1)
else:
corr = np.concatenate(
[np.arange(source_idx.shape[0])[:, None], source_idx[:, None]],
axis=-1)
# compute the ground truth label
frag1 = src_keypts[corr[:, 0]]
frag2 = tgt_keypts[corr[:, 1]]
frag1_warp = transform(frag1, gt_trans)
distance = np.sqrt(np.sum(np.power(frag1_warp - frag2, 2), axis=1))
labels = (distance < self.inlier_threshold).astype(np.int)
# add random outlier to input data
if self.split == 'train' and np.mean(labels) > 0.5:
num_outliers = int(0.0 * len(corr))
src_outliers = np.random.randn(num_outliers, 3) * np.mean(
src_keypts, axis=0)
tgt_outliers = np.random.randn(num_outliers, 3) * np.mean(
tgt_keypts, axis=0)
input_src_keypts = np.concatenate(
[src_keypts[corr[:, 0]], src_outliers], axis=0)
input_tgt_keypts = np.concatenate(
[tgt_keypts[corr[:, 1]], tgt_outliers], axis=0)
labels = np.concatenate([labels, np.zeros(num_outliers)], axis=0)
else:
# prepare input to the network
input_src_keypts = src_keypts[corr[:, 0]]
input_tgt_keypts = tgt_keypts[corr[:, 1]]
if self.in_dim == 3:
corr_pos = input_src_keypts - input_tgt_keypts
elif self.in_dim == 6:
corr_pos = np.concatenate([input_src_keypts, input_tgt_keypts],
axis=-1)
# move the center of each point cloud to (0,0,0).
corr_pos = corr_pos - corr_pos.mean(0)
elif self.in_dim == 9:
corr_pos = np.concatenate([
input_src_keypts, input_tgt_keypts,
input_src_keypts - input_tgt_keypts
],
axis=-1)
elif self.in_dim == 12:
src_pcd = make_point_cloud(src_keypts)
tgt_pcd = make_point_cloud(tgt_keypts)
estimate_normal(src_pcd, radius=self.downsample * 2)
estimate_normal(tgt_pcd, radius=self.downsample * 2)
src_normal = np.array(src_pcd.normals)
tgt_normal = np.array(tgt_pcd.normals)
src_normal = src_normal[src_sel_ind, :]
tgt_normal = tgt_normal[tgt_sel_ind, :]
input_src_normal = src_normal[corr[:, 0]]
input_tgt_normal = tgt_normal[corr[:, 1]]
corr_pos = np.concatenate([
input_src_keypts, input_src_normal, input_tgt_keypts,
input_tgt_normal
],
axis=-1)
return corr_pos.astype(np.float32), \
input_src_keypts.astype(np.float32), \
input_tgt_keypts.astype(np.float32), \
gt_trans.astype(np.float32), \
labels.astype(np.float32),
def __getitem__(self, index):
# load meta data
src_id, tgt_id = self.files[index][0], self.files[index][1]
if random.random() > 0.5:
src_id, tgt_id = tgt_id, src_id
# load point coordinates and pre-computed per-point local descriptors
if self.descriptor == 'fcgf':
src_data = np.load(
f"{self.root}/threedmatch_feat/{src_id}".replace(
'.npz', '_fcgf.npz'))
tgt_data = np.load(
f"{self.root}/threedmatch_feat/{tgt_id}".replace(
'.npz', '_fcgf.npz'))
src_keypts = src_data['xyz']
tgt_keypts = tgt_data['xyz']
src_features = src_data['feature']
tgt_features = tgt_data['feature']
elif self.descriptor == 'fpfh':
src_data = np.load(
f"{self.root}/threedmatch_feat/{src_id}".replace(
'.npz', '_fpfh.npz'))
tgt_data = np.load(
f"{self.root}/threedmatch_feat/{tgt_id}".replace(
'.npz', '_fpfh.npz'))
src_keypts = src_data['xyz']
tgt_keypts = tgt_data['xyz']
src_features = src_data['feature']
tgt_features = tgt_data['feature']
np.nan_to_num(src_features)
np.nan_to_num(tgt_features)
src_features = src_features / (
np.linalg.norm(src_features, axis=1, keepdims=True) + 1e-6)
tgt_features = tgt_features / (
np.linalg.norm(tgt_features, axis=1, keepdims=True) + 1e-6)
# compute ground truth transformation
orig_trans = np.eye(4).astype(np.float32)
# data augmentation (add data augmentation to original transformation)
src_keypts += np.random.rand(src_keypts.shape[0], 3) * 0.005
tgt_keypts += np.random.rand(tgt_keypts.shape[0], 3) * 0.005
aug_R = rotation_matrix(self.augment_axis, self.augment_rotation)
aug_T = translation_matrix(self.augment_translation)
aug_trans = integrate_trans(aug_R, aug_T)
tgt_keypts = transform(tgt_keypts, aug_trans)
gt_trans = concatenate(aug_trans, orig_trans)
# select {self.num_node} numbers of keypoints
N_src = src_features.shape[0]
N_tgt = tgt_features.shape[0]
if self.num_node == 'all':
src_sel_ind = np.arange(N_src)
tgt_sel_ind = np.arange(N_tgt)
else:
src_sel_ind = np.random.choice(N_src, self.num_node)
tgt_sel_ind = np.random.choice(N_tgt, self.num_node)
src_desc = src_features[src_sel_ind, :]
tgt_desc = tgt_features[tgt_sel_ind, :]
src_keypts = src_keypts[src_sel_ind, :]
tgt_keypts = tgt_keypts[tgt_sel_ind, :]
# construct the correspondence set by nearest neighbor searching in feature space.
distance = np.sqrt(2 - 2 * (src_desc @ tgt_desc.T) + 1e-6)
source_idx = np.argmin(distance, axis=1)
source_dis = np.min(distance, axis=1)
if self.use_mutual:
target_idx = np.argmin(distance, axis=0)
mutual_nearest = (target_idx[source_idx] == np.arange(
source_idx.shape[0]))
corr = np.concatenate([
np.where(mutual_nearest == 1)[0][:, None],
source_idx[mutual_nearest][:, None]
],
axis=-1)
else:
corr = np.concatenate(
[np.arange(source_idx.shape[0])[:, None], source_idx[:, None]],
axis=-1)
if len(corr) < 10:
# skip pairs with too few correspondences.
return self.__getitem__(int(np.random.choice(self.__len__(), 1)))
# compute the ground truth label
frag1 = src_keypts[corr[:, 0]]
frag2 = tgt_keypts[corr[:, 1]]
frag1_warp = transform(frag1, gt_trans)
distance = np.sqrt(np.sum(np.power(frag1_warp - frag2, 2), axis=1))
labels = (distance < self.inlier_threshold).astype(np.int)
# prepare input to the network
if self.split == 'train' and np.mean(labels) > 0.5:
# add random outlier to input data (deprecated)
num_outliers = int(0 * len(corr))
src_outliers = np.random.randn(num_outliers, 3) * np.mean(
src_keypts, axis=0)
tgt_outliers = np.random.randn(num_outliers, 3) * np.mean(
tgt_keypts, axis=0)
input_src_keypts = np.concatenate(
[src_keypts[corr[:, 0]], src_outliers], axis=0)
input_tgt_keypts = np.concatenate(
[tgt_keypts[corr[:, 1]], tgt_outliers], axis=0)
labels = np.concatenate([labels, np.zeros(num_outliers)], axis=0)
else:
input_src_keypts = src_keypts[corr[:, 0]]
input_tgt_keypts = tgt_keypts[corr[:, 1]]
if self.in_dim == 3:
corr_pos = input_src_keypts - input_tgt_keypts
elif self.in_dim == 6:
corr_pos = np.concatenate([input_src_keypts, input_tgt_keypts],
axis=-1)
# move the center of each point cloud to (0,0,0).
corr_pos = corr_pos - corr_pos.mean(0)
elif self.in_dim == 9:
corr_pos = np.concatenate([
input_src_keypts, input_tgt_keypts,
input_src_keypts - input_tgt_keypts
],
axis=-1)
elif self.in_dim == 70:
corr_pos = np.concatenate([input_src_keypts, input_tgt_keypts],
axis=-1)
# move the center of each point cloud to (0,0,0).
corr_pos = corr_pos - corr_pos.mean(0)
corr_pos = np.concatenate(
[corr_pos, src_desc[corr[:, 0]], tgt_desc[corr[:, 1]]],
axis=-1)
elif self.in_dim == 12:
src_pcd = make_point_cloud(src_keypts)
tgt_pcd = make_point_cloud(tgt_keypts)
estimate_normal(src_pcd, radius=self.downsample * 2)
estimate_normal(tgt_pcd, radius=self.downsample * 2)
src_normal = np.array(src_pcd.normals)
tgt_normal = np.array(tgt_pcd.normals)
src_normal = src_normal[src_sel_ind, :]
tgt_normal = tgt_normal[tgt_sel_ind, :]
input_src_normal = src_normal[corr[:, 0]]
input_tgt_normal = tgt_normal[corr[:, 1]]
corr_pos = np.concatenate([
input_src_keypts, input_src_normal, input_tgt_keypts,
input_tgt_normal
],
axis=-1)
return corr_pos.astype(np.float32), \
input_src_keypts.astype(np.float32), \
input_tgt_keypts.astype(np.float32), \
gt_trans.astype(np.float32), \
labels.astype(np.float32),
def __getitem__(self, index):
# load meta data
key = list(self.gt_trans.keys())[index]
scene = key.split('@')[0]
src_id = key.split('@')[1].split('_')[0]
tgt_id = key.split('@')[1].split('_')[1]
# load point coordinates and pre-computed per-point local descriptors
if self.descriptor == 'fcgf':
src_data = np.load(
f"{self.root}/fragments/{scene}/cloud_bin_{src_id}_fcgf.npz")
tgt_data = np.load(
f"{self.root}/fragments/{scene}/cloud_bin_{tgt_id}_fcgf.npz")
src_keypts = src_data['xyz']
tgt_keypts = tgt_data['xyz']
src_features = src_data['feature']
tgt_features = tgt_data['feature']
elif self.descriptor == 'fpfh':
src_data = np.load(
f"{self.root}/fragments/{scene}/cloud_bin_{src_id}_fpfh.npz")
tgt_data = np.load(
f"{self.root}/fragments/{scene}/cloud_bin_{tgt_id}_fpfh.npz")
src_keypts = src_data['xyz']
tgt_keypts = tgt_data['xyz']
src_features = src_data['feature']
tgt_features = tgt_data['feature']
src_features = src_features / (
np.linalg.norm(src_features, axis=1, keepdims=True) + 1e-6)
tgt_features = tgt_features / (
np.linalg.norm(tgt_features, axis=1, keepdims=True) + 1e-6)
# compute ground truth transformation
orig_trans = np.linalg.inv(
self.gt_trans[key]
) # the given ground truth trans is target-> source
# data augmentation
aug_R = rotation_matrix(self.augment_axis, self.augment_rotation)
aug_T = translation_matrix(self.augment_translation)
aug_trans = integrate_trans(aug_R, aug_T)
tgt_keypts = transform(tgt_keypts, aug_trans)
gt_trans = concatenate(aug_trans, orig_trans)
# select {self.num_node} numbers of keypoints
N_src = src_features.shape[0]
N_tgt = tgt_features.shape[0]
# use all point during test.
if self.num_node == 'all':
src_sel_ind = np.arange(N_src)
tgt_sel_ind = np.arange(N_tgt)
else:
src_sel_ind = np.random.choice(N_src, self.num_node)
tgt_sel_ind = np.random.choice(N_tgt, self.num_node)
src_desc = src_features[src_sel_ind, :]
tgt_desc = tgt_features[tgt_sel_ind, :]
src_keypts = src_keypts[src_sel_ind, :]
tgt_keypts = tgt_keypts[tgt_sel_ind, :]
# construct the correspondence set by mutual nn in feature space.
distance = np.sqrt(2 - 2 * (src_desc @ tgt_desc.T) + 1e-6)
source_idx = np.argmin(distance, axis=1)
if self.use_mutual:
target_idx = np.argmin(distance, axis=0)
mutual_nearest = (target_idx[source_idx] == np.arange(
source_idx.shape[0]))
corr = np.concatenate([
np.where(mutual_nearest == 1)[0][:, None],
source_idx[mutual_nearest][:, None]
],
axis=-1)
else:
corr = np.concatenate(
[np.arange(source_idx.shape[0])[:, None], source_idx[:, None]],
axis=-1)
# build the ground truth label
frag1 = src_keypts[corr[:, 0]]
frag2 = tgt_keypts[corr[:, 1]]
frag1_warp = transform(frag1, gt_trans)
distance = np.sqrt(np.sum(np.power(frag1_warp - frag2, 2), axis=1))
labels = (distance < self.inlier_threshold).astype(np.int)
# prepare input to the network
input_src_keypts = src_keypts[corr[:, 0]]
input_tgt_keypts = tgt_keypts[corr[:, 1]]
if self.in_dim == 3:
corr_pos = input_src_keypts - input_tgt_keypts
elif self.in_dim == 6:
corr_pos = np.concatenate([input_src_keypts, input_tgt_keypts],
axis=-1)
# move the center of each point cloud to (0,0,0).
corr_pos = corr_pos - corr_pos.mean(0)
elif self.in_dim == 9:
corr_pos = np.concatenate([
input_src_keypts, input_tgt_keypts,
input_src_keypts - input_tgt_keypts
],
axis=-1)
elif self.in_dim == 70:
corr_pos = np.concatenate([input_src_keypts, input_tgt_keypts],
axis=-1)
# move the center of each point cloud to (0,0,0).
corr_pos = corr_pos - corr_pos.mean(0)
corr_pos = np.concatenate(
[corr_pos, src_desc[corr[:, 0]], tgt_desc[corr[:, 1]]],
axis=-1)
elif self.in_dim == 12:
src_pcd = make_point_cloud(src_keypts)
tgt_pcd = make_point_cloud(tgt_keypts)
estimate_normal(src_pcd, radius=self.downsample * 2)
estimate_normal(tgt_pcd, radius=self.downsample * 2)
src_normal = np.array(src_pcd.normals)
tgt_normal = np.array(tgt_pcd.normals)
src_normal = src_normal[src_sel_ind, :]
tgt_normal = tgt_normal[tgt_sel_ind, :]
input_src_normal = src_normal[corr[:, 0]]
input_tgt_normal = tgt_normal[corr[:, 1]]
corr_pos = np.concatenate([
input_src_keypts, input_src_normal, input_tgt_keypts,
input_tgt_normal
],
axis=-1)
return corr_pos.astype(np.float32), \
input_src_keypts.astype(np.float32), \
input_tgt_keypts.astype(np.float32), \
gt_trans.astype(np.float32), \
labels.astype(np.float32),