diff --git a/Interpolation.py b/Interpolation.py new file mode 100644 index 0000000..31064d4 --- /dev/null +++ b/Interpolation.py @@ -0,0 +1,69 @@ +# -*- coding: utf-8 -*- +""" +Created on Tue Jan 29 18:32:13 2019 + +@author: Maria +""" + +import numpy as np +from matplotlib import pyplot as plt +import images +#from math import ceil +#from functools import reduce +from scipy import ndimage + +class DistanceTransformType(): + euclidean = 0 + +class SimpleInterpolation(): + ''' + This class calculates alpha values that gives some 'weight' to the + pixels + ''' + def __init__(self, sim_image, transform_type: DistanceTransformType): + + if transform_type == DistanceTransformType.euclidean: + None + self.dist_transform, _ = ndimage.distance_transform_edt(sim_image,return_indices = True) + self.alpha_dist = None + self.n_pixels = None + self.pts = None + self.ref_pts = None + self.denom = None + self.inverse_d = None + + else: + print('Error') + + def GetResult(self): + self.alpha_dist = np.zeros((self.dist_transform.shape[0], self.dist_transform.shape[1])) + self.n_pixels = self.dist_transform.shape[0]*self.dist_transform.shape[1] + self.pts = sim_image_object.getOtherPointsCoords() + self.ref_pts = sim_image_object.getPointRefCoords() + self.denom = self.dist_transform[self.pts].reshape( self.n_pixels - 1, self.dist_transform.shape[2]) + self.denom = 1/ self.denom[:] + self.inverse_d = np.sum(self.denom[:], axis=0) + + return self.alpha_dist + + +#dist_transform, indices = ndimage.distance_transform_edt(sim_image,return_indices = True) + +#alpha_dist = np.zeros((dist_transform.shape[0],dist_transform.shape[1])) +#n_pixels = dist_transform.shape[0]*dist_transform.shape[1] +# +##pts = np.where(sim_image > 0) +##ref_pts = np.where(sim_image < 1) +# +#pts = sim_image_object.getOtherPointsCoords() +#ref_pts = sim_image_object.getPointRefCoords() + +#denom = dist_transform[pts].reshape(n_pixels - 1, dist_transform.shape[2]) +#denom = 1/denom[:] +#inverse_d = np.sum(denom[:], axis=0) + +w_dist = ((1/dist_transform[pts]).reshape(n_pixels - 1, dist_transform.shape[2]))/inverse_d + +alpha = np.zeros(dist_transform.shape) +alpha[pts] = w_dist.ravel() +alpha[ref_pts] = 1 \ No newline at end of file diff --git a/my_interpolator.py b/my_interpolator.py index 96819c5..6af5ac9 100644 --- a/my_interpolator.py +++ b/my_interpolator.py @@ -8,13 +8,15 @@ """ import numpy as np from functools import reduce -import math +from math import ceil from scipy import ndimage import matplotlib.pyplot as plt from write_vtk_file import write_unstructured_file from pycpd import affine_registration from simulation.images import SimImage + + # ============================================================================= # 1. Crear mapa de distancias para cada punto # 2. Transformada de distancia (scikit-image) @@ -23,6 +25,21 @@ # 4. Aplicar la máscara # ============================================================================= +def show(img): + ''' + Show the image using matplotlib + @param nCol: number of columns of the figure. + ''' + nCol = 3 + plt.figure() + nRow = ceil(img.shape[2]/nCol) + for point_idx in range(img.shape[2]): + plt.subplot(nRow,nCol,point_idx+1) + plt.imshow(img[:,:,point_idx], "gray") + plt.xticks([]),plt.yticks([]) + plt.show() + + # n_ref_pts = 3 # shape = (5,5,n_ref_pts) @@ -52,18 +69,22 @@ sim_image_object = SimImage(shape, n_ref_pts) sim_image = sim_image_object.getImage() + # def Interpolation(sim_image): dist_transform, indices = ndimage.distance_transform_edt(sim_image,return_indices = True) alpha_dist = np.zeros((dist_transform.shape[0],dist_transform.shape[1])) n_pixels = dist_transform.shape[0]*dist_transform.shape[1] -pts = np.where(sim_image > 0) -ref_pts = np.where(sim_image < 1) +#pts = np.where(sim_image > 0) +#ref_pts = np.where(sim_image < 1) + +pts = sim_image_object.getOtherPointsCoords() +ref_pts = sim_image_object.getPointRefCoords() denom = dist_transform[pts].reshape(n_pixels - 1, dist_transform.shape[2]) denom = 1/denom[:] -inverse_d = np.sum(denom[:], axis=0) +inverse_d = np.sum(denom[:], axis=0) #Hay un denominador por cada plano w_dist = ((1/dist_transform[pts]).reshape(n_pixels - 1, dist_transform.shape[2]))/inverse_d @@ -86,17 +107,17 @@ ref_pts = np.array(ref_pts) disp_coords = displaced_pts - ref_pts -a = np.zeros(disp_coords.shape) -a[::3] = disp_coords.ravel()[::3] -a[1::3] = disp_coords.ravel()[1::3] -a[2::3] = disp_coords.ravel()[2::3] - +transforms = np.zeros((n_ref_pts,3)) - -v = np.zeros((3,5,5,3)) -v[0,:,:,:] = a[0] -v[1,:,:,:] = a[1] -v[2,:,:,:] = a[2] +for idx in range(n_ref_pts): + transforms.ravel()[idx::3] = disp_coords.ravel()[idx::n_ref_pts] + + +#(npoints,dim,dim,[x y z]) +v = np.zeros((n_ref_pts,shape[0],shape[1],3)) +v[0,:,:,:] = transforms[0] +v[1,:,:,:] = transforms[1] +v[2,:,:,:] = transforms[2] result = np.zeros(v.shape) for slice_idx in range(n_ref_pts): @@ -121,10 +142,15 @@ #Posiciones finales im_result = np.sum(result, axis=0) + Ym.reshape(5,5,3) + + + im_result0 = result[0,:,:,:] + Ym.reshape(5,5,3) im_result1 = result[1,:,:,:] + Ym.reshape(5,5,3) im_result2 = result[2,:,:,:] + Ym.reshape(5,5,3) -write_unstructured_file(im_result.reshape(25,3),grey_values) - +write_unstructured_file('grid.vtk',im_result.reshape(25,3),grey_values) +write_unstructured_file('grid0.vtk',im_result0.reshape(25,3),grey_values) +write_unstructured_file('grid1.vtk',im_result1.reshape(25,3),grey_values) +write_unstructured_file('grid2.vtk',im_result2.reshape(25,3),grey_values) diff --git a/pycpd b/pycpd deleted file mode 120000 index eeadfe0..0000000 --- a/pycpd +++ /dev/null @@ -1 +0,0 @@ -3rdParty/pycpd/pycpd/ \ No newline at end of file diff --git a/pycpd/__init__.py b/pycpd/__init__.py new file mode 100644 index 0000000..4db30c5 --- /dev/null +++ b/pycpd/__init__.py @@ -0,0 +1,3 @@ +from .affine_registration import affine_registration +from .rigid_registration import rigid_registration +from .deformable_registration import gaussian_kernel, deformable_registration diff --git a/pycpd/affine_registration.py b/pycpd/affine_registration.py new file mode 100644 index 0000000..5b681c2 --- /dev/null +++ b/pycpd/affine_registration.py @@ -0,0 +1,49 @@ +from builtins import super +import numpy as np +from .expectation_maximization_registration import expectation_maximization_registration + +class affine_registration(expectation_maximization_registration): + def __init__(self, B=None, t=None, *args, **kwargs): + super().__init__(*args, **kwargs) + self.B = np.eye(self.D) if B is None else B + self.t = np.atleast_2d(np.zeros((1, self.D))) if t is None else t + + def update_transform(self): + muX = np.divide(np.sum(np.dot(self.P, self.X), axis=0), self.Np) + muY = np.divide(np.sum(np.dot(np.transpose(self.P), self.Y), axis=0), self.Np) + + self.XX = self.X - np.tile(muX, (self.N, 1)) + YY = self.Y - np.tile(muY, (self.M, 1)) + + self.A = np.dot(np.transpose(self.XX), np.transpose(self.P)) + self.A = np.dot(self.A, YY) + + self.YPY = np.dot(np.transpose(YY), np.diag(self.P1)) + self.YPY = np.dot(self.YPY, YY) + + self.B = np.linalg.solve(np.transpose(self.YPY), np.transpose(self.A)) + self.t = np.transpose(muX) - np.dot(np.transpose(self.B), np.transpose(muY)) + + def transform_point_cloud(self, Y=None): + if Y is None: + self.TY = np.dot(self.Y, self.B) + np.tile(self.t, (self.M, 1)) + return + else: + return np.dot(Y, self.B) + np.tile(self.t, (Y.shape[0], 1)) + + def update_variance(self): + qprev = self.q + + trAB = np.trace(np.dot(self.A, self.B)) + xPx = np.dot(np.transpose(self.Pt1), np.sum(np.multiply(self.XX, self.XX), axis =1)) + trBYPYP = np.trace(np.dot(np.dot(self.B, self.YPY), self.B)) + self.q = (xPx - 2 * trAB + trBYPYP) / (2 * self.sigma2) + self.D * self.Np/2 * np.log(self.sigma2) + self.err = np.abs(self.q - qprev) + + self.sigma2 = (xPx - trAB) / (self.Np * self.D) + + if self.sigma2 <= 0: + self.sigma2 = self.tolerance / 10 + + def get_registration_parameters(self): + return self.B, self.t diff --git a/pycpd/deformable_registration.py b/pycpd/deformable_registration.py new file mode 100644 index 0000000..d3ef69b --- /dev/null +++ b/pycpd/deformable_registration.py @@ -0,0 +1,50 @@ +from builtins import super +import numpy as np +from .expectation_maximization_registration import expectation_maximization_registration + +def gaussian_kernel(Y, beta): + (M, D) = Y.shape + XX = np.reshape(Y, (1, M, D)) + YY = np.reshape(Y, (M, 1, D)) + XX = np.tile(XX, (M, 1, 1)) + YY = np.tile(YY, (1, M, 1)) + diff = XX-YY + diff = np.multiply(diff, diff) + diff = np.sum(diff, 2) + return np.exp(-diff / (2 * beta)) + +class deformable_registration(expectation_maximization_registration): + def __init__(self, alpha=2, beta=2, *args, **kwargs): + super().__init__(*args, **kwargs) + self.alpha = 2 if alpha is None else alpha + self.beta = 2 if alpha is None else beta + self.W = np.zeros((self.M, self.D)) + self.G = gaussian_kernel(self.Y, self.beta) + + def update_transform(self): + A = np.dot(np.diag(self.P1), self.G) + self.alpha * self.sigma2 * np.eye(self.M) + B = np.dot(self.P, self.X) - np.dot(np.diag(self.P1), self.Y) + self.W = np.linalg.solve(A, B) + + def transform_point_cloud(self, Y=None): + if Y is None: + self.TY = self.Y + np.dot(self.G, self.W) + return + else: + return Y + np.dot(self.G, self.W) + + def update_variance(self): + qprev = self.sigma2 + + xPx = np.dot(np.transpose(self.Pt1), np.sum(np.multiply(self.X, self.X), axis=1)) + yPy = np.dot(np.transpose(self.P1), np.sum(np.multiply(self.TY, self.TY), axis=1)) + trPXY = np.sum(np.multiply(self.TY, np.dot(self.P, self.X))) + + self.sigma2 = (xPx - 2 * trPXY + yPy) / (self.Np * self.D) + + if self.sigma2 <= 0: + self.sigma2 = self.tolerance / 10 + self.err = np.abs(self.sigma2 - qprev) + + def get_registration_parameters(self): + return self.G, self.W diff --git a/pycpd/expectation_maximization_registration.py b/pycpd/expectation_maximization_registration.py new file mode 100644 index 0000000..6d7db00 --- /dev/null +++ b/pycpd/expectation_maximization_registration.py @@ -0,0 +1,85 @@ +import numpy as np + +def initialize_sigma2(X, Y): + (N, D) = X.shape + (M, _) = Y.shape + XX = np.reshape(X, (1, N, D)) + YY = np.reshape(Y, (M, 1, D)) + XX = np.tile(XX, (M, 1, 1)) + YY = np.tile(YY, (1, N, 1)) + diff = XX - YY + err = np.multiply(diff, diff) + return np.sum(err) / (D * M * N) + +class expectation_maximization_registration(object): + def __init__(self, X, Y, sigma2=None, max_iterations=100, tolerance=0.001, w=0, *args, **kwargs): + if type(X) is not np.ndarray or X.ndim != 2: + raise ValueError("The target point cloud (X) must be at a 2D numpy array.") + if type(Y) is not np.ndarray or Y.ndim != 2: + raise ValueError("The source point cloud (Y) must be a 2D numpy array.") + if X.shape[1] != Y.shape[1]: + raise ValueError("Both point clouds need to have the same number of dimensions.") + + self.X = X + self.Y = Y + self.sigma2 = sigma2 + (self.N, self.D) = self.X.shape + (self.M, _) = self.Y.shape + self.tolerance = tolerance + self.w = w + self.max_iterations = max_iterations + self.iteration = 0 + self.err = self.tolerance + 1 + self.P = np.zeros((self.M, self.N)) + self.Pt1 = np.zeros((self.N, )) + self.P1 = np.zeros((self.M, )) + self.Np = 0 + + def register(self, callback=lambda **kwargs: None): + self.transform_point_cloud() + if self.sigma2 is None: + self.sigma2 = initialize_sigma2(self.X, self.TY) + self.q = -self.err - self.N * self.D/2 * np.log(self.sigma2) + while self.iteration < self.max_iterations and self.err > self.tolerance: + self.iterate() + if callable(callback): + kwargs = {'iteration': self.iteration, 'error': self.err, 'X': self.X, 'Y': self.TY} + callback(**kwargs) + + return self.TY, self.get_registration_parameters() + + def get_registration_parameters(self): + raise NotImplementedError("Registration parameters should be defined in child classes.") + + def iterate(self): + self.expectation() + self.maximization() + self.iteration += 1 + + def expectation(self): + P = np.zeros((self.M, self.N)) + + for i in range(0, self.M): + diff = self.X - np.tile(self.TY[i, :], (self.N, 1)) + diff = np.multiply(diff, diff) + P[i, :] = P[i, :] + np.sum(diff, axis=1) + + c = (2 * np.pi * self.sigma2) ** (self.D / 2) + c = c * self.w / (1 - self.w) + c = c * self.M / self.N + + P = np.exp(-P / (2 * self.sigma2)) + den = np.sum(P, axis=0) + den = np.tile(den, (self.M, 1)) + den[den==0] = np.finfo(float).eps + den += c + + self.P = np.divide(P, den) + self.Pt1 = np.sum(self.P, axis=0) + self.P1 = np.sum(self.P, axis=1) + self.Np = np.sum(self.P1) + + def maximization(self): + self.update_transform() + self.transform_point_cloud() + self.update_variance() diff --git a/pycpd/rigid_registration.py b/pycpd/rigid_registration.py new file mode 100644 index 0000000..67a6f92 --- /dev/null +++ b/pycpd/rigid_registration.py @@ -0,0 +1,55 @@ +from builtins import super +import numpy as np +from .expectation_maximization_registration import expectation_maximization_registration + +class rigid_registration(expectation_maximization_registration): + def __init__(self, R=None, t=None, s=None, *args, **kwargs): + super().__init__(*args, **kwargs) + if self.D != 2 and self.D != 3: + message = 'Rigid registration only supports 2D or 3D point clouds. Instead got {}.'.format(self.D) + raise ValueError(message) + if s == 0: + raise ValueError('A zero scale factor is not supported.') + self.R = np.eye(self.D) if R is None else R + self.t = np.atleast_2d(np.zeros((1, self.D))) if t is None else t + self.s = 1 if s is None else s + + def update_transform(self): + muX = np.divide(np.sum(np.dot(self.P, self.X), axis=0), self.Np) + muY = np.divide(np.sum(np.dot(np.transpose(self.P), self.Y), axis=0), self.Np) + + self.XX = self.X - np.tile(muX, (self.N, 1)) + YY = self.Y - np.tile(muY, (self.M, 1)) + + self.A = np.dot(np.transpose(self.XX), np.transpose(self.P)) + self.A = np.dot(self.A, YY) + + U, _, V = np.linalg.svd(self.A, full_matrices=True) + C = np.ones((self.D, )) + C[self.D-1] = np.linalg.det(np.dot(U, V)) + + self.R = np.transpose(np.dot(np.dot(U, np.diag(C)), V)) + self.YPY = np.dot(np.transpose(self.P1), np.sum(np.multiply(YY, YY), axis=1)) + self.s = np.trace(np.dot(np.transpose(self.A), np.transpose(self.R))) / self.YPY + self.t = np.transpose(muX) - self.s * np.dot(np.transpose(self.R), np.transpose(muY)) + + def transform_point_cloud(self, Y=None): + if Y is None: + self.TY = self.s * np.dot(self.Y, self.R) + np.tile(self.t, (self.M, 1)) + return + else: + return self.s * np.dot(Y, self.R) + np.tile(self.t, (Y.shape[0], 1)) + + def update_variance(self): + qprev = self.q + + trAR = np.trace(np.dot(self.A, self.R)) + xPx = np.dot(np.transpose(self.Pt1), np.sum(np.multiply(self.XX, self.XX), axis =1)) + self.q = (xPx - 2 * self.s * trAR + self.s * self.s * self.YPY) / (2 * self.sigma2) + self.D * self.Np/2 * np.log(self.sigma2) + self.err = np.abs(self.q - qprev) + self.sigma2 = (xPx - self.s * trAR) / (self.Np * self.D) + if self.sigma2 <= 0: + self.sigma2 = self.tolerance / 10 + + def get_registration_parameters(self): + return self.s, self.R, self.t diff --git a/simulation/images.py b/simulation/images.py index 8baee7c..be26891 100644 --- a/simulation/images.py +++ b/simulation/images.py @@ -49,6 +49,12 @@ def getPointRefCoords(self): ''' return np.where(self.img < 1) + def getOtherPointsCoords(self): + ''' + Return the X, Y and Z coords in a list with each axes separated. + [[x_0, x_1, ..., x_n] [y_0, y_1, ..., y_n] [z_0, z_1, ..., z_n]] + ''' + return np.where(self.img > 0) if __name__ == "__main__": test = SimImage((5,5), 4) diff --git a/write_vtk_file.py b/write_vtk_file.py index 39e14ba..b234a06 100644 --- a/write_vtk_file.py +++ b/write_vtk_file.py @@ -8,7 +8,7 @@ import numpy as np #def write_unstructured_file(Ym, r_values, g_values, b_values): -def write_unstructured_file(Ym,r_values,): +def write_unstructured_file(file_name,Ym,r_values): r_values = r_values.reshape(25,1) g_values = r_values b_values = r_values @@ -20,7 +20,7 @@ def write_unstructured_file(Ym,r_values,): alpha = 0.5 #opacity of the rgb values #Create a DATAFILE - fd = open("grid_final.vtk","w") #vtk unstructured_grid_prueba_source.vtk + fd = open("{}".format(file_name),"w") #vtk unstructured_grid_prueba_source.vtk fd.write("# vtk DataFile Version 4.2\n") fd.write("vtk output\n") fd.write("ASCII \n\n")