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,