def vis_skeleton_3d_all(self, pg):
fig = plt.figure()
ax = fig.gca(projection='3d')
for obj_index, obj in enumerate(pg.objects):
p = center_to_corners(pg.objects[obj_index].terminal.obj_center,
pg.objects[obj_index].terminal.obj_size,
pg.objects[obj_index].terminal.angle * 2)
visualize.plot_cuboid(
ax, p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],
visualize.sunrgbd_object_color(pg.objects[obj_index].obj_type))
for obj in self.pg.objects:
if obj.action_group is not None:
r = random.random()
num_actitivity = len(obj.action_group)
if 0 < r < 0.5:
vis_index = 0
else:
vis_index = np.random.permutation(num_actitivity)[0]
cur_skeleton = obj.action_group[vis_index]['skeleton']
visualize.plot_skeleton(ax, cur_skeleton, 'r-')
ax.set_xlim([-1, 2])
ax.set_ylim([-1, 2])
ax.set_zlim([-1, 2])
ax.grid(False)
ax.axis('off')
plt.show()
def vis_skeleton(self, pg, cur_skeleton, involved_index):
fig = plt.figure()
ax = fig.gca(projection='3d')
# ax.scatter(p_camera[0], p_camera[1], p_camera[2], c='r', marker='o')
for obj_index, obj in enumerate(pg.objects):
p = center_to_corners(pg.objects[obj_index].terminal.obj_center,
pg.objects[obj_index].terminal.obj_size,
pg.objects[obj_index].terminal.angle)
if obj_index not in involved_index:
visualize.plot_cuboid(ax, p[0], p[1], p[2], p[3], p[4], p[5],
p[6], p[7], 'g-')
else:
visualize.plot_cuboid(ax, p[0], p[1], p[2], p[3], p[4], p[5],
p[6], p[7], 'b-')
visualize.plot_skeleton(ax, cur_skeleton, 'r-')
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([-2, 2])
plt.show()
plt.close()
# load pre-trained svm model
clf = load(model_path.format(species))
# perform stem vs leaf classification
predict = clf.predict(features)
data = np.column_stack((xyz, predict))
stem = data[np.where(data[:, -1] == 0)]
leaf = data[np.where(data[:, -1] == 1)]
# perform clustering
dbscan = DBSCAN(min_samples=2, p=0).fit(leaf[:, :-1])
leaves, labels = utils.refineClustering(leaf[:, :-1], dbscan.labels_)
organs = utils.prepare4Skel(stem, leaves)
# visualize different organs
fh = plt.figure()
vis.plot_semantic_pointcloud(fh, organs)
plt.title('Sematic classification of plant organs')
# perform skeletonization
cwise_skeleton_nodes = som.getSkeleton(organs)
graph = som.getGraph(cwise_skeleton_nodes, xyz)
# convert this graph to skeleton class
S = utils.convert_to_skeleton_class(cwise_skeleton_nodes, graph)
# plot skeleton
fh = plt.figure()
vis.plot_skeleton(fh, S)
plt.title('Skeleton structure of the plant')
import skeleton_matching as skm
import non_rigid_registration as nrr
from iterative_registration import iterative_registration
import visualize as vis
# %% load skeleton data
species = 'maize'
day1 = '03-13'
day2 = '03-14'
skel_path = '../data/{}/{}.graph.txt'
S1 = skel.Skeleton.read_graph(skel_path.format(species, day1))
S2 = skel.Skeleton.read_graph(skel_path.format(species, day2))
# visualize input data
fh1 = plt.figure()
vis.plot_skeleton(fh1, S1, 'b')
vis.plot_skeleton(fh1, S2, 'r')
plt.show()
# %% compute non-rigid registration params
# set matching params
match_params = {
'weight_e': 0.01,
'match_ends_to_ends': True,
'use_labels': False,
'label_penalty': 1,
'debug': False
}
# set registration params
import skeleton_matching as skm
import pointcloud as pcd
import visualize as vis
# %% Load data
species = 'maize'
day1 = '03-13'
day2 = '03-14'
skel_path = '../data/{}/{}.graph.txt'
pc_path = '../data/{}/{}.xyz'
corres_path = '../data/{}/{}-{}.corres.txt'
reg_path = '../data/{}/{}-{}.reg.npy'
S1 = skel.Skeleton.read_graph(skel_path.format(species, day1))
S2 = skel.Skeleton.read_graph(skel_path.format(species, day2))
P1 = pcd.load_pointcloud(pc_path.format(species, day1))
P2 = pcd.load_pointcloud(pc_path.format(species, day2))
corres = np.loadtxt(corres_path.format(species, day1, day2), dtype=np.int32)
T12 = np.load(reg_path.format(species, day1, day2))
# deform complete pointcloud
P1_ds = pcd.downsample_pointcloud(P1, 3)
P2_ds = pcd.downsample_pointcloud(P2, 3)
P1_deformed = pcd.deform_pointcloud(P1_ds, T12, corres, S1, S2)
# visualize results
fh = plt.figure()
vis.plot_skeleton(fh, S1, 'b')
vis.plot_skeleton(fh, S2, 'r')
vis.plot_pointcloud(fh, P2, 'r')
vis.plot_pointcloud(fh, P1_deformed, 'b')
vis.plot_skeleton_correspondences(fh, S1, S2, corres, 'g')
def iterative_registration(S1, S2, params):
"""
Iterative procedure for non-rigid registration of skeleton graphs.
Parameters
----------
S1, S2 : Skeleton Class
Two skeletons for which we compute the non-rigid registration params
params : Dictionary
num_iter : Maximum number of iterations used in the procedure
default: 10
match_params : parameters used for correspondence estimation step
See skeleton_matching module for details.
reg_params : parameters used for correspondence estimation step
See non_rigid_registration module for details.
Returns
-------
T12 : list of 4x4 numpy arrays
Affine transformation corresponding to each node in S1
corres : numpy array (Mx2)
correspondence between two skeleton nodes
"""
# default params
if 'num_iter' not in params:
params['num_iter'] = 10
# params for matching and registration module
match_params = params['match_params']
reg_params = params['reg_params']
# Initialize solution
m = S1.XYZ.shape[0]
R_init = []
t_init = []
for i in range(m):
R_init.append(np.eye(3))
t_init.append(np.zeros((3,1)))
reg_params['R_init'] = R_init
reg_params['t_init'] = t_init
# initialize
S1_transformed = skel.Skeleton.copy_skeleton(S1)
old_corres = -np.ones([m,2])
# perform matching and deformation in a loop
for i in range(params['num_iter']):
print('-------------------- Global Iteration {} --------------------------'.format(i))
# find correspondences
corres = skm.skeleton_matching(S1_transformed, S2, match_params)
if params['visualize']:
fh_vis = plt.figure()
vis.plot_skeleton(fh_vis, S1,'b');
vis.plot_skeleton(fh_vis, S2,'r');
vis.plot_skeleton_correspondences(fh_vis, S1, S2, corres)
plt.title("Iteration {}: # of Matches = {} ".format(i, corres.shape[0]))
plt.show()
# find registration params
T12 = nrr.register_skeleton(S1, S2, corres, reg_params)
# apply registration params to skeleton S1
S1_transformed = nrr.apply_registration_params_to_skeleton(S1, T12)
# compute registration error for skeleton nodes
err = nrr.compute_skeleton_registration_error(S1, S2, corres, T12)
print('Registration error for skeleton nodes = ', err)
# use results as approximates for next iteration
for j in range(m):
reg_params['R_init'][j] = T12[j][0:3,0:3]
reg_params['t_init'][j] = np.reshape(T12[j][0:3,3], (3,1))
# Is there any change in correspondences ? If no, stop
if corres.shape[0] == old_corres.shape[0] and np.array_equal(np.sort(corres, axis=0), np.sort(old_corres, axis=0)):
break;
old_corres = corres.copy()
# visualize registration results
if params['visualize']:
fh_vis = plt.figure()
vis.plot_skeleton(fh_vis, S1,'b');
vis.plot_skeleton(fh_vis, S2,'r');
vis.plot_skeleton(fh_vis, S1_transformed,'k');
vis.plot_skeleton_correspondences(fh_vis, S1_transformed, S2, corres)
plt.title("Iteration {}: Registration ".format(i))
plt.show()
return T12, corres
def register_skeleton(S1, S2, corres, params):
"""
This function computes the (non-rigid) registration params between the
two skeletons. This function computes the normal equation, solves the
non-linear least squares problem.
Parameters
----------
S1, S2 : Skeleton Class
Two skeletons for which we compute the non-rigid registration params
corres : numpy array (Mx2)
correspondence between two skeleton nodes
params : Dictionary
num_iter : Maximum number of iterations for the optimization routine
default: 10
w_rot : Weight for the rotation matrix constraints
default: 100,
w_reg : Weight for regularization constraints
default: 100
w_corresp: Weight for correspondence constraints
default: 1
w_fix : Weight for fixed nodes
default: 1
fix_idx : list of fixed nodes
default : []
R_fix : list of rotation matrices for fixed nodes
default : [np.eye(3)]
t_fix : list of translation vectors for fixed nodes
default: [np.zeros((3,1))]
use_robust_kernel : Use robust kernels for optimization (recommended if corres has outliers)
default : True
robust_kernel_type : Choose a robust kernel type (huber, cauchy, geman-mcclure)
default: 'cauchy'
robust_kernel_param : scale/outlier parameter for robust kernel
default: 2
debug : show debug visualizations + info
default: False
Returns
-------
T12 : list of 4x4 numpy arrays
Affine transformation corresponding to each node in S1
"""
print('Computing registration params.')
# set default params if not provided
if 'num_iter' not in params:
params['num_iter'] = 10
if 'w_rot' not in params:
params['w_rot'] = 100
if 'w_reg' not in params:
params['w_reg'] = 100
if 'w_corresp' not in params:
params['w_corresp'] = 1
if 'w_fix' not in params:
params['w_fix'] = 1
if 'fix)idx' not in params:
params['fix_idx'] = []
if 'use_robust_kernel' not in params:
params['use_robust_kernel'] = False
if 'robust_kernel_type' not in params:
params['robust_kernel_type'] = 'cauchy'
if 'robust_kernel_param' not in params:
params['robust_kernel_param'] = 2
if 'debug' not in params:
params['debug'] = False
# initialize normal equation
J_rot, r_rot, J_reg, r_reg, J_corresp, r_corresp, J_fix, r_fix = \
initialize_normal_equations(S1, corres, params)
# initialze solution
x = initialize_solution(S1, params)
# initialize weights
W_rot, W_reg, W_corresp, W_fix = initialize_weight_matrices(\
params['w_rot'], len(r_rot), params['w_reg'], len(r_reg), \
params['w_corresp'], len(r_corresp) , params['w_fix'], len(r_fix))
# # initialize variables in optimization
m = S1.XYZ.shape[0]
T12 = [None] * m
R = [None] * m
t = [None] * m
for j in range(m):
xj = x[12 * j:12 * (j + 1)]
R[j] = np.reshape(xj[0:9], (3, 3))
t[j] = xj[9:12]
# perform optimization
if params['debug']:
fh_debug = plt.figure()
E_prev = np.inf
dx_prev = np.inf
for i in range(params['num_iter']):
# counters used for different constraints
jk = 0
jc = 0
jf = 0
# compute jacobian and residual for each constraint types
for j in range(m):
# registration params for jth node
Rj = R[j]
tj = t[j]
# constraints from rotation matrix entries
Jj_rot, rj_rot = compute_rotation_matrix_constraints(Rj)
J_rot[6 * j:6 * (j + 1), 12 * j:12 * (j + 1)] = Jj_rot
r_rot[6 * j:6 * (j + 1)] = rj_rot
# constraints from regularization term
ind = np.argwhere(S1.A[j, :] == 1).flatten()
for k in range(np.sum(S1.A[j, :])):
# params
Rk = R[ind[k]]
tk = t[ind[k]]
Jj_reg, Jk_reg, r_jk_reg = compute_regularization_constraints(
Rj, tj, Rk, tk)
# collect all constraints
nc = r_jk_reg.shape[0]
J_reg[nc * jk:nc * (jk + 1), 12 * j:12 * (j + 1)] = Jj_reg
J_reg[nc * jk:nc * (jk + 1),
ind[k] * 12:12 * (ind[k] + 1)] = Jk_reg
r_reg[nc * jk:nc * (jk + 1)] = r_jk_reg
# increment counter for contraints from neighbouring nodes
jk = jk + 1
# constraints from correspondences
if corres.shape[0] > 0:
ind_C = np.argwhere(corres[:, 0] == j).flatten()
if len(ind_C) > 0:
# observations
Y = Obs(S1.XYZ[j, :].reshape(3, 1),
S2.XYZ[corres[ind_C, 1], :].reshape(3, 1))
# compute constraints
J_jc_corresp, r_jc_corresp = compute_corresp_constraints(
Rj, tj, Y)
# collect all constraints
nc = r_jc_corresp.shape[0]
J_corresp[nc * jc:nc * (jc + 1),
12 * j:12 * (j + 1)] = J_jc_corresp
r_corresp[nc * jc:nc * (jc + 1)] = r_jc_corresp
# increment counter for correspondence constraints
jc = jc + 1
# constraints from fixed nodes
if len(params['fix_idx']) > 0:
if j in params['fix_idx']:
ind_f = params['fix_idx'].index(j)
# observations
R_fix = params['R_fix'][ind_f]
t_fix = params['t_fix'][ind_f]
# compute fix node constraints
J_jf_fix, r_jf_fix = compute_fix_node_constraints(
Rj, tj, R_fix, t_fix)
nc = r_jf_fix.shape[0]
J_fix[nc * jf:nc * (jf + 1),
12 * j:12 * (j + 1)] = J_jf_fix
r_fix[nc * jf:nc * (jf + 1)] = r_jf_fix
# update counter
jf = jf + 1
# compute weights and residual using robust kernel
if params['use_robust_kernel']:
if params['robust_kernel_type'] == 'huber':
_, _, W_corresp = rf.loss_huber(r_corresp,
params['robust_kernel_param'])
elif params['robust_kernel_type'] == 'cauchy':
_, _, W_corresp = rf.loss_cauchy(r_corresp,
params['robust_kernel_param'])
elif params['robust_kernel_type'] == 'geman_mcclure':
_, _, W_corresp = rf.loss_geman_mcclure(
r_corresp, params['robust_kernel_param'])
else:
print('Robust kernel not undefined. \n')
W_corresp = params['w_corresp'] * np.diag(W_corresp.flatten())
# collect all constraints
J = np.vstack((J_rot, J_reg, J_corresp, J_fix))
r = np.vstack((r_rot, r_reg, r_corresp, r_fix))
# construct weight matrix
W = combine_weight_matrices(W_rot, W_reg, W_corresp, W_fix)
# solve linear system
A = J.T @ W @ J
b = J.T @ W @ r
dx = -np.linalg.solve(A, b)
# Errors
E_rot = r_rot.T @ W_rot @ r_rot
E_reg = r_reg.T @ W_reg @ r_reg
E_corresp = r_corresp.T @ W_corresp @ r_corresp
E_fix = r_fix.T @ W_fix @ r_fix
E_total = E_rot + E_reg + E_corresp + E_fix
# print errors
if params['debug']:
print("Iteration # ", i)
print("E_total = ", E_total)
print("E_rot = ", E_rot)
print("E_reg = ", E_reg)
print("E_corresp = ", E_corresp)
print("E_fix = ", E_fix)
print("Rank(A) = ", np.linalg.matrix_rank(A))
# update current estimate
for j in range(m):
#params
dx_j = dx[12 * j:12 * (j + 1)]
R[j] = R[j] + np.reshape(dx_j[0:9], (3, 3), order='F')
t[j] = t[j] + dx_j[9:12]
# collect and return transformation
for j in range(m):
T12[j] = hf.M(R[j], t[j])
# apply registration to skeleton for visualization
if params['debug']:
# compute registration error
S2_hat = apply_registration_params_to_skeleton(S1, T12)
vis.plot_skeleton(fh_debug, S1, 'b')
vis.plot_skeleton(fh_debug, S2, 'r')
vis.plot_skeleton(fh_debug, S2_hat, 'k')
vis.plot_skeleton_correspondences(fh_debug, S2_hat, S2, corres)
plt.title("Iteration " + str(i))
# exit criteria
if np.abs(E_total - E_prev) < 1e-6 or np.abs(
np.linalg.norm(dx) - np.linalg.norm(dx_prev)) < 1e-6:
print("Exiting optimization.")
print('Total error = ', E_total)
break
# update last solution
E_prev = E_total
dx_prev = dx
return T12
import skeleton as skel
import skeleton_matching as skm
import numpy as np
import matplotlib.pyplot as plt
import visualize as vis
# Load data
species = 'maize'
day1 = '03-13'
day2 = '03-14'
skel_path = '../data/{}/{}.graph.txt'
S1_maize = skel.Skeleton.read_graph(skel_path.format(species, day1))
S2_maize = skel.Skeleton.read_graph(skel_path.format(species, day2))
# Perform matching
params = {
'weight_e': 0.01,
'match_ends_to_ends': True,
'use_labels': True,
'label_penalty': 1,
'debug': False
}
corres = skm.skeleton_matching(S1_maize, S2_maize, params)
print("Estimated correspondences: \n", corres)
# visualize results
fh = plt.figure()
vis.plot_skeleton(fh, S1_maize, 'b')
vis.plot_skeleton(fh, S2_maize, 'r')
vis.plot_skeleton_correspondences(fh, S1_maize, S2_maize, corres)
plt.title("Estimated correspondences between skeletons")