X-ICP: Localizabiltiy-Aware LiDAR Registration for Robust Localization in Extreme Environmnets

이 논문은 2024년 TRO에 나온 논문입니다.
취리히 연방공대의 Turcan Tuna님께서 쓰신 X-ICP입니다.
터널이나 동굴등 scan과 matching이 잘안되는 환경에서도 registration을 강건하게 할 수 있는 논문입니다.

---

현재 ICP 알고리즘의 한계점:

ICP 알고리즘은 4가지 error 주요 원인이 있습니다.

1. local minima에 빠질 수 있다.

2. 센서의 noise 와 bias에 예민하다.

3. 부정확한 transformation의 prior의 사용

4. optimization 환경에 의한 geometric constraint 부족

tunnel 같은 환경에서 이러한 오류가 많이 나기 때문에 이것을 보완하는 ICP가 필요합니다.

이것을 보완하기 위해 만든 알고리즘이 X-ICP입니다.

이 알고리즘은 2가지 sub-module을 가지고 있습니다.

1. Loc-Module: Loclaization이 가능한지 확인하는 Module

2. Opt-Module: optimization module

X-ICP contribution

• Point cloud registration 최적화 문제의 주축 방향에 대해 localization 가능성을 판단합니다.

• point correspondence에서 얻을 수 있는 정보를 활용하기 위한 방법을 개발하여 최적화에 활용

• 가능성 인식 ICP를 활용하여 degenerate 시나리오에서도 강건한 point cloud registration을 가능하게 합니다.

• 제안된 프레임워크의 효능이 SOTA를 달성합니다.

---

problem Formulation & Preliminaries

원래 point to plane ICP minimization problem 식은 다음과 같습니다.

이 식의 x를 [$r^T t^T$]로 정리를 하면 minimization 수식을 다시 정의 할 수 있습니다.

$Rp_i$ = $p_i + w \times p_i$ + 고차항 이고 고차항은 날릴 수 있습니다.

그렇게 정리를 하면 이렇게 정의됩니다.

여기서 정의된 A' 와 b'는 나중에도 쓰입니다.

---

System Overview

• Localizability Detection Module Overview

Hessian A'의 null space를 approximate하는 것이 Loc-Module입니다.
localizability vector $\eta$는 이것을 판단하는 vector입니다.
$\eta$ $\in$ {none, partial, full}로 정의할 수 있습니다.

information Analysis -> filtering -> categorization을 통해 vector를 정의하고 이것을 ICP에 적용할 수 있습니다.

• Localizability Aware Optimization Module Overview

vector의 category에 따라 optimization을 진행합니다.

initial guess는 localizabilty vector의 category에 따라 변화를 줍니다.
full일 경우 무조건적으로 변화를 주고 partial은 controlled manner로 변화를 주고 none은 변화를 주지 않습니다.

---

Localizability Detection Module

Information Analysis

• PCA

A'는 위치에 따라 component의 의미를 가집니다.

회전을 담당하는 $A'_{rr}$ matrix와 병진운동을 담당하는 $A'_{tt}$ matrix를 사용합니다.

이 matrix를 SVD를 통해 eigen vector를 얻습니다.

$V_t$은 병진 운동 eigen vector이고 $V_r$은 회전 운동 eigen vector입니다.

이것은 나중에 활용합니다.

• Information Pair Contribution

A를 정의하는 p(point)와 그 point의 normal vector n을 information pair로 정의합니다.
이 두개를 외적한 torque를 계산하고 이것이 registration의 rotation에 대한 information matrix가 됩니다.
normal vector들은 병진운동의 information matrix역할을 합니다.

• Localizability Contribution

아까 계산했던 eigen vector를 내적해주면 registration에 대한 기여도를 나타낼 수 있습니다.

이 contribution의 최댓값은 1이고 최소값은 0입니다.

Filtering

• Filtering Low Contribution

registration에 기여를 많이 하지 않는다면 contribution 값을 0으로 만들어줍니다.

• Filtering High Contribution

low contribution filtering 되지 않은 contribution vector를 모아 놓은 것을 combined localizability contribution vector라고 합니다.

기여도가 특히 큰 contribution vector를 모아 놓은 것을 Strong localizability contribution vector라고 합니다.

Categorization

• Localizability Parameters

κ₁: 완전한 localization 가능성을 정의하는 안전 임계값으로, 완전히 국소 가능하도록 하는 하한을 설정합니다.
κ₂: localization 가능성에서 부분 localization 가능성으로의 전환을 조절하며, 부분 국소 가능성의 상한을 나타냅니다.
κ₃: 환경이 희소하지만 중요한 정보를 포함하는 경우를 다루기 위한 최소 정보 임계값으로, 부분 localization 가능성과 localization 불가 가능성 간의 전환을 조절합니다.

• Parameter Choice

이 세 가지 임계값은 자연스러운 관계를 가집니다: κ₁ ≥ κ₂ > κ₃. 파라미터는 사용하는 ICP 알고리즘의 수렴 범위에 따라 설정되며, 설정 규칙은 다음과 같습니다:

κ₁: localization 가능성과 {부분 localization 가능성, localization 불가 가능성}의 경계를 설정하며, (거의) 임의로 높게 설정할 수 있습니다. 너무 높은 값을 설정하면 국소 가능성 탐지의 추가 계산 비용이 발생하지만, 부분 국소 가능성은 여전히 정보를 충분히 활용할 수 있습니다. 예를 들어 κ₁ = 250은 ICP 최적화를 최소 250개의 완벽하게 정렬된 쌍으로 실행하도록 합니다.
κ₂: 시스템의 강건성과 최적화기에 따라 선택되며, κ₁과 κ₃ 사이에 설정해야 합니다. 이는 부분 localization 가능성을 어떻게 처리할지 정의합니다.
κ₃: 부분 localization과 localization 불가 가능성의 경계를 설정합니다. 예를 들어 κ₃ = 35는 최소 35개의 샘플 쌍으로 제약된 ICP 최적화를 제어된 방식으로 실행할 수 있도록 합니다.

• Decision tree

Contribution vector의 값을 parameter와 비교하여 full, partial, none 3가지로 point를 결정할 수 있습니다.

---

Localizabiltiy Aware Optimization Module

Constraint Calculation

이 모듈에서는 ill-conditioned directions에서 최적화의 solution space를 제한하는 제약 조건을 정의합니다.
각 제약 조건 방향은 고유 벡터 $v_t$와 $v_r$로 나타내며, 제약조건은 아래와 같이 정의됩니다.

이 식은 3d 평면을 정의하며, 평면의 법선 벡터는 $v_o$이고 point는 $t_o,r_o$입니다.

• localizability category에 따른 constraint

$\eta$ = full일 경우

추가 제약 조건 없이 기본 최적화가 수행됩니다.

$\eta$ = none일 경우

방향이 localization이 불가능하기 때문에, $t_o,r_o$를 0으로 설정해 이 방향에서의 포즈 업데이트를 제한합니다.

$\eta$ = Partial일 경우

resampling을 통해 $t_o,r_o$를 계산하여 최적화를 합니다.

• Partial Localizability

Partial category에 속한 pair 중 L값이 큰 몇개를 다시 sampling하여 $t_o$와 $r_o$를 다시 계산하고 optimization을 진행합니다.

Constrained Optimization

계산된 t₀와 r₀ 값을 최적화 문제에 통합합니다. 이를 위해 제약 조건을 다음과 같이 6D 형태로 확장합니다: $Cx = d$

여기서 C는 제약 행렬이고 d는 제약 벡터입니다.

제약 조건이 추가된 최적화 문제는 다음과 같습니다.

라그랑주 multiplier를 통해 비제약 문제로 변환한 후 계산을 해주면 됩니다.

---

결론은 contribution을 계산을 하고 그것에 따라 categorization을 합니다.

강력한 contribution이 있는 point는 제약 조건 없이 registration을 하고

partial contribution이 있는 point는 $Cx =d$ 라는 제약조건을 가지고 registration을 합니다.

contribution이 none이면 optimization하지 않습니다.

이를 통해 point cloud를 잘 걸러서 challenging environment에서도 잘 registration할 수 있는 알고리즘입니다.

이렇게 정리하면 간단한데 수식이 너무 복잡해서 읽기 힘들었네요.. 허허허