icp.py

"""
Iterative Closest Point (ICP) SLAM example
author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
"""

import math

import matplotlib.pyplot as plt
import numpy as np
from test_data import TestData

#  ICP parameters
EPS = 0.0001
MAX_ITER = 100

show_animation = True


def icp_matching(previous_points, current_points):
    """
    Iterative Closest Point matching
    - input
    previous_points: 2D points in the previous frame
    current_points: 2D points in the current frame
    - output
    R: Rotation matrix
    T: Translation vector
    """
    H = None  # homogeneous transformation matrix

    dError = 1000.0
    preError = 1000.0
    count = 0

    while dError >= EPS:
        count += 1

        if show_animation:  # pragma: no cover
            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(previous_points[0, :], previous_points[1, :], ".r")
            plt.plot(previous_points[0, :], previous_points[1, :], color=(1.0, 0.37, 0.22, 1.0), marker=".", linestyle='')
            plt.plot(current_points[0, :], current_points[1, :], color=(0.15, 0.65, 0.65, 1.0), marker=".", linestyle='')
            plt.plot(0.0, 0.0, "xr")
            plt.axis("equal")
            plt.pause(0.05)

        indexes, error = nearest_neighbor_association(previous_points, current_points)
        Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)

        # update current points
        current_points = (Rt @ current_points) + Tt[:, np.newaxis]

        H = update_homogeneous_matrix(H, Rt, Tt)

        dError = abs(preError - error)
        preError = error
        print("Residual:", error)

        if dError <= EPS:
            print("Converge", error, dError, count)
            break
        elif MAX_ITER <= count:
            print("Not Converge...", error, dError, count)
            break

    R = np.array(H[0:2, 0:2])
    T = np.array(H[0:2, 2])

    return R, T


def update_homogeneous_matrix(Hin, R, T):

    H = np.zeros((3, 3))

    H[0, 0] = R[0, 0]
    H[1, 0] = R[1, 0]
    H[0, 1] = R[0, 1]
    H[1, 1] = R[1, 1]
    H[2, 2] = 1.0

    H[0, 2] = T[0]
    H[1, 2] = T[1]

    if Hin is None:
        return H
    else:
        return Hin @ H


def nearest_neighbor_association(previous_points, current_points):

    # calc the sum of residual errors
    delta_points = previous_points - current_points
    d = np.linalg.norm(delta_points, axis=0)
    error = sum(d)

    # calc index with nearest neighbor assosiation
    d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1)
                       - np.tile(previous_points, (1, current_points.shape[1])), axis=0)
    indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1)

    return indexes, error


def svd_motion_estimation(previous_points, current_points):
    pm = np.mean(previous_points, axis=1)
    cm = np.mean(current_points, axis=1)

    p_shift = previous_points - pm[:, np.newaxis]
    c_shift = current_points - cm[:, np.newaxis]

    W = c_shift @ p_shift.T
    u, s, vh = np.linalg.svd(W)

    R = (u @ vh).T
    t = pm - (R @ cm)

    return R, t


def main():
    print(__file__ + " start!!")

    # simulation parameters
    nPoint = 1000
    fieldLength = 50.0
    motion = [0.5, 2.0, np.deg2rad(-10.0)]  # movement [x[m],y[m],yaw[deg]]

    nsim = 3  # number of simulation

    for _ in range(nsim):

        # previous points
        td = TestData()
        p = td.nominal_input.sweep
        px = p[:,0]
        py = p[:,1]
        # px = (np.random.rand(nPoint) - 0.5) * fieldLength
        # py = (np.random.rand(nPoint) - 0.5) * fieldLength
        previous_points = np.vstack((px, py))

        input_data = td.top_wall_input1
        c = input_data.sweep
        cx = c[:,0]
        cy = c[:,1]
        # current points
        # cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
        #       for (x, y) in zip(px, py)]
        # cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
        #       for (x, y) in zip(px, py)]
        current_points = np.vstack((cx, cy))

        R, T = icp_matching(previous_points, current_points)
        print("Calculated R:", R)
        print("Calculated T:", T)
        print("Real R: ", [[math.sin(input_data.theta), math.cos(input_data.theta)], [-math.cos(input_data.theta), math.sin(input_data.theta)]])
        print("Real T: ", [input_data.y, -input_data.x])


if __name__ == '__main__':
    main()