def register_rigid(x, y, w, max_it=150): """ Registers Y to X using the Coherent Point Drift algorithm, in rigid fashion. Note: For affine transformation, t = scale*y*r'+1*t'(* is dot). r is orthogonal rotation matrix here. Parameters ---------- x : ndarray The static shape that y will be registered to. Expected array shape is [n_points_x, n_dims] y : ndarray The moving shape. Expected array shape is [n_points_y, n_dims]. Note that n_dims should be equal for x and y, but n_points does not need to match. w : float Weight for the outlier suppression. Value is expected to be in range [0.0, 1.0]. max_it : int Maximum number of iterations. The default value is 150. Returns ------- t : ndarray The transformed version of y. Output shape is [n_points_y, n_dims]. """ # get dataset lengths and dimensions [n, d] = x.shape [m, d] = y.shape # t is the updated moving shape,we initialize it with y first. t = y # initialize sigma^2 sigma2 = (m * np.trace(np.dot(np.transpose(x), x)) + n * np.trace(np.dot(np.transpose(y), y)) - 2 * np.dot(sum(x), np.transpose(sum(y)))) / (m * n * d) iter = 0 while (iter < max_it) and (sigma2 > 10.e-8): [p1, pt1, px] = cpd_p(x, t, sigma2, w, m, n, d) # precompute Np = np.sum(pt1) mu_x = np.dot(np.transpose(x), pt1) / Np mu_y = np.dot(np.transpose(y), p1) / Np # solve for Rotation, scaling, translation and sigma^2 a = np.dot(np.transpose(px), y) - Np * (np.dot(mu_x, np.transpose(mu_y))) [u, s, v] = np.linalg.svd(a) s = np.diag(s) c = np.eye(d) c[-1, -1] = np.linalg.det(np.dot(u, v)) r = np.dot(u, np.dot(c, v)) scale = np.trace(np.dot( s, c)) / (sum(sum(y * y * np.matlib.repmat(p1, 1, d))) - Np * np.dot(np.transpose(mu_y), mu_y)) sigma22 = np.abs( sum(sum(x * x * np.matlib.repmat(pt1, 1, d))) - Np * np.dot(np.transpose(mu_x), mu_x) - scale * np.trace(np.dot(s, c))) / (Np * d) sigma2 = sigma22[0][0] # ts is translation ts = mu_x - np.dot(scale * r, mu_y) t = np.dot(scale * y, np.transpose(r)) + np.matlib.repmat( np.transpose(ts), m, 1) iter = iter + 1 return t
def register_nonrigid(x, y, w, lamb=3.0, beta=2.0, max_it=150): """ Registers Y to X using the Coherent Point Drift algorithm, in non-rigid fashion. Note: For affine transformation, t = y+g*wc(* is dot). Parameters ---------- x : ndarray The static shape that Y will be registered to. Expected array shape is [n_points_x, n_dims] y : ndarray The moving shape. Expected array shape is [n_points_y, n_dims]. Note that n_dims should be equal for X and Y, but n_points does not need to match. w : float Weight for the outlier suppression. Value is expected to be in range [0.0, 1.0]. lamb : float, optional lamb represents the trade-off between the goodness of maximum likelihood fit and regularization. Default value is 3.0. beta : float, optional beta defines the model of the smoothness regularizer(width of smoothing Gaussian filter in equation(20) of the paper).Default value is 2.0. max_it : int, optional Maximum number of iterations. Used to prevent endless looping when the algorithm does not converge. Default value is 150. tol : float Returns ------- t : ndarray The transformed version of y. Output shape is [n_points_y, n_dims]. """ # Construct G: g = y[:, np.newaxis, :] - y g = g * g g = np.sum(g, 2) g = np.exp(-1.0 / (2 * beta * beta) * g) [n, d] = x.shape [m, d] = y.shape t = y # initialize sigma^2 sigma2 = (m * np.trace(np.dot(np.transpose(x), x)) + n * np.trace(np.dot(np.transpose(y), y)) - 2 * np.dot(sum(x), np.transpose(sum(y)))) / (m * n * d) iter = 0 while (iter < max_it) and (sigma2 > 1.0e-5): [p1, pt1, px] = cpd_p(x, t, sigma2, w, m, n, d) # precompute diag(p) dp = scipy.sparse.spdiags(p1.T, 0, m, m) # wc is a matrix of coefficients wc = np.dot(np.linalg.inv(dp * g + lamb * sigma2 * np.eye(m)), (px - dp * y)) t = y + np.dot(g, wc) Np = np.sum(p1) sigma2 = np.abs((np.sum(x * x * np.matlib.repmat(pt1, 1, d)) + np.sum(t * t * np.matlib.repmat(p1, 1, d)) - 2 * np.trace(np.dot(px.T, t))) / (Np * d)) iter = iter + 1 return t
def register_rigid(x, y, w, max_it=150): """ Registers Y to X using the Coherent Point Drift algorithm, in rigid fashion. Note: For affine transformation, t = scale*y*r'+1*t'(* is dot). r is orthogonal rotation matrix here. Parameters ---------- x : ndarray The static shape that y will be registered to. Expected array shape is [n_points_x, n_dims] y : ndarray The moving shape. Expected array shape is [n_points_y, n_dims]. Note that n_dims should be equal for x and y, but n_points does not need to match. w : float Weight for the outlier suppression. Value is expected to be in range [0.0, 1.0]. max_it : int Maximum number of iterations. The default value is 150. Returns ------- t : ndarray The transformed version of y. Output shape is [n_points_y, n_dims]. """ # get dataset lengths and dimensions [n, d] = x.shape [m, d] = y.shape # t is the updated moving shape,we initialize it with y first. t = y # initialize sigma^2 sigma2 = (m*np.trace(np.dot(np.transpose(x), x))+n*np.trace(np.dot(np.transpose(y), y)) - 2*np.dot(sum(x), np.transpose(sum(y))))/(m*n*d) iter = 0 while (iter < max_it) and (sigma2 > 10.e-8): [p1, pt1, px] = cpd_p(x, t, sigma2, w, m, n, d) # precompute Np = np.sum(pt1) mu_x = np.dot(np.transpose(x), pt1)/Np mu_y = np.dot(np.transpose(y), p1)/Np # solve for Rotation, scaling, translation and sigma^2 a = np.dot(np.transpose(px), y)-Np*(np.dot(mu_x, np.transpose(mu_y))) [u, s, v] = np.linalg.svd(a) s = np.diag(s) c = np.eye(d) c[-1, -1] = np.linalg.det(np.dot(u, v)) r = np.dot(u, np.dot(c, v)) scale = np.trace(np.dot(s, c))/(sum(sum(y*y*np.matlib.repmat(p1, 1, d)))-Np * np.dot(np.transpose(mu_y), mu_y)) sigma22 = np.abs(sum(sum(x*x*np.matlib.repmat(pt1, 1, d)))-Np * np.dot(np.transpose(mu_x), mu_x)-scale*np.trace(np.dot(s, c)))/(Np*d) sigma2 = sigma22[0][0] # ts is translation ts = mu_x-np.dot(scale*r, mu_y) t = np.dot(scale*y, np.transpose(r))+np.matlib.repmat(np.transpose(ts), m, 1) iter = iter+1 return t
def register_nonrigid(x, y, w, lamb=3.0, beta=2.0, max_it=150): """ Registers Y to X using the Coherent Point Drift algorithm, in non-rigid fashion. Note: For affine transformation, t = y+g*wc(* is dot). Parameters ---------- x : ndarray The static shape that Y will be registered to. Expected array shape is [n_points_x, n_dims] y : ndarray The moving shape. Expected array shape is [n_points_y, n_dims]. Note that n_dims should be equal for X and Y, but n_points does not need to match. w : float Weight for the outlier suppression. Value is expected to be in range [0.0, 1.0]. lamb : float, optional lamb represents the trade-off between the goodness of maximum likelihood fit and regularization. Default value is 3.0. beta : float, optional beta defines the model of the smoothness regularizer(width of smoothing Gaussian filter in equation(20) of the paper).Default value is 2.0. max_it : int, optional Maximum number of iterations. Used to prevent endless looping when the algorithm does not converge. Default value is 150. tol : float Returns ------- t : ndarray The transformed version of y. Output shape is [n_points_y, n_dims]. """ # Construct G: g = y[:, np.newaxis, :]-y g = g*g g = np.sum(g, 2) g = np.exp(-1.0/(2*beta*beta)*g) [n, d] = x.shape [m, d] = y.shape t = y # initialize sigma^2 sigma2 = (m*np.trace(np.dot(np.transpose(x), x))+n*np.trace(np.dot(np.transpose(y), y)) - 2*np.dot(sum(x), np.transpose(sum(y))))/(m*n*d) iter = 0 while (iter < max_it) and (sigma2 > 1.0e-5): [p1, pt1, px] = cpd_p(x, t, sigma2, w, m, n, d) # precompute diag(p) dp = scipy.sparse.spdiags(p1.T, 0, m, m) # wc is a matrix of coefficients wc = np.dot(np.linalg.inv(dp*g+lamb*sigma2*np.eye(m)), (px-dp*y)) t = y+np.dot(g, wc) Np = np.sum(p1) sigma2 = np.abs((np.sum(x*x*np.matlib.repmat(pt1, 1, d))+np.sum(t*t*np.matlib.repmat(p1, 1, d)) - 2*np.trace(np.dot(px.T, t)))/(Np*d)) iter = iter+1 return t
def register_affine(x, y, w, max_it=150): """ Registers Y to X using the Coherent Point Drift algorithm, in affine fashion. Note: For affine transformation, t = y*b'+1*t'(* is dot). b is any random matrix here. Parameters ---------- x : ndarray The static shape that y will be registered to. Expected array shape is [n_points_x, n_dims] y : ndarray The moving shape. Expected array shape is [n_points_y, n_dims]. Note that n_dims should be equal for x and y, but n_points does not need to match. w : float Weight for the outlier suppression. Value is expected to be in range [0.0, 1.0]. max_it : int Maximum number of iterations. The default value is 150. Returns ------- t : ndarray The transformed version of y. Output shape is [n_points_y, n_dims]. """ [n, d] = x.shape [m, d] = y.shape # initialize t using y. t = y # initialize sigma^2 sigma2 = (m*np.trace(np.dot(np.transpose(x), x)) + n*np.trace(np.dot(np.transpose(y), y)) - 2*np.dot(sum(x), np.transpose(sum(y))))/(m*n*d) iter = 0 # the epsilon eps = np.spacing(1) while (iter < max_it) and (sigma2 > 10.0*eps): [p1, pt1, px] = cpd_p(x, t, sigma2, w, m, n, d) # precompute Np = np.sum(p1) mu_x = np.dot(np.transpose(x), pt1)/Np mu_y = np.dot(np.transpose(y), p1)/Np # solve for parameters b1 = np.dot(np.transpose(px), y)-Np*(np.dot(mu_x, np.transpose(mu_y))) b2 = np.dot(np.transpose(y*np.matlib.repmat(p1, 1, d)), y)-Np*np.dot(mu_y, np.transpose(mu_y)) b = np.dot(b1, np.linalg.inv(b2)) # ts is the translation ts = mu_x-np.dot(b, mu_y) sigma22 = np.abs(np.sum(np.sum(x*x*np.matlib.repmat(pt1, 1, d)))-Np * np.dot(np.transpose(mu_x), mu_x) - np.trace(np.dot(b1, np.transpose(b))))/(Np*d) # get a float number here sigma2 = sigma22[0][0] # Update centroids positioins t = np.dot(y, np.transpose(b))+np.matlib.repmat(np.transpose(ts), m, 1) iter = iter+1 return t
def register_nonrigid(x, y, w, lamb=3.0, beta=2.0, max_it=50, verbose=True, display=True): """ Registers Y to X using the Coherent Point Drift algorithm, in non-rigid fashion. Note: For affine transformation, t = y+g*wc(* is dot). Parameters ---------- x : ndarray The static shape that Y will be registered to. Expected array shape is [n_points_x, n_dims] y : ndarray The moving shape. Expected array shape is [n_points_y, n_dims]. Note that n_dims should be equal for X and Y, but n_points does not need to match. w : float Weight for the outlier suppression. Value is expected to be in range [0.0, 1.0]. lamb : float, optional lamb represents the trade-off between the goodness of maximum likelihood fit and regularization. Default value is 3.0. beta : float, optional beta defines the model of the smoothness regularizer(width of smoothing Gaussian filter in equation(20) of the paper).Default value is 2.0. max_it : int, optional Maximum number of iterations. Used to prevent endless looping when the algorithm does not converge. Default value is 150. tol : float Returns ------- t : ndarray The transformed version of y. Output shape is [n_points_y, n_dims]. """ import numpy as np import numpy.matlib import scipy.sparse from cpd.cpd_p import cpd_p import matplotlib.pyplot as plt # Construct G: g = y[:, np.newaxis, :] - y g = g * g g = np.sum(g, 2) g = np.exp(-1.0 / (2 * beta * beta) * g) [n, d] = x.shape [m, d] = y.shape t = y # initialize sigma^2 sigma2 = (m * np.trace(np.dot(np.transpose(x), x)) + n * np.trace(np.dot(np.transpose(y), y)) - 2 * np.dot(sum(x), np.transpose(sum(y)))) / (m * n * d) iter = 0 while (iter < max_it) and (sigma2 > 1.0e-5): if verbose and (iter == 0 or iter == max_it): print('error: {}'.format(sigma2)), [p1, pt1, px] = cpd_p(x, t, sigma2, w, m, n, d) # precompute diag(p) dp = scipy.sparse.spdiags(p1.T, 0, m, m) # wc is a matrix of coefficients wc = np.dot(np.linalg.inv(dp * g + lamb * sigma2 * np.eye(m)), (px - dp * y)) t = y + np.dot(g, wc) Np = np.sum(p1) sigma2 = np.abs((np.sum(x * x * np.matlib.repmat(pt1, 1, d)) + np.sum(t * t * np.matlib.repmat(p1, 1, d)) - 2 * np.trace(np.dot(px.T, t))) / (Np * d)) iter += 1 if verbose and (iter == 0 or iter == max_it): print('\tcentroid: x:{0:.3f} y:{1:.3f} z:{2:.3f}'.format( np.average(t[:, 0]), np.average(t[:, 1]), np.average(t[:, 2]))) if display: fig = plt.figure(figsize=(14, 14)) ax = fig.add_subplot(111, projection='3d') ax.scatter(x[:, 0], x[:, 1], x[:, 2], color='#2299FF') ax.scatter(t[:, 0], t[:, 1], t[:, 2], color='#222222', marker='x', s=50) ax.view_init(elev=25, azim=45) plt.title('error: {}'.format(sigma2)) plt.savefig('/projects/memotrack/temp/' + str(iter) + '.png') plt.close() return t