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
