개요

동일하지만 위치와 방향이 다른 두 개의 포인트 클라우드가 있을 때, 어떻게 하면 정합할 수 있으며 두 포인트 클라우드의 관계를 어떻게 하면 구할 수 있을까?

많은 방법들이 있겠지만, 이 문제를 해결할 수 있는 제일 기초적인 방법인 ICP에 대해서 알아보도록 하자.

Reference

ICP (Iterative Closed Point)

Center of Mass

포인트 점군들 Q, P가 있을 때, 각각의 포인트들의 평균 값을 구할 수 있다.

$μQ=1/∣C∣∑qi\mu_Q=1/|C|\sum q_i$

그리고 다시 원래의 포인트들과 평균 점의 차이를 통해 새로운 포인트 클라우드를 계산할 수 있다. (Q`, P`)

수학적으로 보면 포인트 클라우드 평균 위치만큼 평행 이동을 시킨 것이다.
- 즉, cross-covariance 행렬을 구한 것이다.

물리적으로 보면 translation 영향을 제거하여 rotation 성분만을 남겨놓고 싶기 때문이다.

Orthogonal Procrustes Problem

이 문제를 Orthogonal Procrustes 문제라고 보고 있다.
- Orthogonal Procrustes 문제는 선형 대수학에서 두 개의 행렬이 주어졌을 때, 하나의 행렬을 orthogonal 행렬을 사용하여 다른 행렬에 최대한 가깝게 maaping 하는 문제이다. 이 문제는 다양한 응용 분야에서 사용되며, 특히 데이터 분석, 기계 학습, 컴퓨터 비전 등에서 두 데이터 세트 간의 최적의 정합(registration)을 찾는데 활용된다.

회전 행렬 R의 조건과 특징
1. Orthogonal
  1. 모든 열 벡터 (Column vector)들이 서로 직교(내적이 1)
  1. 각 벡터들의 길이가 1 (unit vector)
1. det(R) = 1
  1. 행렬식이 1이라는 의미는 선형 변환 (Linear combination) 관점에서 본다면 변환 이후에도 공간의 부피가 보존된다는 것을 의미
  1. 만약 det(R) = -1이라면 이는 회전과 동시에 공간이 반사 (reflection) 된다는 것을 의미

Singular Value Decomposition

cross-covariance matrix를 계산했을 때, Q`과 P`의 곱을 계산하는 것이다.

이 cross-covariance matrix는 결국 두 포인트 클라우드의 상관 관계를 담고 있는 matrix이다.

covariance matrix를 SVD로 분해하면 D와 V^T를 구할 수 있다.

U와 V^T는 orthogonal matrix이기 때문에 이 둘의 곱 또한 orthogonal matrix이며 행렬식 (det)은 -1 아니면 1이 나온다.

즉, 둘의 곱은 회전 행렬 (Rotation matrix)를 의미한다.

결론적으로 두 포인트 클라우드의 data association을 표현한 상관 관계를 갖는 cross covariance matrix를 SVD 분해를 이용하면 두 포인트 클라우드 사이의 관계를 표현한 Rotation matrix를 계산할 수 있다.

만약 cross covariance matrix인 W의 Rank가 3이고 E를 최소화하는 파라미터가 unique 하면 R(Rotation matrix)와 t(translation)은 위와 같이 계산할 수 있다.

SVD-Based Alignment Summary

수학적으로 보면 Orthogonal Procrustes 문제는 두 행렬 사이의 변환을 찾아내는 문제이다.

행렬 $W$ 가 두 데이터( $Q, P$ )의 연관성을 표현한 cross-covariance matrix라고 해보자. 그럼 $W$ 를 SVD를 통해 분해하면, $W=UΣVTW=U\Sigma V^T$ 라고 표현할 수 있다. 여기서 $UV^T$ 를 최적의 회전 행렬이라고 이야기 할 수 있다. 그것의 대한 근거는 두 포인트 클라우드 $Q, P$ 의 크기 즉, 프로비니우스 norm인 $Q-RP||_F$ 를 최소화 하기 때문이다.

여기서 프로비니우스 norm으로 예시를 들었지만, Euclidean norm 또한 일치한다.

ICP with Unknown Data Association

하지만 대부분 데이터 연관성을 아는 것은 어렵다. 따라서 반복적으로 가장 가까운 점을 찾는 알고리즘이 필요하다. 이것이 바로 ICP이다.

코드 작성

curPointCloud = {};
prePointCloud = {};

covarianceMatrix = {};

u,s,v = 0;

error = 0.0;
threshold = 0.0;

R = 0;
T = 0;

error = get_maximum_value();

while(error > threshold)
{
	curPointCloud = getPointCloud();

	// Get the cross covariacne matrix between two point clouds
	covarianceMatrix = calcCrossCovarianceMatrix(prePointCloud, curPointCloud);
	
	// SVD (Singular Value Decomposition)
	u,s,v = svd(covarianceMatrix);
	
	// Rotation matrix from SVD
	R = u * v.T;

	// Translation
	T = prePointCloud + R * curPointCloud;
	
	// Calculate the error
	error = calcError(R,T);

	prePointCloud = curPointCloud;
}

setPose(R,T);

고려해야 할 사항

Closest-Point Matching

가장 가까운 점을 찾기 위해선?→ KD-tree 같은 알고리즘 사용

Uploaded by Notion2Tistory v1.1.0