<jupyter_start><jupyter_text>Iterative Closest Point algorithm Problem statementA key step in SLAM algorithms is to find the transform between two sets of observations $X_i$ and $X_j$, acquired at two different time instants $t_i$ and $t_j$, $t_j>t_i$. It is assumed that the environment of the robot is static. Hence, if a rigid transformation can be found that transforms the observation $X_j$ so that the observation correspond to $X_i$, then that rigid transform describes the rigid transform of the robot between the two times, from its pose at $t_i$ to its pose at $t_j$. In the rest of this notebook we are following the notation of the original publication on the ICP algorithm, [<NAME> (1992), A Method for Registration of 3_D Shapes IEEE Tr PAMI.](https://graphics.stanford.edu/courses/cs164-09-spring/Handouts/paper_icp.pdf). So, the set of observed points $P$ corresponds to the observation $X_j$, and the set of points $X$ corresponds to $X_i$. The ICP method will try to find a rigid transformation that best maps the points in $P$ to the points in $X$. See the example below, where the set $P_t$ is the transformed set. Given set of points $$P = \{p_1, \ldots, p_{N_p}\}$$ representing observations by the robot sensor (lidar or camera) at some time instant $t_j$, and another set of points $$X = \{x_1, \ldots, x_{N_x}\}$$ representing observations at another time instant $t_i$. Note that the ICP algorithm do not assume as certain order of the points in each set. The  correspondence between points is *not* assumed to be known. It is determined by the algorithm.  If the true point correspondence were knownLet $y_i$ denote the point in $X$ that (truly) corresponds to point $p_i$, that is, they are two observations of the same point/feature in the world. In this case, there is a unique solution (unless all points lie on a line), that can be found in one step (no iteration). The problem is to find the rotation matrix $R$ and translation $t$ that minimizes the least squares criterion function $$ d(R, t) = \sum_{i}^{N_p}||Rp_i + t - y_i||_2. $$There are well-studied methods that solve this optimization problem (for details see [this video](https://youtu.be/dhzLQfDBx2Q) by <NAME>). The situation is illustrated below (source: Rusinkiewicz)  When not knowing the point correspondence: use closest pointIf the correspondence between points are not known, we will have to estimate it. The idea behind the ICP method is to take the point of $X$ closest (in euclidean sense) to each point $p_i$ as the corresponding point $y_i$. This is illustrated below with points on the blue curve representing the set $P$, and points on the red curve representing the  set $X$ (source: Rusinkiewicz)If the two point sets are not "too far" from each other, the ICP algorithm will converge to the correct correspondence and the best (in the least square sense) rotation matrix $R$ and translation vector $t$.If the two points sets are too far from each other the ICP algorithm may fail to find the correct point correspondence. It is guaranteed to converge, but this minimum will not be a global minimum if the point correspondence is wrong.  AlgorithmThe algorithm is summarized as follows in the [original publication](https://graphics.stanford.edu/courses/cs164-09-spring/Handouts/paper_icp.pdf).Note that the authors proposed this method to map a known object (represented by a point cloud, by lines or by polygons) to observations (points, lines or polygons), which explains the unfamiliar terms. So, "registration" is what we call a rigid transformation. Applying the registation means applying the rigid transformation.  What you need to doIn the implementation below, some key parts are missing. They are clearly indicated (YOUR CODE HERE). Complete the implementation, run all the doctests, and the last two cells that plot some examples.<jupyter_code>import numpy as np
import doctest
def closest_point_set(P, X):
    """
    For each point in the set P, returns the point in the set X that are closest. 
    
    Arguments
    ---------
    P : nd-array (Np, 3)
        Set of points. 
    X : nd-array (Nx, 3)
        Set of points. 
    
    Returns
    -------
    Y : nd-array (Np, 3)
        Set of points 

    Tests
    ------
    >>> P = np.array([[1,0,0], [2,0,0], [3,0,0]])
    >>> X = np.array([[2,1,0], [1,1,0]])
    >>> Y = closest_point_set(P, X)
    >>> Y[0]
    array([1, 1, 0])
    >>> Y[1]
    array([2, 1, 0])
    >>> Y[2]
    array([2, 1, 0])
    """
    
    Y = []
    
    for p_ in P:
        # Find point in X that is closest to p_
        # 1) Calculate distance from each point in X to p_
        #    Must be a vector of length equal to the number of points in X
        
        ##################################################################
        # YOUR CODE HERE
        distances = 
        ##################################################################

        # 2) Find the index of the point that has the smallest distance
        ind_min_dist = np.argmin(distances)
        # 3) Append this point to the list Y 
        Y.append(X[ind_min_dist])
        
    return np.asarray(Y)


doctest.run_docstring_examples(closest_point_set, globals())
def cross_covariance(P, Y):
    """
    Calculates the cross-covariance matrix of the two set of points P and Y.
    Needed by the ICP algorithm in the calculation of the rigid transformation between
    the two point sets.
    
    Arguments
    ---------
    P : nd-array (Np, 3)
        Set of points. 
    Y : nd-array (Np, 3)
        Set of points. 
    
    Returns
    -------
    Sigma_py : nd-array (3, 3)
        Covariance matrix
    mu_p : nd-array (3,)
        centroid of set P (a.k.a. center of mass of the set)
    mu_y : nd-array (3,)
        centroid of set Y
        
    
    """
    
    # Center of mass calculations
    Np = len(P)
    mu_p = 1.0 / Np * np.sum(P, axis=0)
    mu_y = 1.0 / Np * np.sum(Y, axis=0)
    
    # Cross-covariance matrix
    Sigma_py = np.array([np.outer(pi, yi) for pi, yi in zip(P, Y)])
    Sigma_py = 1.0 / Np * np.sum(Sigma_py, axis=0) - np.outer(mu_p, mu_y)
    
    return Sigma_py, mu_p, mu_y
from scipy.spatial.transform import Rotation

def point_set_registration(P, Y):
    """
    Returns the rigid transformation (R, t) that best (in the least squares sense)
    transforms points in P to corresponding points in Y.
    
    Arguments
    ---------
    P : nd-array (Np, 3)
        Set of points. 
    Y : nd-array (Np, 3)
        Set of points. 
    
    Returns
    -------
    R : nd-array (3, 3)
        Rotation matrix
    t : nd-array (3,)
        translation

    Tests
    ------
    
    >>> P0 = np.eye(3)
    >>> th = np.pi/3
    >>> R = np.array([[np.cos(th), -np.sin(th), 0], [np.sin(th), np.cos(th), 0], [0, 0, 1]])
    >>> t = np.array([1,2,3])
    >>> Y = np.dot(R, P0.T).T + t
    >>> RR, tt = point_set_registration(P0, Y)
    >>> np.allclose(RR, R)
    True
    >>> np.allclose(tt, t)
    True
   
    
    """
    
    Sigma_py, mu_p, mu_y = cross_covariance(P, Y)
    
    A = Sigma_py - Sigma_py.T #Anti-symmetric matrix
    Delta = np.array([A[1,2], A[2,0], A[0,1]])
    Q = np.zeros((4,4))
    Q[0,0] = np.trace(Sigma_py)
    Q[0,1:] = Delta
    Q[1:, 0] = Delta
    Q[1:, 1:] = Sigma_py + Sigma_py.T - np.trace(Sigma_py)*np.eye(3)
    
    w, v = np.linalg.eigh(Q) # Finding the eigenvalues and eigenvectors
    q = v[:, -1] # Eigenvector corresponding to largest eigenvalue
    Rot = Rotation.from_quat(np.array([q[1], q[2], q[3], q[0]])) # Scipy uses scalar-last
    R = Rot.as_matrix()
    t = mu_y - np.dot(R, mu_p) 
    
    return (R, t)
doctest.run_docstring_examples(point_set_registration, globals())
def icp(P, X, tol=1e-4):
    """
    Implementation of the Iterative Closest Point algorithm. 
    Finds the rigid transformation that best maps the set of points in P to the
    set of points in X.
    
    Arguments
    ---------
    P : nd-array (Np, 3)
        Set of points. 
    X : nd-array (Nx, 3)
        Set of points. 
 
    Returns
    -------
    R : nd-array (3, 3)
        Rotation matrix
    t : nd-array (3,)
        translation

    Tests
    -----
    1) Noise-less case
    >>> th = np.pi/6
    >>> R = np.array([[np.cos(th), -np.sin(th), 0], [np.sin(th), np.cos(th), 0], [0, 0, 1]])
    >>> t = np.array([2,1,0])
    >>> P = np.random.randn(120, 3)
    >>> X = np.dot(R, P.T).T + t
    >>> np.random.shuffle(X) # Shuffles rows in place
    >>> RR, tt = icp(P, X)
    >>> np.allclose(RR, R)
    True
    >>> np.allclose(tt, t)
    True
    
    2) Missing data points
    >>> th = np.pi/6
    >>> R = np.array([[np.cos(th), -np.sin(th), 0], [np.sin(th), np.cos(th), 0], [0, 0, 1]])
    >>> t = np.array([2,1,0])
    >>> P = np.random.randn(120, 3)
    >>> Xall = np.dot(R, P.T).T + t
    >>> np.random.shuffle(Xall) # Shuffles rows in place
    >>> X = Xall[:80] # Remove 40 points 
    >>> RR, tt = icp(P, X)
    >>> th_est = np.arccos(RR[0,0])
    >>> th, th_est
    >>> np.abs(th-th_est) < np.pi*2/180 # Less than 2 degree error
    True
    >>> t-tt
    >>> np.linalg.norm(tt-t) < 0.01 # 
    True
 
    2) Noisy measurements
    >>> th = np.pi/3
    >>> R = np.array([[np.cos(th), -np.sin(th), 0], [np.sin(th), np.cos(th), 0], [0, 0, 1]])
    >>> t = np.array([2,1,0])
    >>> np.random.seed(1234) # For repeatable experiment
    >>> P = np.random.randn(120, 3)
    >>> X = np.dot(R, P.T).T + t
    >>> X += 0.01*np.random.randn(120,3) # Add some noise, stdv 0.01
    >>> np.random.shuffle(X) # Shuffles rows in place
    >>> RR, tt = icp(P, X)
    >>> th_est = np.arccos(RR[0,0])
    >>> th, th_est
    >>> np.abs(th-th_est) < np.pi*2/180 # Less than 2 degree error
    True
    >>> t, tt
    >>> np.linalg.norm(tt-t) < 0.01 # Error less than stdv of measurement noise
    True
    >>> 
   
    
    """
    
    P0 = P
    Pk = P
    
    d_prev = 1e30
    d = 1e20
    
    k=0
    while (d_prev - d) > tol:    
        k = k+1
        d_prev = d
        
        # a) Find the closest points in X to the transformed points Pk
        Yk = closest_point_set(Pk, X)
        
        # b) Compute the point set registration
        Rk, tk = point_set_registration(P0, Yk)
    
        # c) Apply the registration (the rigid transformation) on P0 to obtain P_{k+1}
        ###############################################################
        # YOUR CODE HERE
        Pk = 
        ###############################################################
        
        # d) Calculate the error
        d = np.linalg.norm(Pk-Yk)
    
    print("ICP took %d iterations" %k)
    return Rk, tk
doctest.run_docstring_examples(icp, globals(), verbose=False)<jupyter_output><empty_output><jupyter_text>Visualize points<jupyter_code>import matplotlib.pyplot as plt
%matplotlib notebook<jupyter_output><empty_output><jupyter_text>Point cloud<jupyter_code>#Random point cloud
Np = 12
th = np.pi/6
R = np.array([[np.cos(th), -np.sin(th), 0], [np.sin(th), np.cos(th), 0], [0, 0, 1]])
t = np.array([1,1,0])
P = np.random.randn(Np, 3)
P[:,2] = 0 # Only points in the sam<e plane
X = (np.dot(R, P.T).T + t) + 0.02*np.random.randn(Np,3)
X[:,2] = 0
np.random.shuffle(X) # Shuffles rows in place
RR, tt = icp(P, X)
Pt = np.dot(RR, P.T).T + tt
fig = plt.figure(figsize=(6,5))
plt.plot(X[:,0], X[:,1], 'bo', label='X')
plt.plot(Pt[:,0], Pt[:,1], 'ro', label='Pt')
plt.plot(P[:,0], P[:,1], 'go',label='P')
plt.legend()
fig.axes[0].set_aspect('equal', 'box')<jupyter_output><empty_output><jupyter_text>Angle (corner)<jupyter_code># Angle (corner) 

Np = 12 # Number of points per side 
th = np.pi/6
R = np.array([[np.cos(th), -np.sin(th), 0], [np.sin(th), np.cos(th), 0], [0, 0, 1]])
t = np.array([1,1,0])
P = np.zeros((24, 3))
P[:12, 0] = np.linspace(0, 1, Np)
P[12:, 0] = np.linspace(1, 0, Np)
P[:, 1] = np.linspace(0,2, 2*Np)
X = (np.dot(R, P.T).T + t) + 0.02*np.random.randn(2*Np,3)
X[:,2] = 0
np.random.shuffle(X) # Shuffles rows in place
RR, rr = icp(P, X)
Pt = np.dot(RR, P.T).T + rr
fig = plt.figure(figsize=(6,5))
plt.plot(X[:,0], X[:,1], 'bo', label='X')
plt.plot(Pt[:,0], Pt[:,1], 'ro', label='Pt')
plt.plot(P[:,0], P[:,1], 'go',label='P')
plt.legend()
fig.axes[0].set_aspect('equal', 'box')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.16362893338782183, 'fraction_numerical': 0.02280343277482632, 'mean_word_length': 2.788235294117647, 'pattern_counts': {'":': 0, '<': 24, '<?xml version=': 0, '>': 174, 'https://': 3, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '528776', 'license': 'BSD-2-Clause', 'n_tokens_mistral': 4497, 'n_tokens_neox': 4265, 'n_words': 1442, 'path': 'python/Iterative_Closest_Point.ipynb', 'repo_name': 'robotica-cem/mobile-robots-module', 'size': '12253'}