def METHOD_NAME(pointcloud_A, pointcloud_B, initial_pose=None,
                           max_iterations=20, tolerance=1e-3):
    '''Find best least square fit that transforms pointcloud A to pointcloud B.
    Input:
        A: Nxm numpy array of source mD points
        B: Nxm numpy array of destination mD point
        initial_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 pointcloud_A.shape == pointcloud_B.shape
    pointcloud_A = add_homogenous_coordinate(pointcloud_A)
    pointcloud_B = add_homogenous_coordinate(pointcloud_B)
    pointcloud_A_0 = np.copy(pointcloud_A)
    if initial_pose is not None:
        pointcloud_A = np.dot(initial_pose, pointcloud_A.T).T
    previous_error = 0
    for iteration_arg in range(max_iterations):
        distances, indices = nearest_neighbor(pointcloud_A, pointcloud_B)
        print(indices.shape, pointcloud_A.shape, pointcloud_B.shape)
        pointcloud_B = pointcloud_B[indices]
        print(pointcloud_B.shape)
        print('***********************')
        affine_matrix = calculate_affine_matrix(pointcloud_A[:, :3], pointcloud_B[:, :3])
        pointcloud_A = np.dot(affine_matrix, pointcloud_A.T).T
        mean_error = np.mean(distances)
        print(mean_error)
        if np.abs(previous_error - mean_error) < tolerance:
            break
        previous_error = mean_error
    affine_transform = calculate_affine_matrix(pointcloud_A_0[:, :3], pointcloud_A[:, :3])
    return affine_transform, distances, iteration_arg