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)