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
# 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) # Apply motion to scan
print('Processing new scan') # get list of map scan sorted by distance sorteddist, sortedId = datasets.find_closest_scan(map, scanList[i]) # Keep only the ones below the distance threshold, or the closest one closeScans = sortedId[sorteddist < distThresholdMatch] if len(closeScans) == 0: closeScans = [sortedId[0]] # perform ICP with closest scan to correct future odometry R, t, error = icp.icp(map[closeScans[0]], scanList[i], 200, 1e-7) # Correct all future scans odometry pose for j in range(i, maxScan, step): scanList[j] = datasets.transform_scan(scanList[j], R, t) # --- Add scan to map and update graph if needed if np.linalg.norm(scanList[i]["pose"][0:2] - map[closeScans[0]]["pose"][0:2]) > distThresholdAdd: map.append(scanList[i]) print('Adding new scan with links to : ' + str(closeScans)) # Get ref to last scan in map (i.e. new scan) id2 = len(map) - 1 s2 = map[-1] # ---- Build graph edgeNB=0 for idi in closeScans:
scanList[minScan]["pose"][1], color=c, s=3) ax2.axis([-5.5, 12.5, -12.5, 6.5]) ax2.set_title('Pose after ICP correction') plt.pause(0.1) for a in range(minScan, maxScan, step): s1 = scanList[a] s2 = scanList[a + step] # perform ICP R, t, error = icp.icp(s1, s2, 200, 1e-7, 0.4, 0.85) # correct future scans for b in range((a + step), maxScan, step): scanList[b] = datasets.transform_scan(scanList[b], R, t) # Display c = np.random.rand(3, ) ax1.scatter(odomScanList[a + step]["x"], odomScanList[a + step]["y"], color=c, s=1) ax1.scatter(odomScanList[a + step]["pose"][0], odomScanList[a + step]["pose"][1], color=c, s=3) ax2.scatter(scanList[a + step]["x"], scanList[a + step]["y"], color=c, s=1) ax2.scatter(scanList[a + step]["pose"][0], scanList[a + step]["pose"][1], color=c,