"""
ref: https://github.com/ClayFlannigan/icp/blob/master/icp.py
"""
import numpy as np
from sklearn.neighbors import NearestNeighbors
def best_fit_transform(A, B):
'''
Calculates the least-squares best-fit transform that maps corresponding points A to B in m spatial dimensions
Input:
A: Nxm numpy array of corresponding points
B: Nxm numpy array of corresponding points
Returns:
T: (m+1)x(m+1) homogeneous transformation matrix that maps A on to B
R: mxm rotation matrix
t: mx1 translation vector
'''
assert A.shape == B.shape
# get number of dimensions
m = A.shape[1]
# translate points to their centroids
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
AA = A - centroid_A
BB = B - centroid_B
# rotation matrix
H = np.dot(AA.T, BB)
U, S, Vt = np.linalg.svd(H)
R = np.dot(Vt.T, U.T)
# special reflection case
if np.linalg.det(R) < 0:
Vt[m-1,:] *= -1
R = np.dot(Vt.T, U.T)
# translation
t = centroid_B.T - np.dot(R,centroid_A.T)
# homogeneous transformation
T = np.identity(m+1)
T[:m, :m] = R
T[:m, m] = t
return T, R, t
def nearest_neighbor(src, dst):
'''
Find the nearest (Euclidean) neighbor in dst for each point in src
Input:
src: Nxm array of points
dst: Nxm array of points
Output:
distances: Euclidean distances of the nearest neighbor
indices: dst indices of the nearest neighbor
'''
assert src.shape == dst.shape
neigh = NearestNeighbors(n_neighbors=1)
neigh.fit(dst)
distances, indices = neigh.kneighbors(src, return_distance=True)
return distances.ravel(), indices.ravel()
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[1]
# make points homogeneous, copy them to maintain the originals
src = np.ones((m+1,A.shape[0]))
dst = np.ones((m+1,B.shape[0]))
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
Python을 사용한 ICP 알고리즘
ICP(Iterative Closest Point) 알고리즘은 주로 포인트 클라우드 데이터의 정렬이나 등록에 사용되는 컴퓨터 비전 및 로봇 공학에서 사용되는 기술이다. 두 포인트 세트 간의 거리를 최소화하여 점들을 정렬하는 방법을 보여주는 Python ICP 코드를 공부했다.
코드 개요 :
코드에는 두 포인트 세트 간의 최적 변환을 계산하는 함수, 가장 가까운 이웃을 찾는 함수, ICP 알고리즘을 사용하여 정렬을 반복적으로 세밀하게 조정하는 함수가 포함되어 있다.
1. best_fit_transform
- 두 포인트 클라우드 세트의 질량 중심 점을 구한 후, 질량 중심으로 평행이동 시킨다.
- 두 행렬을 텐서 곱 시킨 후 특이값 분해를 한다.
- 특이값 분해(SVD)를 사용하여 회전행렬('R')과 이동 벡터('t')를 찾으며, 결과 변환행렬 ('T')을 반환한다.
2. nearest_neighbor
- 하나의 데이터세트에서 다른 데이터 세트로 가장 가까운 점을 찾는 함수이다.정렬하는 데 필수적인 단계이다. K-nearest 최근접 이웃 알고리즘을 통해 가장 가까운 점을 찾는다. ravel() 함수로 다차원 배열을 1차원 배열로 평탄화했다.
3. ICP
- Source 데이터 세트를 Destination 데이터 세트에 맞게 반복적으로 best_fit_transform을 적용한다.
- 계산된 변환 행렬을 사용하여 데이터 세트 간의 오차를 줄여나간다.
- 허용 오차 이하로 떨어지거나, 설정된 반복 횟수에 도달하면 종료한다.
Key point
- 회전 행렬의 행렬식 음수 처리:
코드는 회전 행렬의 행렬식이 양수인지 확인하는 검사를 포함하고 있으며, 행렬식이 음수인 경우, 즉 회전이 아닌 반사를 나타내는 경우, SVD에서 나온 Vt의 마지막 행의 부호를 변경하여 회전을 보장한다.
hola!