# ICP(Iterative Closest Point) with Partially Overlapping Conditions & Changing Point Numbers

I am currently working on fixing vehicle odometry data using lidar contour points. Since I am receiving lidar data in the form of countour points, I thought I'd use ICP to correct the odometry error calculated from bicycle model.

Here is my question.

The code I am referencing from https://github.com/kissb2/PyICP-SLAM/blob/master/utils/ICP.py requires that number of points at time t = x must equal number of points at time t = x+k, or at least be in the array of same size. (SHOWN by assert statement "assert A.shape == B.shape" ) However, I do get fluctuating number of contour points per every time frame, thus this algorithm is not applicable to my problem.

Has anyone ever came across an ICP with differing array size of 'source frame' and 'target frame'? If so, how should I address the issue? If you just append the points into the arrays A & B of same size M with empty spots being zeros, would the ICP algorithm autonomously sort itself out despite receiving different number of data points? (For example, if A had 30 points and B had 60 points, append A with empty matrix of size 30 to make it into size 60.)

Thank you!

def icp(A, B, init_pose=None, max_iterations=20, tolerance=0.001):
'''
The Iterative Closest Point method: finds best-fit transform that maps points A on to points B
Input:
A: Nxm numpy array of source mD points
B: Nxm numpy array of destination mD point
init_pose: (m+1)x(m+1) homogeneous transformation
max_iterations: exit algorithm after max_iterations
tolerance: convergence criteria
Output:
T: final homogeneous transformation that maps A on to B
distances: Euclidean distances (errors) of the nearest neighbor
i: number of iterations to converge
'''

assert A.shape == B.shape

# get number of dimensions
m = A.shape

# make points homogeneous, copy them to maintain the originals
src = np.ones((m+1,A.shape))
dst = np.ones((m+1,B.shape))
src[:m,:] = np.copy(A.T)
dst[:m,:] = np.copy(B.T)

# apply the initial pose estimation
if init_pose is not None:
src = np.dot(init_pose, src)

prev_error = 0

for i in range(max_iterations):
# find the nearest neighbors between the current source and destination points
distances, indices = nearest_neighbor(src[:m,:].T, dst[:m,:].T)

# compute the transformation between the current source and nearest destination points
T,_,_ = best_fit_transform(src[:m,:].T, dst[:m,indices].T)

# update the current source
src = np.dot(T, src)

# check error
mean_error = np.mean(distances)
if np.abs(prev_error - mean_error) < tolerance:
break
prev_error = mean_error

# calculate final transformation
T,_,_ = best_fit_transform(A, src[:m,:].T)

return T, distances, i
$$$$
`