3D-3D 대응점

대응하는 두 3D 점 집합 $\{\mathbf{p}_i\}$ (소스)와 $\{\mathbf{q}_i\}$ (타깃)가 주어지면, 3D-3D 정합 문제는 이들을 정렬하는 강체 변환을 찾는다:

$\min_{R,\mathbf{t}} \sum_i \|\mathbf{q}_i - (R\mathbf{p}_i + \mathbf{t})\|^2$

이는 두 프레임이 모두 3D 데이터를 제공할 때마다 발생한다 — RGB-D나 스테레오 프레임, LiDAR 스캔, 혹은 병합해야 할 두 포인트 클라우드 맵. 고전적인 알고리즘은 **ICP (Iterative Closest Point)**다.

닫힌 형태의 SVD 해법 (알려진 대응점)

대응점이 알려진 경우, 최적 정렬은 다음과 같은 닫힌 형태를 가진다:

중심(centroid) 계산: $\bar{\mathbf{p}} = \frac{1}{n}\sum_i \mathbf{p}_i$ , $\bar{\mathbf{q}} = \frac{1}{n}\sum_i \mathbf{q}_i$ .
교차 공분산 계산: $H = \sum_i (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{q}_i - \bar{\mathbf{q}})^T$ .
SVD: $H = U\Sigma V^T$ .
회전: $R = VU^T$ ( $\det(R) = -1$ 인 경우 부호 보정 필요).
이동: $\mathbf{t} = \bar{\mathbf{q}} - R\bar{\mathbf{p}}$ .

왜 중심을 먼저 계산하는가? 비용 함수를 $\mathbf{t}$ 에 대해 미분하여 0으로 놓으면, 최적 이동은 항상 소스 중심을 타깃 중심에 매핑함을 알 수 있다. 이를 다시 대입하면 문제가 분리된다: 회전은 중심화된 점들에서만 구해지고, 이동은 5단계에서 뒤따라 나온다. 최적 스케일 인자 $s$ 까지 확장한 동일한 구성(Umeyama 정렬)은 ATE를 계산하기 전에 추정 궤적과 그라운드 트루스를 정렬하는 데 궤적 평가 도구들이 사용하는 방법이다 — 스케일을 관측할 수 없는 단안 SLAM에서는 스케일까지 정렬하는 변형이 필수적이다.

ICP의 반복 부분

대응점을 모를 때, ICP는 다음을 번갈아 반복한다:

대응점 단계: 각 소스 점에 대해 타깃에서 최근접 이웃을 찾는다(kd-트리로 가속).
정렬 단계: 위의 닫힌 형태 문제를 풀고 변환을 적용한다.

수렴할 때까지 반복한다. ICP는 합리적인 초기 추정이 필요하며(지역 최솟값으로 수렴한다) 이상치에 민감하므로, 실용적인 변형들은 거리 임계값, 강건 커널, 법선 기반 배제를 추가한다.

점-대-평면과 그 이후

점-대-평면(point-to-plane) ICP는 타깃의 법선 $\mathbf{n}_i$ 를 사용하여 점-대-점 지표를 점-대-표면 지표로 대체한다:

$\min_{R,\mathbf{t}} \sum_i \big(\mathbf{n}_i^T\,(R\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i)\big)^2$

점이 표면을 따라 미끄러지는 것은 허용되고, 표면에 수직인 방향으로만 벌점을 받는다 — “최근접 점”이 실제 대응점인 경우는 드물기 때문에 이는 현실에 더 부합한다. 매끄러운 표면에서 훨씬 빠르게 수렴하며, RGB-D와 LiDAR 파이프라인에서 지배적으로 쓰인다(KinectFusion이 이를 사용한다). 비용은 더 이상 닫힌 형태가 아니며, 회전을 선형화(소각 근사)하여 반복마다 6x6 선형 시스템으로 풀어낸다. 같은 계열의 추가적인 개선으로는 Generalized-ICP(평면-대-평면, 공분산 가중)와 LOAM과 같은 LiDAR 오도메트리에서 쓰이는 점-대-선 지표가 있다.

흔한 함정

나쁜 초기화: ICP의 수렴 영역은 작다 — 오도메트리, 등속 운동 모델, 또는 전역적 방법에서 시드를 얻어야 하며, 큰 움직임 이후 항등변환에서 시작해서는 절대 안 된다.
부분적 겹침: 진짜 대응점이 없는 점들은 추정치를 끌어당긴다. 거리 백분위수나 강건 커널로 대응점을 다듬어야 한다.
퇴화된 기하: 단일 평면은 3 자유도(2개의 이동 + 평면 내 1개의 회전)를 제약하지 못한다. 긴 복도와 터널도 마찬가지로 해를 과소 제약한다 — 6x6 정규방정식 행렬의 고윳값을 통해 퇴화를 감지해야 한다.
SVD로부터의 반사: $\det(R) = -1$ 일 때의 부호 보정을 항상 적용해야 한다. 이를 잊으면 잡음이 있거나 평면인 데이터에서 거울에 비친 듯한 “정렬”이 나온다.

SLAM에서의 의미

ICP는 밀집 RGB-D SLAM의 추적 엔진이며(KinectFusion은 각 깊이 프레임을 모델에 점-대-평면 ICP로 정렬한다), 대부분의 LiDAR 오도메트리 시스템의 추적 엔진이기도 하다. 또한 SVD 정렬 단계는 두 서브맵을 병합해야 할 때마다 루프 클로징에서 다시 나타난다. 2D-2D(깊이 없음), 2D-3D(한쪽만 깊이 있음)와 함께, 이는 대응점 도구 상자를 완성한다: 어느 것을 사용할지는 단순히 3D 정보가 어디에 있는지로 결정된다.

실습

ICP 실습