def test_best_fit():
# Generate a random dataset
#A = np.random.rand(N, dim)
A = np.array([])
# Generate a random dataset
plydata = PlyData.read('bun045.ply')
# Generate a random dataset
for j in range(0, plydata.elements[0].count):
A = np.append(A, plydata['vertex'][j][0])
A = np.reshape(A, (plydata.elements[0].count, 1))
total_time = 0
for i in range(num_tests):
B = np.copy(A)
# Translate
t = np.random.rand(dim) * translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
B = np.dot(R, B.T).T
# Add noise
#B += np.random.randn(N, dim) * noise_sigma
# Find best fit transform
start = time.time()
T, R1, t1 = icp.best_fit_transform(B, A)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((A.size, 2))
C[:, 0:1] = B
# Transform C
C = np.dot(T, C.T).T
assert np.allclose(C[:, 0:1], A, atol=6 *
noise_sigma) # T should transform B (or C) to A
assert np.allclose(-t1, t,
atol=6 * noise_sigma) # t and t1 should be inverses
assert np.allclose(R1.T, R,
atol=6 * noise_sigma) # R and R1 should be inverses
#print('best fit time: {:.3}'.format(total_time/num_tests))
print('T_Bestfit\n', T)
print('R_Bestfit\n', R)
return
def test_icp(B, A):
blen = B.shape[0]
alen = A.shape[0]
if alen > blen:
A = A[0:blen, :]
elif blen > alen:
B = B[0:alen, :]
print(B.shape)
print(A.shape)
T, R1, t1 = icp.best_fit_transform(B, A)
return T
def test_best_fit(A, shuffle = 1):
# Generate a random dataset
for i in range(num_tests):
print('test', i)
B = np.copy(A)
# Translate
t = np.random.rand(dim)*translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand()*rotation)
B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
if (shuffle == 1):
np.random.shuffle(B)
print("Normal squared_distance", squared_distance(A, B))
plot3d(A, B)
# Find best fit transform
A -= rmsd.centroid(A)
B -= rmsd.centroid(B)
T, R1, t1 = icp.best_fit_transform(B, A)
# Make C a homogeneous representation of B
C = np.ones((N, 4))
C[:,0:3] = B
# Transform C
C = np.dot(T, C.T).T
C = C[:, 0:3]
print("Rotated squared_distance", squared_distance(A, C))
plot3d(A, C)
#assert np.allclose(C[:,0:3], A, atol=6*noise_sigma) # T should transform B (or C) to A
#assert np.allclose(-t1, t, atol=6*noise_sigma) # t and t1 should be inverses
#assert np.allclose(R1.T, R, atol=6*noise_sigma) # R and R1 should be inverses
return
def __update_of(self):
if not self.of_enabled:
return
# Calculate optical flow
new_gray = cv2.cvtColor(realsensecam().bgr, cv2.COLOR_BGR2GRAY)
new_pts, st, err = cv2.calcOpticalFlowPyrLK(self.old_gray, new_gray,
self.of_old_pts, None,
**self.of_lk_params)
if new_pts is None:
self.__stop_of_tracking()
return
# Mark points that are not on the hand as invalid
for i, ptt in enumerate(new_pts):
if not handdetector().cnt_intersects_with_hand(
np.array([ptt]).astype(int)):
st[i][0] = 0
# Delete lost points
self.of_orig_existing_pts = self.of_orig_existing_pts[st == 1]
self.of_old_pts = self.of_old_pts[st == 1]
self.of_is_fingertip = self.of_is_fingertip[st.flatten() == 1]
new_pts = new_pts[st == 1]
if len(new_pts) == 0:
self.__stop_of_tracking()
return
old_finger_delta = self.finger_delta
old_finger_deg_delta = self.finger_deg_delta
T, R, t = icp.best_fit_transform(self.of_orig_existing_pts, new_pts)
self.finger_delta = tuple(t)
self.finger_deg_delta = np.rad2deg(np.arctan2(R[1, 0], R[0, 0]))
self.finger_transform = T
dt = np.linalg.norm(np.array(t) - np.array(old_finger_delta))
dr = abs(old_finger_deg_delta - self.finger_deg_delta)
# Prepare next iteration
self.old_gray = new_gray.copy()
self.of_old_pts = new_pts.reshape(-1, 1, 2)
self.of_orig_existing_pts = self.of_orig_existing_pts.reshape(-1, 1, 2)
return dt > 1 or dr > .3
def test_best_fit():
# Generate a random dataset
A = np.random.rand(N, dim)
total_time = 0
for i in range(num_tests):
B = np.copy(A)
# Translate
t = np.random.rand(dim) * translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Find best fit transform
start = time.time()
T, R1, t1 = icp.best_fit_transform(B, A)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((N, 4))
C[:, 0:3] = B
# Transform C
C = np.dot(T, C.T).T
assert np.allclose(C[:, 0:3], A, atol=6 *
noise_sigma) # T should transform B (or C) to A
assert np.allclose(-t1, t,
atol=6 * noise_sigma) # t and t1 should be inverses
assert np.allclose(R1.T, R,
atol=6 * noise_sigma) # R and R1 should be inverses
print('best fit time: {:.3}'.format(total_time / num_tests))
return
def test(filename):
# read file
f = open(filename, "r")
fileRaw = np.fromfile(f, dtype=np.double)
# copy without header
fileUsable = fileRaw[6:]
N = int(len(fileUsable)/7)
# reshape to array with one pair each line
fileUsable = fileUsable.reshape(N,7)
# sort by distance and cut 10% from start and end (discard outliers)
cut = int(N*0.1)
fileUsable = fileUsable[fileUsable[:,6].argsort()]
fileUsable = fileUsable[cut:-cut]
N = N - 2*cut
# slice to separate vectors
A = fileUsable[:, :3]
B = fileUsable[:, 3:6]
# create matrix panda->lmd
toLmd = np.array([ [0.999199, 0.0, 0.040010, 25.378128],
[0.0, 1.0, 0.0, 0.0],
[-0.040010, 0.0, 0.999199, 1109.130000],
[0.0, 0.0, 0.0, 1.0]])
# create matrix panda->module
toMod = np.array([ [0.999199, 0.0, 0.040010, 24.902014],
[0.0, 1.0, 0.0, 0.0],
[-0.040010, 0.0, 0.999199, 1097.239528],
[0.0, 0.0, 0.0, 1.0]])
# create matrix panda->sensor0
toMod = np.array([ [0.999199, 0.0, 0.040010, 24.902014],
[0.0, 1.0, 0.0, 0.0],
[-0.040010, 0.0, 0.999199, 1097.239528],
[0.0, 0.0, 0.0, 1.0]])
# create matrix panda->sensor1
toMod = np.array([ [0.999199, 0.0, 0.040010, 24.902014],
[0.0, 1.0, 0.0, 0.0],
[-0.040010, 0.0, 0.999199, 1097.239528],
[0.0, 0.0, 0.0, 1.0]])
# and inverse
toLmdInv = np.linalg.inv(toLmd)
toModInv = np.linalg.inv(toMod)
# Make C a homogeneous representation of A and B
C = np.ones((N, 4))
C[:,0:3] = A
D = np.ones((N, 4))
D[:,0:3] = B
# Transform C and D
C = np.matmul(toModInv, C.T).T
D = np.matmul(toModInv, D.T).T
# make 2D versions for ICP
A = C[:, :2]
B = D[:, :2]
# find ideal transformation
T, R1, t1 = icp.best_fit_transform(B, A)
print('\nfor', filename, ':')
print('rotation:\n', R1)
print('translation:\n', t1)
def test(filename):
# read file
f = open(filename, "r")
fileRaw = np.fromfile(f, dtype=np.double)
# copy without header
fileUsable = fileRaw[6:]
N = int(len(fileUsable)/7)
# reshape to array with one pair each line
fileUsable = fileUsable.reshape(N,7)
# sort by distance and cut 10% from start and end (discard outliers)
cut = int(N*0.05)
fileUsable = fileUsable[fileUsable[:,6].argsort()]
fileUsable = fileUsable[cut:-cut]
N = N - 2*cut
# slice to separate vectors
A = fileUsable[:, :3]
B = fileUsable[:, 3:6]
#print('vector A:\n', A)
#print('vector B:\n', B)
# create matrix panda->sensor0
toSen0 = np.array([ [0.999199, 0.0, 0.040010, 29.397911],
[0.0, 1.000000, 0.000000, 1.0],
[-0.040010, 0.0, 0.999199, 1097.657930],
[0.0, 0.0, 0.0, 1.0]])
# create matrix panda->sensor10
toSen0 = np.array([ [0.808369, -0.587315, 0.040010, 27.971866],
[0.587785, 0.809017, 0.000000, 3.454051],
[-0.032368, 0.023517, 0.999199, 1097.604497],
[0.0, 0.0, 0.0, 1.0]])
# create matrix panda->sensor5
toSen5 = np.array([ [0.808369, 0.587315, -0.040010, 29.127490],
[ 0.587785, -0.809017, -0.000000, 1.836017],
[-0.032368, -0.023517, -0.999199, 1097.082843],
[0.0, 0.0, 0.0, 1.0]])
# and inverse
toSen0Inv = np.linalg.inv(toSen0)
toSen5Inv = np.linalg.inv(toSen5)
# Make C a homogeneous representation of A and B
C = np.ones((N, 4))
C[:,0:3] = A
D = np.ones((N, 4))
D[:,0:3] = B
# Transform C and D
C = np.matmul(toSen0Inv, C.T).T
D = np.matmul(toSen0Inv, D.T).T
#print('vec C:\n', C)
#print('vec D:\n', D)
# make 2D versions for ICP
A = C[:, :3]
B = D[:, :3]
# find ideal transformation
T, R1, t1 = icp.best_fit_transform(B, A)
print('\nfor', filename, ':')
print('rotation:\n', R1)
print('translation:\n', t1)
