Commit 6e6f486b authored by Qianfang Liao's avatar Qianfang Liao
Browse files

add python code

parent e23254bd
import time
import numpy as np
from math import sin, cos, pi
from scipy.linalg import norm
from fuzzypsreg import FuzzyPSReg
'''import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d'''
P_F = np.loadtxt('../data/fixedSet1.txt') # fixed point set P_F (N_PF x 3)
P_M = np.loadtxt('../data/movingSet1.txt') # moving point set P_M (N_PM x 3)
'''
###### Initial pose setting for P_M
r0 = -pi + 2*pi*np.random.rand(3) # r0 = np.array([ ])
theta = norm(r0)
if ( theta == 0 ):
R0 = np.eye(3)
else:
M = 1.0/theta*np.array([[0, -r0[2], r0[1]], [r0[2], 0, -r0[0]], [-r0[1], r0[0], 0]])
R0 = np.eye(3) + M*sin(theta) + M.dot(M)*(1 - cos(theta))
P_M = P_M.dot(R0.T)
'''
###### Parameter setting
noisyF, noisyM = 0, 0
trimRatio = 0
N_C = 50
gsF, gsM = 0.003, 0.0019
gsF_fine, gsM_fine = 0.0065, 0.0035
RRange, TRange = pi, 0.5
sigma_r, sigma_t = 0.01, TRange/2.0
epsilon = 0
###### Fuzzy Cluster-based Point Set Registration
start = time.time()
f = FuzzyPSReg(RRange, TRange, sigma_r, sigma_t, epsilon)
R, t = f.psr(P_F, P_M, noisyF, noisyM, trimRatio, N_C, gsF, gsM, gsF_fine, gsM_fine)
end = time.time()
print('The time cost (sec) is: ')
print(end - start)
'''
###### Plot the point sets
P_MRnT = P_M.dot(R.T) + t
fig = plt.figure()
ax1 = fig.add_subplot(121, projection='3d')
ax1.set_title('initial poses', fontsize = 12)
ax1.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax1.scatter(P_M[:,0], P_M[:,1], P_M[:,2], c='b', marker='.')
ax2 = fig.add_subplot(122, projection='3d')
ax2.set_title('after registration', fontsize = 12)
ax2.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax2.scatter(P_MRnT[:,0], P_MRnT[:,1], P_MRnT[:,2], c='b', marker='.')
plt.show()
'''
import time
import numpy as np
from math import sin, cos, pi
from scipy.linalg import norm
from fuzzypsreg import FuzzyPSReg
'''import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d'''
P_F = np.loadtxt('../data/fixedSet2.txt') # fixed point set P_F (N_PF x 3)
P_M = np.loadtxt('../data/movingSet2.txt') # moving point set P_M (N_PM x 3)
###### Initial pose setting for P_M
r0 = np.array([0, 0.5*pi, 0])
theta = norm(r0)
if ( theta == 0 ):
R0 = np.eye(3)
else:
M = 1.0/theta*np.array([[0, -r0[2], r0[1]], [r0[2], 0, -r0[0]], [-r0[1], r0[0], 0]])
R0 = np.eye(3) + M*sin(theta) + M.dot(M)*(1 - cos(theta))
P_M = P_M.dot(R0.T)
###### Parameter setting
noisyF, noisyM = 0, 0
trimRatio = 0
N_C = 80
gsF, gsM = 70, 70
gsF_fine, gsM_fine = 173, 194
RRange, TRange = pi, 0.2
sigma_r, sigma_t = 0.01, TRange/4.0
epsilon = 0
###### Fuzzy Cluster-based Point Set Registration
start = time.time()
f = FuzzyPSReg(RRange, TRange, sigma_r, sigma_t, epsilon)
R, t = f.psr(P_F, P_M, noisyF, noisyM, trimRatio, N_C, gsF, gsM, gsF_fine, gsM_fine)
end = time.time()
print('The time cost (sec) is: ')
print(end - start)
'''
###### Plot the point sets
P_MRnT = P_M.dot(R.T) + t
fig = plt.figure()
ax1 = fig.add_subplot(121, projection='3d')
ax1.set_title('initial poses', fontsize = 12)
ax1.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax1.scatter(P_M[:,0], P_M[:,1], P_M[:,2], c='b', marker='.')
ax2 = fig.add_subplot(122, projection='3d')
ax2.set_title('after registration', fontsize = 12)
ax2.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax2.scatter(P_MRnT[:,0], P_MRnT[:,1], P_MRnT[:,2], c='b', marker='.')
plt.show()
'''
import time
import numpy as np
from math import sin, cos, pi
from scipy.linalg import norm
from fuzzypsreg import FuzzyPSReg
'''import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d'''
P_F = np.loadtxt('../data/fixedSet3.txt') # fixed point set P_F (N_PF x 3)
P_M = np.loadtxt('../data/movingSet3.txt') # moving point set P_M (N_PM x 3)
###### Initial pose setting for P_M
r0 = -pi + 2*pi*np.random.rand(3) # r0 = np.array([ ])
theta = norm(r0)
if ( theta == 0 ):
R0 = np.eye(3)
else:
M = 1.0/theta*np.array([[0, -r0[2], r0[1]], [r0[2], 0, -r0[0]], [-r0[1], r0[0], 0]])
R0 = np.eye(3) + M*sin(theta) + M.dot(M)*(1 - cos(theta))
P_M = P_M.dot(R0.T)
###### Parameter setting
noisyF, noisyM = 0, 0
trimRatio = 0.005
N_C = 60
gsF, gsM = 0.3, 0.3
gsF_fine, gsM_fine = 0.65, 0.55
RRange, TRange = pi, 0.2
sigma_r, sigma_t = 0.01, TRange/4.0
epsilon = 0
###### Fuzzy Cluster-based Point Set Registration
start = time.time()
f = FuzzyPSReg(RRange, TRange, sigma_r, sigma_t, epsilon)
R, t = f.psr(P_F, P_M, noisyF, noisyM, trimRatio, N_C, gsF, gsM, gsF_fine, gsM_fine)
end = time.time()
print('The time cost (sec) is: ')
print(end - start)
'''
###### Plot the point sets
P_MRnT = P_M.dot(R.T) + t
fig = plt.figure()
ax1 = fig.add_subplot(121, projection='3d')
ax1.set_title('initial poses', fontsize = 12)
ax1.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax1.scatter(P_M[:,0], P_M[:,1], P_M[:,2], c='b', marker='.')
ax2 = fig.add_subplot(122, projection='3d')
ax2.set_title('after registration', fontsize = 12)
ax2.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax2.scatter(P_MRnT[:,0], P_MRnT[:,1], P_MRnT[:,2], c='b', marker='.')
plt.show()
'''
import time
import numpy as np
from math import sin, cos, pi
from scipy.linalg import norm
from fuzzypsreg import FuzzyPSReg
'''import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d'''
P_F = np.loadtxt('../data/fixedSet4.txt') # fixed point set P_F (N_PF x 3)
P_M = np.loadtxt('../data/movingSet4.txt') # moving point set P_M (N_PM x 3)
###### Initial pose setting for P_M
r0 = -pi + 2*pi*np.random.rand(3) # r0 = np.array([ ])
theta = norm(r0)
if ( theta == 0 ):
R0 = np.eye(3)
else:
M = 1.0/theta*np.array([[0, -r0[2], r0[1]], [r0[2], 0, -r0[0]], [-r0[1], r0[0], 0]])
R0 = np.eye(3) + M*sin(theta) + M.dot(M)*(1 - cos(theta))
P_M = P_M.dot(R0.T)
###### Parameter setting
noisyF, noisyM = 0, 0
trimRatio = 0.2
N_C = 80
gsF, gsM = 0.0018, 0.0018
gsF_fine, gsM_fine = 0.0044, 0.0044
RRange, TRange = pi, 0.5
sigma_r, sigma_t = 0.01, TRange/2.0
epsilon = 0
###### Fuzzy Cluster-based Point Set Registration
start = time.time()
f = FuzzyPSReg(RRange, TRange, sigma_r, sigma_t, epsilon)
R, t = f.psr(P_F, P_M, noisyF, noisyM, trimRatio, N_C, gsF, gsM, gsF_fine, gsM_fine)
end = time.time()
print('The time cost (sec) is: ')
print(end - start)
'''
###### Plot the point sets
P_MRnT = P_M.dot(R.T) + t
fig = plt.figure()
ax1 = fig.add_subplot(121, projection='3d')
ax1.set_title('initial poses', fontsize = 12)
ax1.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax1.scatter(P_M[:,0], P_M[:,1], P_M[:,2], c='b', marker='.')
ax2 = fig.add_subplot(122, projection='3d')
ax2.set_title('after registration', fontsize = 12)
ax2.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax2.scatter(P_MRnT[:,0], P_MRnT[:,1], P_MRnT[:,2], c='b', marker='.')
plt.show()
'''
import time
import numpy as np
from math import sin, cos, pi
from scipy.linalg import norm
from fuzzypsreg import FuzzyPSReg
'''import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d'''
P_F = np.loadtxt('../data/fixedSet5.txt') # fixed point set P_F (N_PF x 3)
P_M = np.loadtxt('../data/movingSet5.txt') # moving point set P_M (N_PM x 3)
'''
###### Initial pose setting for P_M
r0 = -pi + 2*pi*np.random.rand(3) # r0 = np.array([ ])
theta = norm(r0)
if ( theta == 0 ):
R0 = np.eye(3)
else:
M = 1.0/theta*np.array([[0, -r0[2], r0[1]], [r0[2], 0, -r0[0]], [-r0[1], r0[0], 0]])
R0 = np.eye(3) + M*sin(theta) + M.dot(M)*(1 - cos(theta))
P_M = P_M.dot(R0.T)
'''
###### Parameter setting
noisyF, noisyM = 1, 1
trimRatio = 0.2
N_C = 80
gsF, gsM = 0.0023, 0.0023
gsF_fine, gsM_fine = 0.0041, 0.0041
RRange, TRange = pi, 0.5
sigma_r, sigma_t = 0.01, TRange/4.0
epsilon = 0
###### Fuzzy Cluster-based Point Set Registration
start = time.time()
f = FuzzyPSReg(RRange, TRange, sigma_r, sigma_t, epsilon)
R, t = f.psr(P_F, P_M, noisyF, noisyM, trimRatio, N_C, gsF, gsM, gsF_fine, gsM_fine)
end = time.time()
print('The time cost (sec) is: ')
print(end - start)
'''
###### Plot the point sets
P_MRnT = P_M.dot(R.T) + t
fig = plt.figure()
ax1 = fig.add_subplot(121, projection='3d')
ax1.set_title('initial poses', fontsize = 12)
ax1.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax1.scatter(P_M[:,0], P_M[:,1], P_M[:,2], c='b', marker='.')
ax2 = fig.add_subplot(122, projection='3d')
ax2.set_title('after registration', fontsize = 12)
ax2.scatter(P_F[:,0], P_F[:,1], P_F[:,2], c='r', marker='.')
ax2.scatter(P_MRnT[:,0], P_MRnT[:,1], P_MRnT[:,2], c='b', marker='.')
plt.show()
'''
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment