def main():
"""
Creates two separate point clouds and performs rigid registration.
Great visualization, works well with small batches of data.
Need to find appropriate way of downsampling data
Can define threshold error in pycpd source files
Args:
None
Returns:
None
"""
target = np.loadtxt('../data/heart_scan_processed.txt')
source = np.loadtxt('../data/heart_scan_processed.txt')
X = source[780:800,:]
pts = X.transpose()
theta = np.radians(30)
c,s = np.cos(theta), np.sin(theta)
R = np.array( ( (c, -s, 0), (s, c, 0), (0, 0, 1) ) )
pts = np.matmul(R, pts)
source = pts.transpose()
target = target[700:900,:]
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(**{ 'X': source, 'Y': target })
TY, (s_reg, R_reg, t_reg) = reg.register(callback)
print(s_reg, R_reg, t_reg)
plt.show()
def RigidTransform(Y, X):
# Y = points_right
# X = points_left
muY = np.mean(Y,axis=0)
muX = np.mean(X,axis=0)
X_centrized = X - muX
Y_centrized = Y - muY
#reg = rigid_registration(**{ 'X': X_centrized, 'Y':Y_centrized })
reg = rigid_registration(**{ 'X': X, 'Y':Y })
reg.register(callback)
'''
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg.register(callback)
plt.show()
'''
permutate_idx = np.argmax(reg.P,axis=1)
#resid = X[permutate_idx] - (Y.dot(reg.R.T) + muX - muY.dot(reg.R.T) + reg.t)
scale= reg.s
rotation = Rot.from_dcm(reg.R)
quat = rotation.as_quat()
#trans = muX - muY.dot(reg.R.T) + reg.t
trans = reg.t
return scale, quat, trans, reg.err
def run_CPD(input_data):
'''Run the CPD algorithm and return the identified object'''
try:
X = np.array(input_data['X'] if 'X' in input_data else input_data['x'])
Y = np.array(input_data['Y'] if 'Y' in input_data else input_data['y'])
except:
abort(404)
reg = rigid_registration(**{'X': Y, 'Y': X, 'tolerance': 0.00001})
return reg.register()
def main():
X = np.loadtxt('data/fish_target.txt')
Y = np.loadtxt('data/fish_source.txt')
fig = plt.figure()
fig.add_axes([0, 0, 1, 1])
callback = partial(visualize, ax=fig.axes[0])
reg = rigid_registration(**{'X': X, 'Y': Y})
reg.register(callback)
plt.show()
def test_2D():
theta = np.pi / 6.0
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
t = np.array([0.5, 1.0])
X = np.loadtxt('data/fish_target.txt')
Y = np.dot(X, R) + np.tile(t, (np.shape(X)[0], 1))
reg = rigid_registration(**{'X': X, 'Y': Y})
TY, _ = reg.register()
assert_array_almost_equal(TY, X)
def main():
X = np.loadtxt('data/bunny_target.txt')
Y = np.loadtxt(
'data/bunny_source.txt') #synthetic data, equaivalent to X + 1
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(**{'X': X, 'Y': Y})
reg.register(callback)
plt.show()
def main():
#fish = loadmat('./bunny.mat')
#X = fish['X']
#print "Number of X Vertices: %d" % (len(X))
#Y = X + 1
#print "Number of Y Vertices: %d" % (len(Y))
X = np.array([]).reshape(0, 3)
with open(join('public', 'uploads', sys.argv[1]), "r") as filestream:
for line in filestream:
if line.startswith("v "):
line = line[2:]
currentLine = line.split(" ")
currentLine[2] = currentLine[2][:-1]
currentVertex = np.array(map(float, currentLine))
X = np.vstack([X, currentVertex])
#print "Number of Vertices: %d" % (len(X))
#print 'Type of X data struct: ',type(X)
#print X
Y = np.array([]).reshape(0, 3)
with open(join('public', 'uploads', sys.argv[2]), "r") as filestream:
for line in filestream:
if line.startswith("v "):
line = line[2:]
currentLine = line.split(" ")
currentLine[2] = currentLine[2][:-1]
currentVertex = np.array(map(float, currentLine))
Y = np.vstack([Y, currentVertex])
#print "Number of Vertices: %d" % (len(Y))
#print Y
## fig = plt.figure()
## ax = fig.add_subplot(111, projection='3d')
## callback = partial(visualize, ax=ax)
reg = rigid_registration(X, Y)
reg.maxIterations = 150
## reg.register(callback)
reg.register(None)
#print 'Total Iterations: ',reg.iteration
#print 'End Error: ',reg.err
## np.savetxt("X.txt", X)
## np.savetxt("Y.txt", Y)
np.savetxt(join('public', 'uploads', sys.argv[2] + "T.txt"), reg.Y)
corrArray = cpdCorresp(reg.X, reg.Y, reg.sigma2, 0.6, reg.X.shape[0],
reg.Y.shape[0], reg.X.shape[1])
np.savetxt(join('public', 'uploads', sys.argv[2] + "Corr.txt"), corrArray)
with open(join('public', 'uploads', sys.argv[2] + "Corr.json"),
"w") as stream:
json.dump(corrArray.tolist(), stream)
def main():
fish = loadmat('../data/fish.mat')
X = fish['X']
Y = fish['Y']
fig = plt.figure()
fig.add_axes([0, 0, 1, 1])
callback = partial(visualize, ax=fig.axes[0])
reg = rigid_registration(X, Y)
reg.register(callback)
plt.show()
def main():
fish = loadmat('../data/bunny.mat')
X = fish['X']
Y = X + 1
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(X, Y)
reg.register(callback)
plt.show()
def alignment_rigid(target, source, visualization=False):
start = time.time()
X = np.loadtxt(target)
Y = np.loadtxt(source) #synthetic data, equaivalent to X + 1
if visualization is True:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(**{'X': X, 'Y': Y}, max_iterations=max_iterations)
reg.register(callback)
plt.show()
else:
callback = partial(saveData)
reg = rigid_registration(**{'X': X, 'Y': Y}, max_iterations=max_iterations)
reg.register(callback)
print('Elapsed time: ' + str(time.time() - start))
def main(f_femur_target, f_model):
# reg = rigid_registration(**{'X': femur_target, 'Y': model})
reg = rigid_registration(**{
'X': f_femur_target,
'Y': f_model
}) # register the femur surface from the Depth Camera to
# the model, minimising the distance between the points of the femur surface and the model
reg.register()
f_dictionary = {
"transformed_model": reg.register()[0],
"probability": reg.register()[2],
"rotation_matrix": reg.register()[3],
"translation": reg.register()[4]
}
return f_dictionary
def main():
fish = loadmat('../data/fish.mat')
X = np.zeros((fish['X'].shape[0], fish['X'].shape[1] + 1))
X[:, :-1] = fish['X']
Y = np.zeros((fish['Y'].shape[0], fish['Y'].shape[1] + 1))
Y[:, :-1] = fish['Y']
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(X, Y)
reg.register(callback)
plt.show()
def main():
fish_target = np.loadtxt('data/fish_target.txt')
X = np.zeros((fish_target.shape[0], fish_target.shape[1] + 1))
X[:,:-1] = fish_target
fish_source = np.loadtxt('data/fish_source.txt')
Y = np.zeros((fish_source.shape[0], fish_source.shape[1] + 1))
Y[:,:-1] = fish_source
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(**{ 'X': X, 'Y':Y })
reg.register(callback)
plt.show()
def test_3D():
theta = np.pi / 6.0
R = np.array([[np.cos(theta), -np.sin(theta), 0], [np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
t = np.array([0.5, 1.0, -2.0])
fish_target = np.loadtxt('data/fish_target.txt')
Y = np.zeros((fish_target.shape[0], fish_target.shape[1] + 1))
Y[:,:-1] = fish_target
X = np.dot(Y, R) + np.tile(t, (np.shape(Y)[0], 1))
reg = rigid_registration(**{ 'X': X, 'Y':Y })
TY, (s_reg, R_reg, t_reg) = reg.register()
assert_almost_equal(1.0, s_reg)
assert_array_almost_equal(R, R_reg)
assert_array_almost_equal(t, t_reg)
assert_array_almost_equal(X, TY)
def align_images(t_img, cp_img, p_img):
# Aligning page with template (template size and location is stationary, origin is top left corner of image)
# Binarizing the template and the cleaned_page, pre-processing for the alignment
threshold_level = 250
t_img = (t_img > threshold_level) * np.uint8(255)
cp_img = (cp_img > threshold_level) * np.uint8(255)
# Getting the point clouds that we will match via ICP
X = np.column_stack(np.where(t_img < threshold_level))
Y = np.column_stack(np.where(cp_img < threshold_level))
# This can be done with 3000 points each
maxpoints = 3000
X = X[::int(X.shape[0] / maxpoints)]
Y = Y[::int(Y.shape[0] / maxpoints)]
# Point matching via ICP from pycpd, which scales, rotates and translates
reg = rigid_registration(**{'X': X, 'Y': Y, 'max_iterations': 1000, 'tolerance': 0.0001})
reg.register()
# Applying the transformation from the point cloud registration with an affine transformation
adj_page = p_img.copy() # Copying the page, adj_page will be the output of this (adjusted page)
M = np.zeros((2, 3)) # Creating the affine transformation matrix
M[:, 0:2] = reg.s * reg.R
M[0, 2] = reg.t[1]
M[1, 2] = reg.t[0]
adj_page = cv2.warpAffine(adj_page, M, (adj_page.shape[1], adj_page.shape[0]), flags=cv2.INTER_AREA)
# Cropping or padding image to make it the same size as the template
old_sz = adj_page.shape
new_sz = t_img.shape
# First dim (removing or padding bottom of image)
if old_sz[0] > new_sz[0]:
adj_page = adj_page[0:new_sz[0], :]
elif old_sz[0] < new_sz[0]:
adj_page = cv2.copyMakeBorder(adj_page, 0, 0, 0, new_sz[0] - old_sz[0], cv2.BORDER_CONSTANT, value=255)
# Second dim (removing or padding right edge of image)
if old_sz[1] > new_sz[1]:
adj_page = adj_page[:, 0:new_sz[1]]
elif old_sz[1] < new_sz[1]:
adj_page = cv2.copyMakeBorder(adj_page, 0, new_sz[1] - old_sz[1], 0, 0, cv2.BORDER_CONSTANT, value=255)
return adj_page, reg.s, reg.R, reg.t
def rigid_alignment(X, Y): #rigid alignment
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = rigid_registration(**{ 'X': X, 'Y': Y })
reg.register(callback)
plt.show()
print("the scalar is ")
print(reg.s)
print("the translation vector is ")
print(reg.t)
print("the rotation matrix is ")
print(reg.R)
print(Y.shape[0])
return (reg.s, reg.t, reg.R)
def Update(self, target, source, mode='affine'):
# registration
import pycpd as cpd
self.source = source
if mode == 'affine':
self.reg = cpd.affine_registration(target,
self.source,
tolerance=1e-3)
else:
self.reg = cpd.rigid_registration(target,
self.source,
tolerance=1e-3)
self.reg.register(callback=None)
def Update(self, target, source, mode='affine'):
# registration
import pycpd as cpd
from functools import partial
self.source = source
nodes_before = np.array(self.source.GetNodesPos())
if mode == 'affine':
self.reg = cpd.affine_registration(np.array(target.GetNodesPos()),
nodes_before,
tolerance=1e-3)
else:
self.reg = cpd.rigid_registration(np.array(target.GetNodesPos()),
nodes_before,
tolerance=1e-3)
self.reg.register(callback=None)
self.reg.updateTransform()
def main(ID_target, ID_source):
target_load = o3d.io.read_point_cloud('C:/Users/monre/OneDrive - Imperial College London/ME4/FYP/DATA/OAI-ZIB/processed_data/04.registered_scaled_PCDs/femur_bone_registered_' + ID_target + '.ply')
femur_target = np.asarray(target_load.points)
source_load = o3d.io.read_point_cloud('C:/Users/monre/OneDrive - Imperial College London/ME4/FYP/code/SSM/VCG/MeshLab/03.downsampled_PCDs/femur_bone_downsampled_' + ID_source + '.ply')
femur_source = np.asarray(source_load.points)
'''
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
'''
reg = rigid_registration(**{ 'X': femur_target, 'Y':femur_source })
#reg.register(callback)
#plt.show()
#return reg.register(callback)[0]
#return reg.register(callback)[2]
dictionary = {"femur": reg.register()[0],
"scaling": reg.register()[2]}
return dictionary
def calc_rot_without_correspondence(from_pts, to_pts):
"""
Calculates the rotation between two point clouds - note that no correspondence between
the point clouds is required, the points can be provided in any order.
d - number of dimensions
n - number of points
:param from_pts: `d x n` matrix
:param to_pts: `d x n` matrix
:return: rotation matrix, translation vector, scale
"""
from_cent = np.mean(from_pts, axis=1)
to_cent = np.mean(to_pts, axis=1)
params = {
'X': from_pts.T - from_cent,
'Y': to_pts.T - to_cent,
}
cpd = pycpd.rigid_registration(**params)
TY, (s, R, t) = cpd.register()
return R, t, s
def RegisterGraph(target, source, mode='affine', tolerance=1e-3):
# registration
import pycpd as cpd
from functools import partial
nodes_before = np.array(source.GetNodesPos())
if mode == 'affine':
new_pos = cpd.affine_registration(np.array(target.GetNodesPos()),
nodes_before,
tolerance=tolerance)
else:
new_pos = cpd.rigid_registration(np.array(target.GetNodesPos()),
nodes_before,
tolerance=tolerance)
new_pos.register(callback=None)
r = new_pos.updateTransform()
nodes_after = new_pos.TY
for idx, i in zip(source.GetNodes(), nodes_after):
source.node[idx]['pos'] = i
return source
def main():
time_start = ph.start_timing()
# Read input
parser = argparse.ArgumentParser(
description="Perform rigid registration using landmarks",
prog=None,
epilog="Author: Michael Ebner ([email protected])",
)
parser.add_argument(
"-f",
"--fixed",
help="Path to fixed image landmarks.",
type=str,
required=1,
)
parser.add_argument(
"-m",
"--moving",
help="Path to moving image landmarks.",
type=str,
required=1,
)
parser.add_argument(
"-o",
"--output",
help="Path for obtained SimpleITK registration transform (.txt)",
type=str,
required=1,
)
parser.add_argument(
"-v",
"--verbose",
help="Turn on/off verbose output",
type=int,
required=0,
default=0,
)
parser.add_argument(
"--pca",
"-pca",
action="store_true",
help="If given, principal component analysis (PCA) is used "
"to test various initializations for the point based registrations.")
args = parser.parse_args()
landmarks_fixed_nda = dr.DataReader.read_landmarks(args.fixed)
landmarks_moving_nda = dr.DataReader.read_landmarks(args.moving)
if args.pca:
ph.print_subtitle("Use PCA to initialize registrations")
pca_fixed = pca.PrincipalComponentAnalysis(landmarks_fixed_nda)
pca_fixed.run()
eigvec_fixed = pca_fixed.get_eigvec()
mean_fixed = pca_fixed.get_mean()
pca_moving = pca.PrincipalComponentAnalysis(landmarks_moving_nda)
pca_moving.run()
eigvec_moving = pca_moving.get_eigvec()
mean_moving = pca_moving.get_mean()
# test different initializations based on eigenvector orientations
orientations = [
[1, 1],
[1, -1],
[-1, 1],
[-1, -1],
]
error = np.inf
for i_o, orientation in enumerate(orientations):
eigvec_moving_o = np.array(eigvec_moving)
eigvec_moving_o[:, 0] *= orientation[0]
eigvec_moving_o[:, 1] *= orientation[1]
# get right-handed coordinate system
cross = np.cross(eigvec_moving_o[:, 0], eigvec_moving_o[:, 1])
eigvec_moving_o[:, 2] = cross
# transformation to align fixed with moving eigenbasis
R = eigvec_moving_o.dot(eigvec_fixed.transpose())
t = mean_moving - R.dot(mean_fixed)
ph.print_info(
"Registration based on PCA eigenvector initialization "
"%d/%d ... " % (i_o + 1, len(orientations)),
newline=False)
reg = pycpd.rigid_registration(
**{
"Y": landmarks_fixed_nda,
"X": landmarks_moving_nda,
"max_iterations": 100,
"R": R,
"t": t,
})
reg.register()
params = reg.get_registration_parameters()
scale, R, t = params
error_o = reg.err
print("done. Error: %.2f" % error_o)
if error_o < error:
error = error_o
rotation_matrix_nda = np.array(R)
translation_nda = np.array(t)
ph.print_info("Currently best estimate")
else:
reg = pycpd.rigid_registration(
**{
"Y": landmarks_fixed_nda,
"X": landmarks_moving_nda,
"max_iterations": 100,
})
if args.verbose:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
else:
callback = None
ph.print_info("Registration ... ", newline=False)
reg.register(callback)
if args.verbose:
plt.show(block=False)
# reg.register()
scale, R, t = reg.get_registration_parameters()
rotation_matrix_nda = R
translation_nda = t
print("done. Error: %.2f" % reg.err)
rigid_transform_sitk = sitk.Euler3DTransform()
rigid_transform_sitk.SetMatrix(rotation_matrix_nda.flatten())
rigid_transform_sitk.SetTranslation(translation_nda)
dw.DataWriter.write_transform(rigid_transform_sitk,
args.output,
verbose=True)
elapsed_time_total = ph.stop_timing(time_start)
ph.print_info("Computational Time: %s" % elapsed_time_total)
return 0
def register3DscanWithHead(f1, f2):
m1 = aims.read(f1)
m2 = aims.read(f2)
tr = rigid_registration(np.array(m1.vertex()), np.array(m2.vertex()))
return tr
def pycpd(detections_df, verbose=False, display=True):
'''
THIS DOESN'T WORK ! pycpd IS NOT WELL IMPLEMENTED IN 3D !
:param detections_df:
:param verbose:
:param display:
:return:
'''
import memotrack.registration
import sys
import numpy as np
import pycpd
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from functools import partial
# This is the callback for the cpd class
def visualize(iteration, error, X, Y):
if display:
print('error: {}'.format(error))
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(111, projection='3d')
ax.scatter(X[:, 0], X[:, 1], X[:, 2], color='#2233FF')
ax.scatter(Y[:, 0], Y[:, 1], Y[:, 2], color='#22FF33')
ax.view_init(elev=25, azim=45)
plt.title('error: {}'.format(error))
plt.savefig('/projects/memotrack/temp/' + str(iteration) + '.png')
print('error: {}'.format(error))
# Get total number of frames
nframes = detections_df['t'].nunique()
# Get the reference frame for the required registration step
middle_frame = int(nframes / 2.0)
# middle_frame = 5 # FOR DEBUG
if verbose:
print('\nStarting registration using Coherent Point Drift')
print('Using frame {} as reference for registration.'.format(
middle_frame))
# Creating registered coords as copy and reseting index
if 'xreg' not in detections_df.columns:
detections_df = detections_df.reset_index()
detections_df['xreg'] = detections_df['x']
detections_df['yreg'] = detections_df['y']
detections_df['zreg'] = detections_df['z']
# Get the referance timeframe as a numpy array
ref_df = detections_df[detections_df['t'] == middle_frame]
ref = ref_df[['x', 'y', 'z']].as_matrix().astype('float')
for frame in range(nframes):
if verbose:
print('\nFrame: {}'.format(frame))
# Get current frame as numpy array
current_df = detections_df[detections_df['t'] == frame]
current = current_df[['x', 'y', 'z']].as_matrix().astype('float')
if verbose:
print('{} to {}'.format(np.shape(current), np.shape(ref)))
print('Current: {} \t Ref: {}'.format(np.average(current),
np.average(ref)))
reg = pycpd.rigid_registration(current, ref, sigma2=0.01)
orig_z = reg.TY[:, 2]
# ## Plot before
# fig = plt.figure(figsize=(20, 20))
# ax = fig.add_subplot(111, projection='3d')
# ax.scatter(reg.X[:, 0], reg.X[:, 1], reg.X[:, 2], color='#2233FF')
# ax.scatter(reg.Y[:, 0], reg.Y[:, 1], reg.Y[:, 2], color='#22FF33')
# ax.view_init(elev=30, azim=45)
# Register
reg.register(partial(visualize))
# ## Plot after
# fig2 = plt.figure(figsize=(20, 20))
# ax2 = fig2.add_subplot(111, projection='3d')
# ax2.scatter(reg.X[:, 0], reg.X[:, 1], reg.X[:, 2], color='#2233FF')
# ax2.scatter(reg.TY[:, 0], reg.TY[:, 1], reg.TY[:, 2], color='#22FF33')
# ax2.view_init(elev=30, azim=45)
print('Current: {} \t Ref: {}'.format(np.average(reg.X),
np.average(reg.TY)))
new_z = reg.TY[:, 2]
diff_z = abs(orig_z - new_z)
print(np.sort(diff_z))
return detections_df
break
prev_err = err
icp_t += it
R = R @ rotmat
#print(it)
#print(rotmat)
icp_q = transforms3d.quaternions.mat2quat(R)
icp_t = icp_t
icp_times.append(time.time() - t1)
data_log_icp.append([icp_q.dot(true_q), np.linalg.norm(icp_t - t)])
t1 = time.time()
reg = rigid_registration(X=source,
Y=samples_for_icp,
max_iterations=CPD_ITERS,
tolerance=CPD_THRESH)
TY, (s_reg, R_reg, t_reg) = reg.register()
cpd_times.append(time.time() - t1)
H = (TY - TY.mean(0)).T @ (samples_for_icp - samples_for_icp.mean(0))
u, s, vt = np.linalg.svd(H)
R_reg = vt.T @ np.diag([1, 1, np.linalg.det(vt.T @ u.T)]) @ u.T
t_reg = TY.mean(0) - samples_for_icp.mean(0)
cpd_q = transforms3d.quaternions.mat2quat(R_reg)
data_log_cpd.append([cpd_q.dot(true_q), np.linalg.norm(t_reg - t)])
if False:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(source[:, 0], source[:, 1], source[:, 2], label='orig')
ax.scatter(samples[:, 0], samples[:, 1], samples[:, 2], label='trans')
