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
Exemple #2
0
# 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
Exemple #3
0
    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,