ICP 계산속도 향상을 위한 빠른 Correspondence 매칭 방법

2.1 ICP (Iterative Closest Point) 알고리즘

ICP (Iterative Closest Point)는 서로 다른 위치에서 얻은 두 포인트 클라우드가 있을 때 아래 4단계를 반복하여 두 포인트 클라우드를 정합하고 마지막 단계가 끝날 때마다 새롭게 위치 차이를 추정한다. [Fig. 1]에서 볼 수 있듯이, 상대적 포즈 차이는 두 포인트 클라우드의 기준 좌표계 사이의 강체 변환 행렬(Rigid Body Transformation Matrix)로 나타낸다. 기준 포인트 클라우드의 기준 좌표계가 (X₁, Y₁), 변환 포인트 클라우드의 기준 좌표계가 (X₂, Y₂)일 때 두 좌표계 사이의 변환 행렬들의 관계는 식 (1)과 같다.

포인트 클라우드는 라이다의 기준 좌표계에서 Z=0 평면에 표현되고 Z축으로 라이다의 물리적 이동이 없다고 가정한다. 4단계를 모두 수행하면 ICP를 한번 수행했다고 하며 ICP가 k번 수행되었을 때 추정한 위치 차이를 q_k(R_k, t_k)으로 나타낸다. [Fig. 2]의 순서도에 해당하는ICP알고리즘은 다음 네가지 단계를 반복한다.

[Fig. 2]

A flow chart of ICP algorithm

1. 변환(Transformation): 이전 변환 행렬 q_k-1 (R_k-1, t_k-1)으로 변환 포인트 클라우드를 변환하여 새로운 포인트 클라우드를 만들고 이를 k번째 ‘중간 포인트 클라우드(Middle Point Cloud, M_PC)’라 한다. 변환 행렬의 초기값(q₀)은 ICP 성능에 큰 영향을 주는데 이는 Wheel Odometry 또는 관성 측정 장치(IMU)와 같은 다른 센서를 통해 예측한다. 포인트 클라우드는 라이다의 기준 좌표계에서 Z=0 평면에 나타나므로 포인트 클라우드의 점들에 일정한 규칙으로 순서를 매겼을 때 중간 포인트 클라우드의 i번째 점은 $$ P_i^M=\left[\begin{array}{c}{x}_i^M\\ y_i^M\end{array}\right]$$으로 표현된다. 그리고 q_k를 이용하여 변환 포인트 클라우드를 변환하여 중간 포인트 클라우드를 구하는 식은 식 (2)와 같다.

2. 대응 관계(Correspondence): k번째 중간 포인트 클라우드의 각 점에 대응하는 기준 포인트 클라우드 위의 유의미한 부분을 찾는다. 여기서 유의미한 부분(Segment)은 유클리드 공간에서 거리가 가장 가까운 점을 기반으로 형성되는데 [2]의 점-점(point-to-point) ICP의 경우 가장 가까운 점이 되며 [5]의 점-선(point-to-line) ICP의 경우 가장 가까운 점 두개를 이은 직선이 된다. 기준 포인트 클라우드 위의 유의미한 부분은 대응하는 변환 포인트 클라우드의 점($$ P_i^T$$)과 함께 배열로 저장되며 점-점의 경우 , 점-선의 경우 이다. $$ P_{i_1}^R$$는 $$ P_i^M$$에서 가장 가까이에 위치한 기준 포인트 클라우드의 한 점이고 $$ P_{i_2}^R$$는 두번째로 가까운 점이다.

3. 이상치 제거(Outlier rejection): ICP는 correspondence결과를 바탕으로 최소자승법(Least Squares Method)를 통해 해를 구한다. 최소자승법은 이상치에 취약하므로 Correspondence 결과에 가중치를 부여하는 M-estimation 방법을 사용하거나 이상치를 판별하여 제거하는 RANSAC 방법이 많이 사용된다.

4. 오차함수 최소화(Minimizing Error Function): 이상치를 제거한 Correspondence 결과를 이용하여 오차함수를 구한 뒤 최소자승법으로 이를 최소화하는 q_k를 구한다. ICP 종류에 따라 오차함수의 형태가 구분된다. [Fig. 3(a)]의 점-점의 경우 강체 변환된 $$ P_i^T$$와 $$ P_{i_1}^R$$ 사이의 유클리디안 거리이고 [Fig. 3(b)]의 점-선의 경우 강체 변환된 $$ P_i^T$$에서 $$ P_{i_1}^R$$,$$ P_{i_2}^R$$ 두 점을 이은 직선까지의 수직 거리이다. 현 단계에서 구한 q_k와 이전 단계에서 구한 q_k-1의 차이를 통해 ICP 종료 여부를 판별하는데 서로 다른 위치에서 얻은 포인트 클라우드가 완벽하게 정합하는 것은 불가능하므로 일정 임계값 ϵ를 이용하여 판별한다. q_k와 q_k-1의 차이가 ϵ보다 크면 1번으로 돌아가 q_k를 이용하여 다시 변환하고 차이가 작다면 ICP를 종료하고 최종 변환 행렬을 q_k로 확정한다. ϵ의 크기는 정합의 정도와 정확도 사이의 절충을 표현한다. ϵ의 크기가 크면 정합이 충분히 되지 않고 ϵ 값이 작으면 올바르지 않은 Correspondence이 발생하여 최종 정합이 어긋날 수 있다.

[Fig. 3]

Schematic diagrams of two different residual and cost functions: (a) Point-to-point and (b) Point-to-line

ICP 알고리즘은 변환 포인트 클라우드의 각 점에 대응하는 기준 포인트 클라우드의 유의미한 부분을 찾고 그 사이의 거리 합을 최소로 만드는 변환 행렬 q(R,t)를 구하는 과정을 반복적으로 수행한다. ICP를 한번 수행했을 때 4단계를 하나의 식으로 표현하면 식 (5)와 같다. $$ S_i^{ref}$$는 $$ P_i^T$$에 대한 기존 포인트 클라우드 위의 유의미한 부분이며 $$ \overleftarrow{\left(S_i^{ref},\right)}$$는 유클리드 공간에서 $$ S_i^{ref}$$ 위로의 사영을 의미한다.

2.2 Jump Table을 이용한 Correspondence

A.Censi는 Correspondence의 시간을 Jump Table을 사용하여 단축했다^[5]. Correspondence는 중간 포인트 클라우드의 각 점에 대해 기준 포인트 클라우드를 탐색하며 가장 가까운 점을 찾는다. [5]에서는 기준 포인트 클라우드 위의 모든 점을 차례대로 탐색하지 않고 Jump Table을 사용하여 기존 포인트 클라우드의 점들을 점프하며 탐색한다.

Jump Table은 원점(라이다)에서 기준 포인트 클라우드의 각 점까지의 거리 비교를 통해 한 점에 대해 서로 다른 네 개의 점을 설정한다(up_small, up_big, down_small, down_big). 기준 포인트 클라우드 한 점에 대해 반 시계 방향을 위(up), 시계 방향을 아래(down)로 할 때, 최초로 거리가 큰 점을 big, 거리가 작은 점을 small로 설정한다. 예를 들어, 어떤 기준 포인트 클라우드의 20번째 점의 Jump Table은 [Fig. 4(a)]와 같이 설정된다.

[Fig. 4]

(a) Example of jump table (b) Finding next check point when searching direction is down (c) Exception situation of existing Jump table-based Correspondence algorithm

중간 포인트 클라우드의 한 점($$ P_i^M$$)에서 가장 가까운 기준 포인트 클라우드의 한 점을 찾는 상황에서 현재 탐색하고 있는 점($$ P_{CHECK}^R$$)이 가장 가까운 거리(best_dis)를 갖는 점($$ P_{Best}^R$$)으로 업데이트 된 경우는 [Fig. 4(b)]와 같다.

아래 방향으로 탐색할 때 새롭게 best_dis가 업데이트 될 수 있는 경우는 기준 포인트 클라우드의 점이 주황색 구역에 존재할 때이다. 위 상황에서 [5]는 Jump Table을 이용하여 아래 방향에 존재하는 점들을 차례대로 탐색하지 않고 주황색 영역에 존재할 가능성이 있는 점들만 Jump하며 탐색한다. 그리고 $$ P_i^M$$에서 원점과 기준 포인트 클라우드의 한 점을 이은 직선에 수직으로 내린 점($$ P_{MIN}^R$$)까지의 거리(MIN_DIST)가 현재의 best_dis보다 크게 되면 탐색을 중지하고 처음 탐색을 시작한 점($$ P_{START}^R$$)로 돌아가 반대 방향으로 탐색을 시작한다. 여기서 $$ P_{START}^R$$은 원점에서 $$ P_i^M$$으로의 방향 벡터 위에 존재하는 기준 포인트 클라우드 위의 한 점이다. [5]에서 ICP를 수행하기 전에 기준 포인트 클라우드의 각 점에 대한 Jump Table을 저장하는 시간은 전체 ICP 시간의 1%를 차지하며 Jump Table을 사용했을 때 기준 포인트 클라우드 위의 모든 점을 차례대로 탐색하는 일반적인 방법보다 시간을 82% 단축한 결과를 보였다.

3.1 Next check point 결정

[Fig. 4(b)]의 $$ P_{CHECK}^R$$에서 아래 방향으로 탐색할 때 [5]는 다음으로 탐색할 점($$ P_{NEXT}^R$$)을 $$ P_{CHECK}^R$$의 Jump Table에서 선택하는데 그 기준은 원점에서 $$ P_i^M$$의 거리($$ P_i^M$$.r)와 $$ P_{CHECK}^R$$의 거리($$ P_{CHECK}^R$$.r)의 대소이다[Table 1(a)]. [Fig. 4(b)]에서는 아래 방향으로 탐색하고 있으며 $$ P_i^M$$.r이 $$ P_{CHECK}^R$$.r보다 작으므로 $$ P_{NEXT}^R$$는 $$ P_{CHECK}^R$$의 down _small이 되어 주황색 영역에 존재할 수 있는 점을 확인하게 되는 것이다.

[Table 1]

(a) Existing rule presented in [5] to select PNEXTR (b) Our proposed rule to select PNEXTR

대부분의 상황에서는 [Table 1(a)]의 기준이 합리적이지만 $$ P_{CHECK}^R$$이 $$ P_{MIN}^R$$.r<$$ P_{CHECK}^R$$.r<$$ P_i^M$$.r인 구간에 존재할 경우[Fig. 4(c)]는 합리적이지 않다. $$ P_{MIN}^R$$.r은 원점에서부터 $$ P_{MIN}^R$$까지의 거리이다. best_dis가 업데이트 되기 위해서는 $$ P_{NEXT}^R$$이 주황색 영역에 존재해야 하며, [Fig. 4(c)]를 보았을 때, $$ P_{CHECK}^R$$의 Jump Table에서 down_small이 선택되어야 하지만 [5]의 기준으로는 $$ P_i^M$$.r가 $$ P_{CHECK}^R$$.r 보다 크므로 down_big을 선택한다.

[5]에서 제안된 Jump Table에서 $$ P_{NEXT}^R$$을 선택하는 기준에는 오류가 있으며 우리는 $$ P_{CHECK}^R$$에서 바라보는 원점과 $$ P_i^w$$의 사이각(θ)을 이용하여 새로운 기준을 [Table 1(b)]와 같이 제시한다. θ이 예각일 경우 best_dis가 업데이트 될 수 있는 영역이 $$ P_{CHECK}^R$$의 원점 방향에 존재하므로 $$ P_{NEXT}^R$$의 small을 선택하고 둔각일 경우 best_dis가 업데이트 될 수 있는 영역이 $$ P_{CHECK}^R$$의 바깥 방향에 존재하므로 big을 선택한다.

3.2 360°에 유효한 Jump Table

[5]의 경우, $$ P_{NEXT}^R$$가 아래 방향 탐색 시 기준 포인트 클라우드의 시작점(0^th), 위 방향 탐색 시 기준 포인트 클라우드의 끝점(n^th)에 도달할 경우 해당 방향으로의 탐색을 중지하므로, 라이다의 시야각이 360° 에 근접할 때 오류가 발생한다. $$ P_{START}^R$$가 0^th에 가까울 경우 best_dis가 0^th을 넘어 n^th 부근에서 업데이트 될 수 있기 때문이다. 우리는 $$ P_{NEXT}^R$$가 0^th에 도달하면 n^th으로, n^th에 도달하면 0^th 넘겨주고, $$ P_{CHECK}^R$$가 $$ P_{START}^R$$에서 180° 떨어진 점에 도달했을 때 탐색을 중지하도록 했다. 이를 통해 시야각에 제한을 받지 않고 모든 범위에서 Jump Table을 사용할 수 있다.

3.3 Pseudo Code 설명

우리가 새로 고안한 알고리즘의 Pseudo Code는 [Algorithm 1]과 같으며 사용된 기호의 의미는 [Table 2]와 같다.

[Algorithm 1]

Pseudo Code of our jump table algorithm

[Table 2]

Symbols used in [Algorithm 1]

[Algorithm 1]의 주요 구문의 의미는 다음과 같다:

line 1 : 포인트 클라우드 P^M 위의 모든 점에 대해 탐색.

line 9 : up_stopped 과 down_stopped 모두 참이면 탐색 종료.

line 10 : 탐색이 중지된 방향의 반대쪽으로 탐색하도록 설정.

line 12~14 : $$ P_{CHECK}^R$$이 끝지점에 도달하면 반대 끝지점으로 넘어가 같은 방향으로 탐색 지속.

line 24 : best_dis가 min_dist보다 크면 더 이상 해당 방향으로 탐색하는 것이 무의미하므로 탐색 종료.

line 26~27 : θ_criteria를 코사인 제 2 법칙을 이용하여 구함.

line 30~34 : Table 1(1)를 기준으로 θ_criteria의 값에 따라 다음으로 탐색할 점을 결정.

4.1 실험

Hokuyo-10LX와 F1TENTH Simulator를 각각 활용하여 두 번의 실험을 진행했다[Fig. 5]. 먼저, 270°의 시야각을 가진 Hokuyo-10LX 라이다를 F1TENTH 경주차에 부착하고 인하대학교 하이테크 건물 6층에서 구동하여 실시간으로 데이터를 받았다. 이 데이터를 우리의 Jump Table 기반 Correspondence, [5]의 Jump Table 기반 Correspondence, 그리고 일반적인 Correspondence 알고리즘(Naïve algorithm)에 동일하게 입력하고 비교했다. 일반적인 Correspondence는 $$ P_i^M$$에서 기준 포인트 클라우드의 모든 점들과 거리를 빠짐없이 비교하여 $$ P_{BEST}^R$$점을 찾는다. 우리의 알고리즘에서 수행된 중간 포인트 클라우드의 모든 점들에 대한 $$ P_{BEST}^R$$가 일반적인 Correspondence에서 수행된 것과 일치한다면 Correspondence가 정확히 수행된 것으로 판단했다. Correspondence 이후 단계인 이상치 제거 단계는 생략했으며 오류함수 최소화 단계는 [5]의 PL-ICP의 방법을 사용했다.

[Fig. 5]

(a) F1TENTH racecar with Hokuyo-10LX; (b) F1TENTH Simulator

Jump Table이 360°에 대해 유효한 우리의 알고리즘의 성능을 270°시야각의 Hokuyo-10LX으로 확인하기에는 한계가 있어 360°시야각의 라이다가 구현된 F1TENTH Simulator^[10]에서 추가로 실험했다. [Fig. 5(b)]와 같이 Simulator에서 제공하는 주행환경에서 차량이 한 쪽 벽면을 따라가며 얻은 라이다 데이터를 이용하여 같은 비교를 진행하였다. 차량의 선속도와 각속도는 각각 1.8 m/s와 0.3 rad/s이며 차량의 가로 길이와 세로 길이는 각각 0.33 m와 0.2 m이다.

두 실험에서 사용된 소스코드는 https://github.com/jahii/F1tenth의 Experiment 브랜치 안에 scan_matching_skeleton/src/correspond.cpp에서 확인할 수 있다. 또한, 실험 영상은 https://youtu.be/VvvXB3yQ_Wg 에서 확인할 수 있다.

4.2 실험 결과

Hokuyo-10LX를 이용한 첫번째 실험에서는 대부분의 경우 중간 포인트 클라우드의 각 점에 대한 $$ P_{BEST}^R$$이 세 개의 알고리즘에서 일치했지만 급격한 커브 구간에서 [Fig. 4(c)] 상황으로 인해 [5]의 알고리즘에서 일반적인 Correspondence와 일치하지 않는 점들이 존재했다. 반면에 우리의 알고리즘은 일반적인 Correspondence와 모두 일치했다. [Fig. 6]는 우리 알고리즘의 각 Correspondence 단계에서 중간 포인트 클라우드의 모든 점들이 $$ P_{BEST}^R$$를 찾기 위해 탐색한 기준 포인트 클라우드의 점의 개수를 나타낸 그래프이며, [Fig. 7]는 일반적인 Correspondence와 우리 알고리즘에서 위의 탐색 과정에 소요되는 시간을 비교한 그래프이다. 기준, 중간 포인트 클라우드의 점의 개수는 각각 1080개(n=1080)이다. 각 Correspondence 과정에서 일반적인 Correspondence의 경우, 중간 포인트 클라우드의 각 점에 대해 기준 포인트 클라우드의 모든 점을 탐색하면서 $$ P_{BEST}^R$$를 찾기 때문에 총 1080²=1,166,400개의 점을 탐색한다. 그러나 우리의 알고리즘은 80% 이상의 Correspondence 과정에서 22,000개 이하로 탐색했으며 일반적인 Correspondence보다 평균적으로 95% 이상 적게 탐색했다[Fig. 8][Table 3]. 시간 측면에서 비교하면, 일반적인 Correspondence는 각 Correspondence 과정이 평균적으로 584.01 ms가 걸리고 약 90%의 과정이 580 ms~590 ms에 해당하여 일정한 시간이 소요되었다[Table 3][Fig. 9]. 반면에 우리 알고리즘은 평균적으로 70.142 ms가 걸렸으며, 약 90%의 Correspondence 과정이 130 ms이하로 소요되며 안정적인 시간 분포를 보였다[Table 3][Fig. 9].

[Fig. 6]

The number of searched points in our algorithm

[Fig. 7]

Time spent processing the correspondence in Naïve algorithm and our algorithm

[Fig. 8]

Histogram of the number of checking points in jump table algorithm (x-axis: [counts])

[Table 3]

Mean value of number of searched points and searching time in Naïve correspondence & our jump table algorithm

[Fig. 9]

Histogram of time spent processing the correspondence in Naïve algorithm and our algorithm (x-axis: [msec])

F1TENTH Simulator를 이용한 두 번째 실험에서는 $$ P_{START}^R$$가 0^th 또는 n^th에 가까울 때 n^th 또는 0^th로 $$ P_{NEXT}^R$$을 넘기지 못해 [5]의 알고리즘에서 일반적인 Correspondence와 일치하지 않는 점이 존재하는 반면 우리의 알고리즘에서는 모두 일치했다. 중간 포인트 클라우드의 모든 점들이 $$ P_{BEST}^R$$를 찾기 위해 탐색한 기준 포인트 클라우드의 점의 개수와 탐색하는 데에 걸리는 시간을 비교했을 때 첫번째 실험과 같은 경향을 보였다.