frame_spacing=20,
no_grad=True,
num_pc=2,
)
#pc_ref = torch.load('base_model.pth')
#pc_ref = torch.cat(pcs, 1)
#torch.save(pc_ref.detach(), 'full_model.pth')
fig = plt.figure(3)
ax = fig.add_subplot(111, projection='3d')
pas = 10
utils.plt_pc(pc_ref, ax, pas, 'b')
utils.plt_pc(pcs[idx], ax, pas, 'r')
plt.show()
pc_ref.requires_grad = False
#pc_ref = torch.load('base_model.pth')
pose = poses[1][:3, :]
im_fwd = ims[1].unsqueeze(0)
#net = init_net()
#net = small_init_net()
net = nn.Sequential(
CA.PixEncoder(k_size=4, d_fact=4),
CA.PixDecoder(k_size=4, d_fact=4, out_channel=1, div_fact=2))
q = utils.rot_to_quat(pose[:3, :3])
import matplotlib.pyplot as plt
import pose_utils.utils as pc_utils
import torch
from mpl_toolkits.mplot3d import axes3d
pas = 10
fig = plt.figure(1)
ax = fig.add_subplot(111, projection='3d')
pc1_ = pc_ref.detach()
pc2_ = pc_to_align.detach()
centroid = torch.mean(pc1_[:3, :], -1)
pc_utils.plt_pc(pc2_.cpu(), ax, pas, 'r', size=100, marker='+')
pc_utils.plt_pc(pc1_.detach().cpu(), ax, pas, 'm', size=100, marker='*')
for n in range(0, pc1_.size(-1), pas):
plt.plot([pc1_[0, n], pc2_[0, n]], [pc1_[1, n], pc2_[1, n]],
[pc1_[2, n], pc2_[2, n]],
color='g')
ax.set_xlim([centroid[0].cpu().item() - 5, centroid[0].cpu().item() + 5])
ax.set_ylim([centroid[1].cpu().item() - 5, centroid[1].cpu().item() + 5])
ax.set_zlim([centroid[2].cpu().item() - 5, centroid[2].cpu().item() + 5])
plt.show()
def view_localization(self, final=False, pas=100):
nets_to_test = self.trainer.networks
if not final:
nets_to_test = dict()
for name, network in self.trainer.networks.items():
nets_to_test[name] = copy.deepcopy(network)
try:
nets_to_test[name].load_state_dict(
self.trainer.best_net[1][name])
except KeyError:
logger.warning(
"Unable to load best weights for net {}".format(name))
for network in nets_to_test.values():
network.eval()
dataset = 'test'
mode = 'queries'
self.data['test']['queries'].used_mod = self.testing_mod
self.data['test']['data'].used_mod = self.testing_mod
dtload = data.DataLoader(self.data['test']['data'],
batch_size=1,
shuffle=False,
num_workers=8)
variables = dict()
logger.info('Db feats computating...')
for b in tqdm.tqdm(dtload):
with torch.no_grad():
for action in self.trainer.eval_forwards['data']:
variables['batch'] = self.trainer.batch_to_device(b)
variables = self.trainer._sequential_forward(
action, variables, nets_to_test)
dtload = data.DataLoader(self.data[dataset][mode],
batch_size=1,
shuffle=False)
for b in dtload:
with torch.no_grad():
variables['batch'] = self.trainer.batch_to_device(b)
variables['ref_data'] = self.data['test']['data']
for action in self.trainer.eval_forwards['queries']:
variables = self.trainer._sequential_forward(
action, variables, nets_to_test)
#ref_pc = trainers.minning_function.recc_acces(variables, ['model']).squeeze()
try:
ref_pc = trainers.minning_function.recc_acces(
variables, ['model', 'pc']).squeeze()
except (KeyError, TypeError):
try:
ref_pc = trainers.minning_function.recc_acces(
variables, [
'model',
]).squeeze()
except (KeyError, TypeError):
ref_pc = trainers.minning_function.recc_acces(
variables, ['ref_pcs', 0]).squeeze()
#output_pose = trainers.minning_function.recc_acces(variables, ['Tf', 'T'])[0]
#output_pose = trainers.minning_function.recc_acces(variables, ['icp', 'poses', 'T'])[0]
output_pose = trainers.minning_function.recc_acces(
variables, self.trainer.access_pose + ['T'])[0]
pc = trainers.minning_function.recc_acces(variables,
['pc']).squeeze()
output_pc = output_pose.matmul(pc)
gt_pose = trainers.minning_function.recc_acces(
variables, ['batch', 'pose', 'T'])[0]
gt_pc = gt_pose.matmul(pc)
if 'posenet_pose' in variables.keys():
posenet_pose = trainers.minning_function.recc_acces(
variables, ['posenet_pose', 'T'])[0]
posenet_pc = posenet_pose.matmul(pc)
print('Real pose:')
print(gt_pose)
print('Computed pose:')
print(output_pose)
if 'posenet_pose' in variables.keys():
print('Posenet pose:')
print(posenet_pose)
print('Diff distance = {} m'.format(
torch.norm(gt_pose[:3, 3] - output_pose[:3, 3]).item()))
gtq = trainers.minning_function.recc_acces(
variables, ['batch', 'pose', 'q'])[0]
#q = trainers.minning_function.recc_acces(variables, ['icp', 'poses', 'q'])[0]
q = trainers.minning_function.recc_acces(
variables, self.trainer.access_pose + ['q'])[0]
print('Diff orientation = {} deg'.format(
2 * torch.acos(torch.abs(gtq.dot(q))) * 180 / 3.14159260))
if 'posenet_pose' in variables.keys():
print('Diff distance = {} m (posenet)'.format(
torch.norm(gt_pose[:3, 3] - posenet_pose[:3, 3]).item()))
posenetq = trainers.minning_function.recc_acces(
variables, ['posenet_pose', 'q'])[0]
print('Diff orientation = {} deg (posenet)'.format(
2 * torch.acos(torch.abs(gtq.dot(posenetq))) * 180 /
3.14159260))
if 'noised_T' in variables.keys():
noise_pose = trainers.minning_function.recc_acces(
variables, ['noised_T']).squeeze()
print('Noised pose:')
print(noise_pose)
print('Diff distance = {} m (noise T)'.format(
torch.norm(gt_pose[:3, 3] - noise_pose[:3, 3]).item()))
fig = plt.figure(1)
ax = fig.add_subplot(111, projection='3d')
plot_size = 60
pc_utils.plt_pc(ref_pc.cpu(), ax, pas, 'b', size=plot_size)
pc_utils.plt_pc(gt_pc.cpu(),
ax,
pas,
'c',
size=2 * plot_size,
marker='*')
if 'posenet_pose' in variables.keys():
pc_utils.plt_pc(posenet_pc.cpu(),
ax,
pas,
'm',
size=2 * plot_size,
marker='o')
plt.plot([gt_pose[0, 3], posenet_pose[0, 3]],
[gt_pose[1, 3], posenet_pose[1, 3]],
[gt_pose[2, 3], posenet_pose[2, 3]],
color='m')
plt.plot([gt_pose[0, 3], output_pose[0, 3]],
[gt_pose[1, 3], output_pose[1, 3]],
[gt_pose[2, 3], output_pose[2, 3]],
color='r')
pc_utils.plt_pc(output_pc.cpu(),
ax,
pas,
'r',
size=2 * plot_size,
marker='o')
centroid = torch.mean(ref_pc[:3, :], -1)
ax.set_xlim(
[centroid[0].cpu().item() - 1, centroid[0].cpu().item() + 1])
ax.set_ylim(
[centroid[1].cpu().item() - 1, centroid[1].cpu().item() + 1])
ax.set_zlim(
[centroid[2].cpu().item() - 1, centroid[2].cpu().item() + 1])
plt.show()
pcs[2].unsqueeze(0),
] #pcs[3].unsqueeze(0)]
desc_ref = [
pcs[0].unsqueeze(0),
pcs[2].unsqueeze(0),
] #pcs[3].unsqueeze(0)]
inits_T = [
poses[0].unsqueeze(0),
poses[2].unsqueeze(0),
] #poses[3].unsqueeze(0)]
fig = plt.figure(10)
ax = fig.add_subplot(111, projection='3d')
ax.set_title('Before alignement')
pas = 1
utils.plt_pc(pc_to_align.squeeze(0), ax, pas, 'b', size=50)
utils.plt_pc(torch.cat((pcs[0], pcs[2])), ax, pas, 'r', size=50)
match_net_param = {
'normalize_desc': False,
'knn': 'bidirectional',
#'knn': 'hard_cpu',
#'bidirectional': True,
'n_neighbors': 1
}
#T=PnP(pc_to_align.unsqueeze(0), pcs[1].unsqueeze(0), pc_to_align.unsqueeze(0), pcs[1].unsqueeze(0),
'''
T=rPnP(pc_to_align, pc_ref, desc_to_align, desc_ref, inits_T,
match_function=ICPNet.MatchNet(**match_net_param),
desc_function=None, iterations=1000, K=K, ransac_threshold=1e-7)['T']
print('Real P:')
print(K.matmul(pose.inverse()[:3, :]))
print('Real pose:')
print(pose.inverse()[:3, :])
print('DLT pose:')
print(dlt_pose)
print('Diff:')
print(dlt_pose - pose.inverse()[:3, :])
pc.append(X)
plt.figure(1)
grid = torchvision.utils.make_grid(im)
plt.imshow(grid.numpy().transpose((1, 2, 0)))
grid = torchvision.utils.make_grid(depth)
plt.figure(2)
plt.imshow(grid.numpy().transpose((1, 2, 0))[:, :, 0],
cmap=plt.get_cmap('jet'))
fig = plt.figure(3)
ax = fig.add_subplot(111, projection='3d')
pas = int(250 * scale)
pas = 1
color = ['c', 'b']
for n_pc, point_cloud in enumerate(pc):
utils.plt_pc(point_cloud, ax, pas, color[n_pc])
plt.show()
def ICPwNet(pc_to_align, pc_ref, desc_to_align, desc_ref, init_T, **kwargs):
verbose = kwargs.pop('verbose', False)
outliers_filter = kwargs.pop('outliers_filter', False)
iter = kwargs.pop('iter', 200)
epsilon = kwargs.pop('epsilon', 1e-5)
match_function = kwargs.pop('match_function', None)
pose_function = kwargs.pop('pose_function', None)
desc_function = kwargs.pop('desc_function', None)
fit_pc = kwargs.pop('fit_pc', False)
timing = False
if timing:
t_beg = time.time()
if kwargs:
raise TypeError('Unexpected **kwargs: %r' % kwargs)
if verbose:
fig1 = plt.figure(1)
ax1 = fig1.add_subplot(111, projection='3d')
plt.ion()
plt.show()
pas = 1
T = init_T
#pc_ref = pc_ref[:3, :]
#pc_to_align = pc_to_align[:3, :]
if desc_function is not None:
desc_ref = desc_function(pc_ref, desc_ref)
else:
desc_ref = pc_ref
if fit_pc:
match_function.fit(pc_ref[0])
else:
match_function.fit(desc_ref[0])
teye = torch.eye(4, 4).to(pc_to_align.device)
for i in range(iter):
logger.debug('Iteration {}'.format(i))
if timing:
t = time.time()
pc_rec = T.matmul(pc_to_align)
if desc_function is not None:
desc_ta = desc_function(pc_rec, desc_to_align)
else:
desc_ta = pc_rec
res_match = match_function(pc_rec, pc_ref, desc_ta, desc_ref)
if outliers_filter:
res_match['nn'] = res_match['nn'][:, :, res_match['inliers'].
squeeze().byte()]
pc_rec = pc_rec[:, :, res_match['inliers'].squeeze().byte()]
new_T = pose_function(pc_rec.squeeze(), res_match['nn'].squeeze())
T = torch.matmul(new_T['T'], T)
if timing:
print('Iteration on {}s'.format(time.time() - t))
if verbose:
# Ploting
ax1.clear()
utils.plt_pc(pc_ref[0], ax1, pas, 'b', size=50, marker='*')
utils.plt_pc(pc_rec[0], ax1, pas, 'r', size=50, marker='o')
ax1.set_xlim([-1, 1])
ax1.set_ylim([-1, 1])
ax1.set_zlim([-1, 1])
plt.pause(0.1)
variation = torch.norm(teye - new_T['T'].squeeze())
if variation < epsilon:
logger.debug('Convergence in {} iterations'.format(i))
break
if verbose:
plt.ioff()
ax1.clear()
plt.close()
match_function.unfit()
if timing:
print('ICP converge on {}s'.format(time.time() - t_beg))
logger.debug('Final RANSAC score is {} ({}% inliers)'.format(
new_T['score'], new_T['inliers_ratio']))
return {'T': T, 'inliers': new_T['inliers_ratio'], 'score': new_T['score']}
def soft_icp(pc_ref, pc_to_align, init_T, **kwargs):
iter = kwargs.pop('iter', 100)
tolerance = kwargs.pop('tolerance', 1e-3)
unit_fact = kwargs.pop('fact', 1)
outlier_rejection = kwargs.pop('outlier', False)
hard_rejection = kwargs.pop('hard_rejection', False)
verbose = kwargs.pop('verbose', False)
use_hard_nn = kwargs.pop('use_hard_nn', False)
fixed_fact = kwargs.pop('fixed_fact', False)
custom_filter = kwargs.pop('custom_filter', None)
reject_ratio = kwargs.pop('reject_ratio', 1)
T_gt = kwargs.pop('T_gt', None)
if kwargs:
raise TypeError('Unexpected **kwargs: %r' % kwargs)
if verbose:
fig1 = plt.figure(1)
ax1 = fig1.add_subplot(111, projection='3d')
fig2 = plt.figure(2)
ax2 = fig2.add_subplot(111, projection='3d')
plt.ion()
plt.show()
pas = 1
# Trying to speed up
pc_ref = pc_ref.cpu()
pc_to_align = pc_to_align.cpu()
init_T = init_T.cpu()
T = init_T
# Row data
row_pc_ref = pc_ref.view(4, -1)
row_pc_to_align = pc_to_align.view(4, -1)
indexor = pc_to_align.new_ones(row_pc_to_align.size(-1))
# First iter
fact = 1 * unit_fact
prev_dist = 0
for i in range(iter):
logger.debug('Iteration {}'.format(i))
#t = time.time()
pc_rec = T.matmul(row_pc_to_align)
if use_hard_nn:
pc_rec, pc_nearest, dist = hard_knn(row_pc_ref, pc_rec, fact=fact)
else:
#pc_nearest, dist = soft_knn(row_pc_ref, pc_rec, softmax_tool, fact=fact, d_norm=distance_norm)
pc_nearest, dist = fast_soft_knn(row_pc_ref, pc_rec, fact=fact)
#print('Elapsed for matching {}'.format(time.time() - t))
if outlier_rejection:
indexor = soft_outlier_filter(pc_nearest, pc_rec, reject_ratio)
if hard_rejection:
pc_nearest, pc_rec = hard_outlier_filter(pc_nearest, pc_rec,
reject_ratio)
indexor = pc_to_align.new_ones(pc_nearest.size(-1))
if custom_filter is not None:
indexor = indexor * custom_filter
new_T = best_fit_transform(pc_nearest, pc_rec, indexor)
T = torch.matmul(new_T, T)
entrop = abs(prev_dist - dist.item())
if entrop != 0:
fact = unit_fact if fixed_fact else min(1000, max(
1, 1 / entrop)) * unit_fact
if entrop < tolerance:
logger.debug('Done in {} it'.format(i))
break
else:
prev_dist = dist.item()
#print('Elapsed all {}'.format(time.time() - t))
if T_gt is not None:
logger.debug(
'Training mode: stopping if no improvment in localization.')
Id = init_T.new_zeros(4, 4)
Id[0, 0] = Id[1, 1] = Id[2, 2] = Id[3, 3] = 1
if torch.norm(Id - T.matmul(T_gt.inverse())) > torch.norm(
Id - init_T.matmul(T_gt.inverse())):
logger.debug(
'No improvment, stopping at iteration {}'.format(i))
break
else:
logger.debug('Loc error: {}'.format(
torch.norm(Id - T.matmul(T_gt.inverse())).item()))
init_T = T
if verbose:
# Ploting
ax1.clear()
utils.plt_pc(row_pc_ref, ax1, pas, 'b')
utils.plt_pc(pc_rec, ax1, pas, 'r')
ax1.set_xlim([-1, 1])
ax1.set_ylim([-1, 1])
ax1.set_zlim([-1, 1])
ax2.clear()
utils.plt_pc(pc_nearest, ax2, pas, 'c')
utils.plt_pc(pc_rec, ax2, pas, 'r')
ax2.set_xlim([-1, 1])
ax2.set_ylim([-1, 1])
ax2.set_zlim([-1, 1])
plt.pause(0.1)
if verbose:
plt.ioff()
ax1.clear()
plt.close()
ax2.clear()
plt.close()
'''
pc_rec = T.matmul(row_pc_to_align)
pc_nearest, dist = fast_soft_knn(row_pc_ref, pc_rec, fact=1e5) # hard assigment
if hard_rejection:
pc_nearest, pc_rec = hard_outlier_filter(pc_nearest, pc_rec, reject_ratio)
indexor = pc_to_align.new_ones(pc_nearest.size(-1))
elif outlier_rejection:
indexor = soft_outlier_filter(pc_nearest, pc_rec, reject_ratio)
real_error = torch.mean(torch.sum(((pc_rec - pc_nearest)*indexor)**2, 0))
'''
real_error = dist
return T, real_error
#rd_trans[:,3] = torch.FloatTensor([0.5, -1, 1])
rd_trans[:3, :3] = utils.rotation_matrix(torch.Tensor([1, 0, 0]),
torch.Tensor([1]))
rd_trans[:3, :] = poses[1][:3, :]
pc_ref = torch.cat((pcs[0], pcs[2]), 1)
pc_to_align = rd_trans.matmul(pcs[1])
print('Loading finished')
fig = plt.figure(10)
ax = fig.add_subplot(111, projection='3d')
ax.set_title('Before alignement')
pas = 1
utils.plt_pc(pc_ref, ax, pas, 'b', size=50)
utils.plt_pc(pc_to_align, ax, pas, 'r', size=50)
#T, d = ICPwNet(pc_ref, pc_to_align, torch.eye(4, 4), iter=20, verbose=True,
# arg_net={'fact': 2, 'reject_ratio': 1, 'pose_solver': 'svd', })
match_net_param = {
'normalize_desc': False,
'knn': 'fast_soft_knn',
#'knn': 'hard_cpu',
#'bidirectional': True,
'n_neighbors': 15
}
T = ICPwNet(
pc_ref.unsqueeze(0),
pc_to_align.unsqueeze(0),
pc_ref.unsqueeze(0),
hyp_loss.backward()
optimizer.step()
if not i % 10:
print('Losses')
print(losses)
print('Scores')
print(scores)
print('SoftScores')
print(soft_score)
fig = plt.figure(3)
plt.clf()
ax = fig.add_subplot(111, projection='3d')
pas = 1
utils.plt_pc(pc_ref, ax, pas, 'b')
utils.plt_pc(poses[1].detach().matmul(pc_to_align.detach()), ax,
pas, 'c')
plt.figure(1)
grid = torchvision.utils.make_grid(torch.cat(
(depths[1].unsqueeze(0).detach(), 1 /
(1 + depths[1]).unsqueeze(0).detach(), depth_map.detach(),
inv_depth_map.detach()), ),
nrow=2)
plt.imshow(grid.numpy().transpose((1, 2, 0))[:, :, 0],
cmap=plt.get_cmap('jet'))
'''
plt.figure(2)
grid = torchvision.utils.make_grid(ims_nn[1])
plt.imshow(grid.numpy().transpose((1, 2, 0)))