def move_to_right(self): T, dist, _ = icp.icp(self.Real_P, self.Fake_P, max_iterations=20) #assert dist < 1e-2 self.Loc[0] += T[0][2] self.Loc[1] += T[1][2] #print(T[0][0], T[1][0], self.Loc) self.Argument = Calc_Arg([T[0][0], T[1][0] ]) / self.delt_arg % self.Max_Argument + 0.5 print( "The Precise Location of the Agent is (%.0f, %.0f) with Argument %d" % (self.Loc[0], self.Loc[1], self.Argument)) self.Loc[0] -= T[0][2] self.Loc[1] -= T[1][2] start = [] end = [] flag = False for i in range(dist.shape[0]): if dist[i] > 40: #if i == self.Max_Argument - 1: break self.hindrance[i] = self.Real_Radar[i] if flag == False: print(i) start.append(i) flag = True if dist[i] <= 40 and flag: end.append(i) flag = False #i+=1 start = np.array(start) end = np.array(end) for i in range(start.shape[0]): print("Possible human tracks on with Argument Range %d to %d" % (start[i], end[i])) #second_time_optimization with hindrance available for i in range(start.shape[0]): st = self.Real_Radar[start[i] - 1] ed = self.Real_Radar[end[i]] inv = end[i] - start[i] + 1 for j in range(start[i], end[i]): self.Real_Radar[j] = st + (ed - st) * (j - start[i] + 1) / inv print(self.Real_Radar[j - 1]) self.arg2ind() T, dist, _ = icp.icp(self.Real_P, self.Fake_P, max_iterations=40) #assert dist < 1e-2 self.Loc[0] += T[0][2] self.Loc[1] += T[1][2] #print(T[0][0], T[1][0], self.Loc) self.Argument = Calc_Arg([T[0][0], T[1][0] ]) / self.delt_arg % self.Max_Argument + 0.5 print( "The Precise Location of the Agent is (%.0f, %.0f) with Argument %d" % (self.Loc[0], self.Loc[1], self.Argument))
def get_linear_transform_matrix(ideal_pos, real_pos): """ To get the linear transformation matrix mapping the Bragg peaks in the data set to their theoretical positions, using the Iterative Closest Point method. """ src, dst = np.array(real_pos), np.array(ideal_pos) return icp(src, dst)[0]
def getRotMat(self): if self.tablepnt.shape[0] == 0: self.tablepnt = self.getTablePcd() T, distances = icp.icp(self.temppnt, self.tablepnt, max_iterations=20) return T, distances
def move_to_right(self): T, dist, _ = icp.icp(self.Real_P, self.Fake_P, max_iterations= 40) #assert dist < 1e-2 self.Loc[0] += T[0][2] self.Loc[1] += T[1][2] #print(T[0][0], T[1][0], self.Loc) self.Argument = Calc_Arg([T[0][0], T[1][0]]) / self.delt_arg % self.Max_Argument + 0.5 print("The Precise Location of the Agent is (%.0f, %.0f) with Argument %d" % (self.Loc[0], self.Loc[1], self.Argument)) start = [] end = [] flag = False for i in range(dist.shape[0]): while dist[i] > 40: if i == self.Max_Argument - 1: break self.hindrance[i] = self.Real_Radar[i] if flag == False: flag = True start.append(i) if dist[i + 1] <= 40: end.append(i + 1) flag = False break i+=1 print("Possible human tracks on with Argument Range %d to %d" % (start[0], end[0]))
def fit_from_candidates(node_array, pdb_array, max_iterations, req_tolerance, num_candidates): """ This program uses an ICP (iterative closest point) algorithm to get a transformation matrix that will align the FFEA structure to a PDB. However, this algorithm tends to get stuck in local minima. This function will randomly rotate the FFEA structure and run the algorithm, and output the rotation and the following translation for the result with the lowest RMSD. """ candidates = {} for candidate in range(num_candidates): print_progress_bar("Finding optimal alignment", num_candidates, candidate) temp_node_array = copy.copy(node_array) XYZ = [ random.random() * 2 * np.pi, random.random() * 2 * np.pi, random.random() * 2 * np.pi ] rot_euler(temp_node_array, XYZ) T, distances = icp.icp(temp_node_array, pdb_array, max_iterations=max_iterations, tolerance=req_tolerance) candidates[np.average(distances)] = [T, XYZ] return min(candidates.items())
def test(dist_thres, percent): # Reading some data scanList = datasets.read_u2is(56) scanOriginal = scanList[55] scanTruePose = np.array( [0.3620, 0.0143, 0.0483]) # Manual estimation for scan 55 of u2is dataset # Initialise error log nb_test = 10 poseError = np.zeros((3, nb_test)) time_start = time.process_time() for a in range(nb_test): idref = np.random.randint(50) refscan = scanList[idref] # Generate random displacement and applies it to the second scan randT = np.random.rand(2, 1) - 0.5 randR = 0.6 * np.random.rand(1, 1) - 0.3 R = np.array([[math.cos(randR), -math.sin(randR)], [math.sin(randR), math.cos(randR)]]) scan = datasets.transform_scan(scanOriginal, R, randT) # # Displays initial positions plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(refscan["x"], refscan["y"], "ob", label='Ref Scan') plt.plot(scan["x"], scan["y"], ".r", label='Scan before ICP') plt.axis("equal") # perform ICP R, t, error = icp.icp(refscan, scan, 200, 1e-7, dist_thres, percent) # Apply motion to scan scan = datasets.transform_scan(scan, R, t) poseError[:, a] = np.transpose(scan["pose"] - scanTruePose) # # Display plt.axis("equal") plt.plot(scan["x"], scan["y"], ".g", label='Scan after ICP') plt.legend() plt.pause(0.1) time_elapsed = time.process_time() - time_start tErrors = np.sqrt(np.square(poseError[0, :]) + np.square(poseError[1, :])) oErrors = np.sqrt(np.square(poseError[2, :])) print("Mean (var) translation error : {:e} ({:e})".format( np.mean(tErrors), np.var(tErrors))) print("Mean (var) rotation error : {:e} ({:e})".format( np.mean(oErrors), np.var(oErrors))) print("Mean computation time : {:f}".format(time_elapsed / nb_test)) print("Press Q in figure to finish...") plt.show() return np.mean(tErrors), np.var(tErrors), np.mean(oErrors), np.var( oErrors), time_elapsed / nb_test
def scale_transformation(car_points_3d,keypoints): #car_points_3d_center = [[0],[0],[0],[1]] #for # print(np.mean(car_points_3d)) # get the overlapping key points points_mesh = [] points_ours = [] for ind,points in enumerate(car_points_3d): if len(points) < 4 or ind == 8 or ind == 9: #print(keypoints[ind]) #print(points) ind = ind else: points_ours.append(points[0:3].reshape(1,3)) points_mesh.append(keypoints[ind].astype(np.float)) #print(points_ours) #print(points_mesh) # find the scale between the mesh and the triangulated points scale_sum = 0 coun = 0 scale =[0] * 100 for indices,points_3d in enumerate(points_ours): for indices_2,points_3d_2 in enumerate(points_ours): if indices_2 > indices and np.abs(np.mean(points_3d - points_3d_2))/np.abs(np.mean(points_mesh[indices] - points_mesh[indices_2])) < 5: scale[coun] = np.abs(np.mean(points_3d - points_3d_2))/np.abs(np.mean(points_mesh[indices] - points_mesh[indices_2])) coun += 1 # ransac compute transformation scale_all = scale[0:coun] ransac_max = 10000 scale_best = 1 RT_transform_best = np.zeros([4,4]) for i,scale_check in enumerate(scale_all): #scale_check = np.mean(scale_all) points_ours_icp = np.array([[0,0,0]] * len(points_ours)).astype(np.float) points_mesh_icp = np.array([[0,0,0]] * len(points_ours)).astype(np.float) for indices,points_3d in enumerate(points_ours): points_ours_icp[indices] = points_ours[indices] points_mesh_icp[indices] = points_mesh[indices]*scale_check try: RT_transform,distance = icp(points_mesh_icp,points_ours_icp, None, 200, 0.0001) icp_error = 0 for k,asa in enumerate(points_mesh_icp): icp_error += np.mean((np.dot(RT_transform,np.transpose(np.append(points_mesh_icp[k],1)))[0:3] - points_ours_icp[k])**2) if ransac_max > icp_error:#np.mean(distance) : #print(points_mesh_icp) #print(points_ours_icp) #print(icp_error) ransac_max = icp_error RT_transform_best = RT_transform scale_best = scale_check except: print('icp failed')#scale = print(points_3d) # print(np.dot(RT_transform_best,np.transpose(np.append(points_mesh_icp[k],1)))[0:3]) # print(points_ours_icp[k]) return scale_best,RT_transform_best
def test_plot_icp_cluster2cluster(laser_a, laser_b): # source: a. mst_a = EMST(laser_a) clusters_a = Cluster(mst_a, k=1.0, min_size=10) # destination: b. mst_b = EMST(laser_b) clusters_b = Cluster(mst_b, k=1.0, min_size=10) # icp dst_indices, T = icp(laser_a.cartesian, laser_b.cartesian, init_pose=None, max_iterations=20, tolerance=0.001) matched_cluster_indices, _ = match_clusters(dst_indices, laser_a, laser_b, clusters_a, clusters_b) # plot plt.figure() # colors to iterate over colors_a = ['r', 'y'] * 20 colors_b = ['b', 'g'] * 20 # plot source clusters and arrows to destination count = 1 for cluster, color, cluster_b_index in zip(clusters_a.clusters, colors_a, matched_cluster_indices): plt.scatter(laser_a.cartesian[cluster, [0]], laser_a.cartesian[cluster, [1]], s=10.0, c=color, linewidths=0) if cluster_b_index >= 0: # plot arrow points to the corresponding cluster x1 = np.mean(laser_a.cartesian[cluster, [0]]) y1 = np.mean(laser_a.cartesian[cluster, [1]]) cluster_b = clusters_b.clusters[cluster_b_index] x2 = np.mean(laser_b.cartesian[cluster_b, [0]]) y2 = np.mean(laser_b.cartesian[cluster_b, [1]]) dx = x2 - x1 dy = y2 - y1 plt.arrow(x1, y1, dx, dy, width=0.001, head_width=0.05) plt.text((x1 + x2) / 2.0, (y1 + y2) / 2.0, str(count), fontsize=18) count += 1 # plot destination clusters for cluster, color in zip(clusters_b.clusters, colors_b): plt.scatter(laser_b.cartesian[cluster, [0]], laser_b.cartesian[cluster, [1]], s=10.0, c=color, linewidths=0) plt.show()
def compare_maps(path1, path2): points1 = get_coordinates(path1) points2 = get_coordinates(path2) n_samples = int(min(len(points1), len(points2)) * 0.7) # use 70% of the smallest map for sampling sp1, sp2 = sample_arrays(np.asarray(points1), np.asarray(points2), n_samples) T, distances, iters = icp.icp(sp1, sp2) score = np.sqrt(np.mean(distances ** 2)) print("Map score: " + str(score) + " (took " + str(iters) + "iterations)")
def main(): # Each point is a row vector data = read_pc_from_file('images/depthImage1ForHW.txt') model = read_pc_from_file('images/depthImage2ForHW.txt') # Toy PCs for debugging # x = np.linspace(0, 2*np.pi, 100) # y = np.zeros(100) # z1 = np.sin(x) # z2 = np.sin(x) + 0.2 # data = np.vstack((x,y,z1)).transpose() # model = np.vstack((x,y,z2)).transpose() # Plot the two orginal point clouds fig = plt.figure() ax = fig.gca(projection='3d') ax.scatter(data[:, 0], data[:, 1], data[:, 2], c='b', marker='.', edgecolor='none', depthshade=False) ax.scatter(model[:, 0], model[:, 1], model[:, 2], c='r', marker='.', edgecolor='none', depthshade=False) ax.view_init(elev=16, azim=-52) # plt.savefig('./images/before.png') plt.show() aligned = icp(data, model, 20) fig = plt.figure() ax = fig.gca(projection='3d') ax.scatter(aligned[:, 0], aligned[:, 1], aligned[:, 2], c='b', marker='.', edgecolor='none', depthshade=False) ax.scatter(model[:, 0], model[:, 1], model[:, 2], c='r', marker='.', edgecolor='none', depthshade=False) ax.view_init(elev=16, azim=-52) # plt.savefig('./images/after.png') plt.show()
def test_icp(): # 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 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 np.random.shuffle(B) # Run ICP start = time.time() T, distances, iterations = icp.icp(B, A, None, num_tests, tolerance) total_time += time.time() - start # Make C a homogeneous representation of B C = np.ones((A.size, 2)) C[:, 0:1] = np.copy(B) # Transform C C = np.dot(T, C.T).T assert np.mean(distances) < 6 * noise_sigma # mean error should be small assert np.allclose(T[0:1, 0:1].T, R, atol=6 * noise_sigma) # T and R should be inverses assert np.allclose(-T[0:1, 1], t, atol=6 * noise_sigma) # T and t should be inverses #print('icp time: {:.3}'.format(total_time/num_tests)) print('T_ICP\n', T) print('R_ICP\n', R) return
def refine(pose, cloud_observed, cloud_model, p_distance=0.1, iterations=1): obj_T = pose.copy() cloud_estimated = np.dot(cloud_model, obj_T[:3, :3].T) + obj_T[:3, 3].T if cloud_estimated.shape[0] == 0 or cloud_observed.shape[0] == 0: return obj_T T = icp.icp(cloud_observed, cloud_estimated, iterations, p_distance) obj_T = T @ obj_T return obj_T
def test_icp(): # 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 # Shuffle to disrupt correspondence np.random.shuffle(B) # Run ICP start = time.time() T, distances, iterations, r, t2 = icp.icp(B, A, max_iterations=20, tolerance=0.000001) total_time += time.time() - start # Make C a homogeneous representation of B C = np.ones((N, dim + 1)) C[:, 0:dim] = np.copy(B) # Transform C C = np.dot(T, C.T).T print('icp time: {:.3}'.format(total_time / num_tests)) print(r) print('\n') print(t2) fig = plt.figure() ax = fig.add_subplot(111) ax.scatter([x[0] for x in A], [y[1] for y in A], c='red') ax.scatter([x[0] for x in B], [y[1] for y in B], c='blue') ax.scatter([x[0] for x in C], [y[1] for y in C], c='green') plt.show() return
def test_icp(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) #B[0:100] = 0 #Bcenter = rmsd.centroid(B) #B[0:100] = Bcenter print("Normal squared_distance", squared_distance(A, B)) plot3d(A, B) # Run ICP A -= rmsd.centroid(A) B -= rmsd.centroid(B) T, distances, iterations = icp.icp(B, A, tolerance=0.000001) print(T.shape) # Make C a homogeneous representation of B C = np.ones((N, 4)) C[:,0:3] = np.copy(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.mean(distances) < 6*noise_sigma # mean error should be small #assert np.allclose(T[0:3,0:3].T, R, atol=6*noise_sigma) # T and R should be inverses #assert np.allclose(-T[0:3,3], t, atol=6*noise_sigma) # T and t should be inverses return
def test_icp(): # 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 # Shuffle to disrupt correspondence np.random.shuffle(B) # Run ICP start = time.time() T, distances, iterations = icp.icp(B, A, tolerance=0.000001, use_colors=False) total_time += time.time() - start # Make C a homogeneous representation of B C = np.ones((N, 4)) C[:, 0:3] = np.copy(B) # Transform C C = np.dot(T, C.T).T assert np.mean( distances) < 6 * noise_sigma # mean error should be small assert np.allclose(T[0:3, 0:3].T, R, atol=6 * noise_sigma) # T and R should be inverses assert np.allclose(-T[0:3, 3], t, atol=6 * noise_sigma) # T and t should be inverses print('icp time: {:.3}'.format(total_time / num_tests)) print(T) return
def on_laser_data(data): global compass, last_laser_data, current_position points = [] for i, distance in enumerate(data.ranges): angle = data.angle_min + i * data.angle_increment dy = sin(angle) * distance dx = cos(angle) * distance points.append((dx, dy)) B = np.array(points) if last_laser_data is not None: T, _, _ = icp(B, last_laser_data, max_iterations=100, tolerance=0.000001) current_position = np.matmul(current_position, T) if MIMIC_COMPASS: stamp = data.header.stamp if len(compass) == 0: time.sleep( 0.001 ) #XXX: wait for position to arrive, is there a yield in python? _, theta = min(compass, key=lambda x: abs(cmp_stamp(x[0], stamp))) compass = [x for x in compass if cmp_stamp(x[0], stamp) > 0] current_position[0][0] = cos(theta) current_position[0][1] = -sin(theta) current_position[1][0] = sin(theta) current_position[1][1] = cos(theta) else: theta = atan2(current_position[1][0], current_position[0][0]) p = PoseStamped() p.header.frame_id = "pose" p.header.stamp = data.header.stamp p.pose.position.x = current_position[0][2] p.pose.position.y = current_position[1][2] p.pose.position.z = 0 p.pose.orientation.w = cos(theta / 2.0) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = sin(theta / 2.0) path.append(p) pub.publish(p) last_laser_data = B
def icp_show(self): b=np.array(self.icp_medal) a=np.array(self.icp_data) M2 = icp.icp(a, b, [0.1, 0.33, np.pi / 2.2], 30) # Plot the result src = np.array([a.T]).astype(np.float32) res = cv2.transform(src, M2) plt.figure() # plt.plot(b[0], b[1], 'g') # plt.plot(res[0].T[0], res[0].T[1], 'r.') # plt.plot(a[0], a[1], 'b') plt.scatter(b[0],b[1]) plt.scatter(res[0].T[0], res[0].T[1]) plt.scatter(a[0], a[1]) plt.show()
def test_plot_icp_point2point(a, b): ''' Deprecated Need to change icp code (last few lines) in order to work ''' # A = a.cartesian B = b.cartesian # ICP dst_indices, T, distances, iterations = icp(A, B, init_pose=None, max_iterations=20, tolerance=0.001) A_matched = A[src_indices, :] B_matched = B[dst_indices, :] # transform A according to T m = A.shape[1] A_homo = np.ones((m + 1, A.shape[0])) A_homo[:m, :] = np.copy(A.T) A_homo_after = np.dot(T, A_homo) A_after = A_homo_after[:m, :].T # print results print('Homogeneous Transformation Matrix: ' + str(T)) print('Number of iterations: %f' % iterations) # plot setup plt.figure() plt.title('Result of Iterative Closest Point') # plot scan plt.scatter(A[:, 0], A[:, 1], c='r', linewidths=0, s=300) plt.scatter(B[:, 0], B[:, 1], c='g', linewidths=0, s=300) plt.scatter(A_after[:, 0], A_after[:, 1], c='b', linewidths=0, s=300) plt.scatter(A_matched[:, 0], A_matched[:, 1], c='w', linewidths=0, s=100) plt.scatter(B_matched[:, 0], B_matched[:, 1], c='w', linewidths=0, s=100) # plot lines between matching points for a, b in zip(A_matched, B_matched): x = a[0] y = a[1] dx = b[0] - a[0] dy = b[1] - a[1] plt.arrow(x, y, dx, dy, width=0.0001, head_width=0.005) plt.show()
def single_line_fit(skyline_r, skyline_q): dim = 2 # number of dimensions of the points noise_sigma = 10 # standard deviation error to be added translation = 100 # max translation of the test set rotation = .1 # max rotation (radians) of the test set # image_base = np.zeros((4800, 640, 3), dtype=np.uint8) new_points = np.array(list(zip(range(len(skyline_q)), skyline_q)), dtype=np.float32) t = np.random.rand(dim) * translation R = rotation_matrix() new_points += t new_points = np.dot(R, new_points.T).T # linePoints(image_base, new_points, size=2, color=(0, 0, 255)) skyline = skyline_r length = len(skyline) ref_points = np.array(list(zip(range(length), skyline)), dtype=np.float32) # linePoints(image_base, ref_points, size=2, color=(0, 255, 0)) # cv2.imshow('image', image_base) # cv2.waitKey(0) fit_points = np.copy(new_points) for i in range(10): # image = np.copy(image_base) r, t, err, indices = icp.icp(fit_points, ref_points, max_iterations=1, tolerance=0.8, index=True) # for p1, p2 in zip(fit_points, ref_points[indices]): # cv2.line(image, (int(p1[0]), int(p1[1])), (int(p2[0]), int(p2[1])), (0, 0, 255), 1) # cv2.circle(image, (int(p1[0]), int(p1[1])), 1, (255, 0, 255), 1) # linePoints(image, fit_points, size=2, color=(255, 0, 255)) fit_points = np.dot(r, fit_points.T).T + t # cv2.putText(image, 'err = {}'.format(err), (20, 30), 2, cv2.FONT_HERSHEY_PLAIN, (255, 0, 0)) # cv2.imshow('image', image) # cv2.waitKey(max(int(err) * 5, 10)) if err < 0.8: break # print 'r = {}\nt = {}\nmean_err = {}'.format(r, t, err) # print 'Done.' # cv2.imshow('image', image) # cv2.waitKey(0) return err
def test_icp(): # Generate a random dataset A = np.random.rand(N, dim) print ("(#pts, dimen, num_test) = ({}, {}, {})".format(N, dim, num_tests)) print ("Noise added in tests: ") print ("(sigma, translation, rotation) = ({}, {}, {})".format(noise_sigma, translation, rotation)) 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 # Shuffle to disrupt correspondence np.random.shuffle(B) # Run ICP start = time.time() T, distances, iterations = icp.icp(B, A, tolerance=0.000001) total_time += time.time() - start # Make C a homogeneous representation of B C = np.ones((N, 4)) C[:,0:3] = np.copy(B) # Transform C C = np.dot(T, C.T).T if i % 10 == 0: print ("result round {} mean error: {}".format(i, np.mean(distances))) assert np.mean(distances) < 6*noise_sigma # mean error should be small assert np.allclose(T[0:3,0:3].T, R, atol=6*noise_sigma) # T and R should be inverses assert np.allclose(-T[0:3,3], t, atol=6*noise_sigma) # T and t should be inverses print('icp time: {:.3}'.format(total_time/num_tests)) return
def label(templates: np.array, samples: np.array) -> np.array: """Use ICP to classify all samples.""" labels = None for sample in samples: results = [icp(sample, template) for template in templates] distances = [np.sum(distance) for _, _, distance in results] i = int(np.argmin(distances)) T, s, _ = results[i] # T (4x4 homogenous transformation) label = np.hstack( (i, distances[i], np.ravel(T), s, distances)).reshape((1, -1)) print(' * [Info] Final distance:', distances[i], 'Label:', template_index_to_name[i]) labels = label if labels is None else np.vstack((labels, label)) if labels is None: raise UserWarning('No samples found.') labels[:, 1] = 1 - (labels[:, 1] / np.max(labels[:, 1])) return labels
def test_match(map_name, image_name, tolerance): im_model_raw = cv2.imread(map_name, cv2.IMREAD_GRAYSCALE) _, im_model = cv2.threshold( im_model_raw, 254, 255, cv2.THRESH_BINARY) im_raw = cv2.imread(image_name, cv2.IMREAD_GRAYSCALE) _, im = cv2.threshold(im_raw, 1, 255, cv2.THRESH_BINARY) points_model = read_image(im_model) points = read_image(im) np.random.shuffle(points_model) extension = points.shape[0] - points_model.shape[0] points_model_upsampled = np.append( points_model, points_model[0:extension, :], axis=0) T, distances, iterations = icp.icp( points, points_model_upsampled, tolerance=tolerance) # Make C a homogeneous representation of B points_transformed = np.ones((points.shape[0], 4)) points_transformed[:, 0:3] = points # Transform C points_transformed = np.dot(T, points_transformed.T).T print('Transformation Matrix:', T) print('Point distances:', distances) print('Pixel squared error:', np.average(distances ** 2)) print('Number of iterations:', iterations) plt.plot(points_model[:, 0], points_model[:, 1], 'ro', points_transformed[:, 0], points_transformed[:, 1], 'bo') plt.axis('equal') plt.show()
def main_icp(A, B): # takes point arrays A & B # Run ICP # start = time.time() T, distances, iterations = icp.icp(B, A, tolerance=0.11) # total_time = time.time() - start # Make C a homogeneous representation of B C = np.ones((len(B), 4)) C[:, 0:3] = np.copy(B) print T sf = 6.125 # timescale factor S = np.array([[1.0, sf, sf, sf], [sf, 1.0, sf, sf], [sf, sf, 1.0, sf], [0.0, 0.0, 0.0, 1.0]]) # print S # print S*T # print T*S # Transform C C = np.dot(T, C.T).T # C = np.dot((T*S), C.T).T # Test results noise_sigma = 0.1 # assert np.mean(distances) < 6*noise_sigma # mean error should be small print "mean distances:" print np.mean(distances) # assert np.allclose(C[:,[0,1,2]], B, atol=6*noise_sigma) print "number of iterations:" print iterations return list(C[:, [0, 1, 2]])
def test_icp(): A = np.random.rand(N, dim) total_time = 0 for i in range(num_tests): B = np.copy(A) #Applying Random transformation # 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 np.random.shuffle(B) # Run ICP start = time.time() T,_,_ = icp.icp(B, A, tolerance=0.000001) 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 print('icp fit time: {:.3}'.format(total_time/num_tests))
def callback(data): global cluster_pointcloud, dock_pose, T, find_dock angle_min = data.angle_min angle_max = data.angle_max angle_increment = data.angle_increment # rospy.loginfo("scan type, min angle: %f, max_angle: %f, increment: %f" % (angle_min, angle_max, angle_increment)) # rospy.loginfo("get scan data: %d points" % (len(data.ranges))) last_range = 0 last_point = Point32() cluster_index = 0 clouds = [] potential_clouds = [] for i in xrange(len(data.ranges)): if (angle_min + i * angle_increment) >= min_detect_angle and ( angle_min + i * angle_increment) <= max_detect_angle: point = Point32() point.x = math.cos(angle_min + i * angle_increment) * data.ranges[i] point.y = math.sin(angle_min + i * angle_increment) * data.ranges[i] point.z = 0 distance = math.sqrt( math.pow(point.x - last_point.x, 2) + math.pow(point.y - last_point.y, 2)) if (distance > cluster_threshold): if (len(clouds) > potential_clouds_min_points): potential_clouds.append(clouds) clouds = [] clouds.append(point) last_point = point last_range = data.ranges[i] # add last cloud into potential_clouds # filter cloud with points count if (len(clouds) > potential_clouds_min_points): potential_clouds.append(clouds) if (len(potential_clouds) <= 0): find_dock = False # rospy.logwarn("can not find dock") return best_p_i = -1 best_distance = icp_distance_threshold best_T = None for p_i in xrange(len(potential_clouds)): ideal_dock_cloud = GenerateIdealDock(1000) ideal_dock_np = Point32ToNumpy(ideal_dock_cloud) potential_cloud_np = Point32ToNumpy(potential_clouds[p_i]) T, distance, i = icp.icp(ideal_dock_np, potential_cloud_np) mean_distance = np.mean(distance) if mean_distance < icp_distance_threshold: if (mean_distance < best_distance): best_p_i = p_i best_distance = mean_distance best_T = T if best_p_i >= 0: qt = tf.transformations.quaternion_from_matrix(best_T) find_dock = True dock_pose = PoseStamped() dock_pose.header.frame_id = "laser_link" dock_pose.header.stamp = rospy.Time.now() dock_pose.pose.position.x = best_T[0][3] dock_pose.pose.position.y = best_T[1][3] dock_pose.pose.position.z = best_T[2][3] dock_pose.pose.orientation.x = qt[0] dock_pose.pose.orientation.y = qt[1] dock_pose.pose.orientation.z = qt[2] dock_pose.pose.orientation.w = qt[3] else: find_dock = False # rospy.loginfo("icp distance %f" % np.mean(distance)) # rospy.loginfo(T) cluster_pointcloud = PointCloud() cluster_pointcloud.header = data.header cluster_pointcloud.header.stamp = rospy.Time.now() color_r = ChannelFloat32('rgb', []) color_map = [0x00ff00, 0xffff00, 0xff00ff, 0x00ffff] for i in xrange(len(potential_clouds)): for j in xrange(len(potential_clouds[i])): cluster_pointcloud.points.append(potential_clouds[i][j]) color_r.values.append(color_map[i % 4]) cluster_pointcloud.channels.append(color_r)
pc_ref = np.zeros((len(ref_pts),3)) pc_cur = np.zeros((len(cur_pts),3)) for id in range(len(ref_pts)): # z component pc_ref[id, 2] = ref_depth[int(ref_pts[id, 1]), int(ref_pts[id, 0])]*depth_scale pc_cur[id, 2] = cur_depth[int(cur_pts[id, 1]), int(cur_pts[id, 0])]*depth_scale # x component pc_ref[id, 0] = (ref_pts[id, 0]-cx)*(pc_ref[id, 2]/fx) pc_cur[id, 0] = (cur_pts[id, 0]-cx)*(pc_cur[id, 2]/fx) # y component pc_ref[id, 1] = (ref_pts[id, 1]-cy)*(pc_ref[id, 2]/fy) pc_cur[id, 1] = (cur_pts[id, 1]-cy)*(pc_cur[id, 2]/fy) # ICP T, distances, iterations = icp.icp(pc_cur, pc_ref, tolerance=0.000001) ROT = T[0:3, 0:3] TR = np.array([[T[0,3]],[T[1,3]],[T[2,3]]]) # trajectory calculation Rot_pose = np.dot(ROT, Rot_pose) Tr_pose = Tr_pose + np.dot(Rot_pose, TR) # update ref_frame = cur_frame ref_depth = cur_depth kp_ref = kp_cur d_ref = d_cur if cv2.waitKey(1) == 27: break
tplRot = cv2.warpPerspective(tpl, M, (w,h)) tplKeyRot = cv2.warpPerspective(tplKey, M, (w,h)) tplRot2 = cv2.cvtColor(tplRot, cv2.COLOR_GRAY2BGR) dst = cv2.addWeighted(tplRot2, 0.4, frame,0.6, 0) score = np.sum(np.logical_and(blank, tplRot)) cv2.putText(dst, str(score), (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2) pos = (0,0,0) if matched is not None: #dilate = cv2.dilate(matched,kernel,iterations = 2) #mask = 255*np.logical_and(blank, dilate).astype(np.uint8) #cv2.imshow('detected', mask) #import pdb; pdb.set_trace() src = np.transpose(np.nonzero(matched)).T tgt = np.transpose(np.nonzero(blank)).T tr, pos = icp(src, tgt, init_pose=pos, no_iterations=30) M = np.dot(tr, M) cv2.imshow('detected', cv2.addWeighted(tplRot, 0.6, blank, 0.4, 0)) #cv2.imshow('detected', tplKeyRot) k = cv2.waitKey(50) & 0xFF if k == 27: break #matched = tplKeyRot matched = 255*np.logical_and(blank, tplRot).astype(np.uint8) cap.release() #out.release() cv2.destroyAllWindows()
cor = np.concatenate((cor, ST), axis=0) cO = camOrds cT = camT testT = np.reshape([], (0, 3)) testO = np.reshape([], (0, 3, 3)) Sprev = S preDes = des triag = triangulate(ST) else: bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) match = bf.match(preDes[:, -(window - shift) + 2, :], des[:, 0, :]) idx = np.reshape(map(lambda x: [x.queryIdx, x.trainIdx], match), (len(match), 2)) preIdx = idx[:, 0] curIdx = idx[:, 1] K, t = icp.icp(S[:, curIdx], Sprev[:, preIdx]) S = np.dot(K, S) + t camT = (np.dot(K, camT.T) + t).T camOrds = np.reshape( map(lambda x: normalize(np.dot(K, x.T).T), camOrds), camOrds.shape) ST = S.T cO = np.concatenate((cO, camOrds), axis=0) cT = np.concatenate((cT, camT), axis=0) cor = np.concatenate((cor, ST), axis=0) Sprev = S preDes = des tr = triangulate(ST) triag = np.concatenate((triag, tr), axis=0) q += 1 k += shift xx, yy, zz = getS(ST)
my_r_unrefined = quaternion_matrix(my_r)[:3, :3] my_t_unrefined = my_t + 0 emb = last_emb ICP = False if ICP: #Refinement new_cloud = torch.bmm((points - T), R).contiguous() # dummy = points.squeeze().detach().cpu().numpy() init_cloud = new_cloud.squeeze().detach().cpu().numpy().copy() original_cloud = model_points[0].cpu().detach().numpy().copy() # my_r = quaternion_matrix(my_r)[:3, :3] # original_cloud_shifted = np.dot(original_cloud, my_r.T) + my_t # from IPython import embed; embed() delta_T, distances, iterations = icp.icp(original_cloud, init_cloud, max_iterations=20000, tolerance=0.000001) t_itr.append(iterations) # pcd_src = o3d.geometry.PointCloud() # pcd_target = o3d.geometry.PointCloud() # pcd_src.points = o3d.utility.Vector3dVector(init_cloud) # pcd_target.points = o3d.utility.Vector3dVector(original_cloud) # t_itr.append(0) # reg_p2p = o3d.pipelines.registration.registration_icp(pcd_target, pcd_src, 0.2, np.eye(4),o3d.pipelines.registration.TransformationEstimationPointToPoint(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration = 20000, relative_rmse = 1.0e-10, relative_fitness=1.000000e-10)) # delta_T = reg_p2p.transformation my_mat_final = np.dot(my_mat, delta_T) my_r_final = copy.deepcopy(my_mat_final) my_r_final[0:3, 3] = 0 my_r_final = quaternion_from_matrix(my_r_final, True) my_t_final = np.array(
depth = depth[::down_factor, ::down_factor] vertex_map = transforms.unproject(depth, intrinsic) color_map = np.asarray(o3d.io.read_image('{}/{}.png'.format(rgb_path, i))).astype(float) / 255.0 color_map = color_map[::down_factor, ::down_factor] normal_map = np.load('{}/{}.npy'.format(normal_path, i)) normal_map = normal_map[::down_factor, ::down_factor] if i > 1: print('Frame-to-model icp') T_world_to_cam = np.linalg.inv(T_cam_to_world) T_world_to_cam = icp(m.points[::down_factor], m.normals[::down_factor], vertex_map, normal_map, intrinsic, T_world_to_cam, debug_association=False) T_cam_to_world = np.linalg.inv(T_world_to_cam) print('Point-based fusion') m.fuse(vertex_map, normal_map, color_map, intrinsic, T_cam_to_world, time=i) # A shift is required as gt starts from 1 T_gt.append(gt_poses[i - 1]) T_est.append(T_cam_to_world) global_pcd = o3d_utility.make_point_cloud(m.points, colors=m.colors, normals=m.normals) o3d.visualization.draw_geometries([global_pcd.transform(o3d_utility.flip_transform)]) # Visualize the trajectories
def realtime_test(): ''' Internal test. ''' # laser sub laser = LaserSub() # init loop r = rospy.Rate(1.0) while not rospy.is_shutdown( ): # wait until first scan is received so that laser is initialized if laser.first_recieved_done: # laser specs initialized by ROS sensor_msgs/LaserScan message break else: rospy.loginfo( '[coarse_level_association] Waiting for laser to init.') r.sleep() laser_prev = None clusters_prev = None clusters_id = None # init plot fig, ax = plt.subplots() ax.set_aspect('equal', adjustable='box') ax.set_title('Clusters Tracking', fontsize=18) ax.axis([-5.0, 5.0, -5.0, 5.0]) ax.plot([0], [0], marker='>', markersize=20, color="red") # plot the origin # loop r = rospy.Rate(30) while not rospy.is_shutdown(): # --- 1. laser input (copy self.laser at this moment) laser_now = copy.deepcopy(laser) # --- 2. EMST (create input graph for segmentation) mst = EMST(laser_now) # --- 3. EGBIS (apply image segmentation technique to clustering) clusters_now = Cluster(mst, k=1.0, min_size=10) if clusters_id is None: # first set of clusters generated clusters_id = [i for i in range(len(clusters_now.clusters))] # --- 4. ICP if laser_prev is not None: dst_indices, T = icp(laser_now.cartesian, laser_prev.cartesian, init_pose=None, max_iterations=20, tolerance=0.001) matched_cluster_indices, _ = match_clusters( dst_indices, laser_now, laser_prev, clusters_now, clusters_prev) # --- 5. Visualize laser_a = laser_now laser_b = laser_prev clusters_a = clusters_now clusters_b = clusters_prev clusters_id_new = [] # colors to iterate over colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k'] * 20 # plot source clusters with the same id as the matched destination clusters ax_list = [] for cluster, color, cluster_b_index in zip( clusters_a.clusters, colors, matched_cluster_indices): a = ax.scatter(laser_a.cartesian[cluster, [0]], laser_a.cartesian[cluster, [1]], s=20.0, c=color, linewidths=0) ax_list.append(a) if cluster_b_index >= 0: # show id x1 = np.mean(laser_a.cartesian[cluster, [0]]) y1 = np.mean(laser_a.cartesian[cluster, [1]]) cluster_b = clusters_b.clusters[cluster_b_index] x2 = np.mean(laser_b.cartesian[cluster_b, [0]]) y2 = np.mean(laser_b.cartesian[cluster_b, [1]]) _id = clusters_id[cluster_b_index] clusters_id_new.append(_id) a = ax.text((x1 + x2) / 2.0, (y1 + y2) / 2.0, str(_id), fontsize=22) ax_list.append(a) else: # no cluster_b match me. _id = max(clusters_id) + 1 clusters_id_new.append(_id) clusters_id = clusters_id_new # plot plt.pause(1e-12) # pause for real time display for a in ax_list: # clear data on the plot a.remove() # end of loop laser_prev = laser_now clusters_prev = clusters_now r.sleep() # plot plt.show()