• https://voxblox.readthedocs.io/en/latest/api/classvoxblox_1_1ICP.html

클래스 ICP

파일 icp.h에 정의됨

---

중첩 관계

중첩 타입

• Struct ICP::Config

---

클래스 문서

클래스 ICP

• 기존 TSDF(Truncated Signed Distance Function) 정보와 포인트 클라우드를 정렬하여 정합하는
  • ICP(Iterative Closest Point) 방식으로 포인트 매칭을 수행하는 클래스
• 이 과정은 전통적인 ICP와 약간 다릅니다:
  • 1. "미니 배치"로 일부 포인트가 선택되고, 이들의 정렬에 대해 최소 제곱 오차를 최소화하는 변환을 찾습니다.
  • 2. 이 정렬에서 얻어진 "정보"(R/t 회전 추정의 신뢰도)를 추정하여, 이 정제된 변환을 초기 추정치와 융합
  • 3. 포인트 클라우드 내의 모든 포인트가 사용될 때까지 새 미니 배치로 이 과정을 반복
• 이 방식은 실제로 미니 배치 1개에 1번만 계산되며 (반복하지 않으며), kdtree를 구축하는 대신 TSDF 거리를 사용
  • 이러한 선택은 캡처 영역을 제한하며, 초기 추정치가 비교적 정확하다고 가정
    • 그러나 이러한 제한 사항 덕분에 correspondence estimation과 병렬 처리가 효율적으로 이루어져 실시간 성능을 제공

추가 설명

• 이 문장은 TSDF(Truncated Signed Distance Function) 정보를 사용하여 포인트 클라우드를 정렬하는 ICP(Iterative Closest Point) 방식의 변형을 설명하는 것
• 일반적인 ICP 방식과 비교해 몇 가지 차이점이 있음
• 이 ICP 변형 방식은 전통적인 ICP 방식과는 달리, 미니 배치를 사용하여 효율적으로 포인트 클라우드를 정렬
• k-d 트리 대신 TSDF 거리를 사용하며,
  • 초기 추정치의 정확성을 가정하고 빠르게 최적 변환을 계산하는 것이 특징
  • 이 방식은 효율적인 병렬 처리와 실시간 성능을 가능하게 합니다.

2. ICP와의 차이점

a) Mini Batch 사용

• 일반적인 ICP는 전체 포인트 클라우드에서 한 번에 작업하지만, 이 변형된 방식은 "미니 배치"라는 소규모 점 집합을 사용
• 미니 배치 선택:
  • 전체 포인트 클라우드에서 소규모 점 집합(예: 20개)을 선택
• 최적 변환 계산:
  • 선택된 이 미니 배치에 대해 최소 제곱 오차를 최소화하는 최적의 변환을 찾음

b) 정보 추정 및 융합

• 최적 변환을 찾은 후, 이 변환의 "정보"가 추정됨
• 여기서 "정보"는 R/t 변환의 신뢰도와 관련된 것으로, R/t 변환의 정확성이나 안정성
  • 이 정보는 이후의 과정에서 다음과 같이 사용됩니다:
• R/t 변환의 신뢰도와 초기 추정과의 융합:
  • 이 정보는 최초의 추정치와 결합되어 새로운, 보다 정밀한 변환을 만들어냅니다.
• 

c) TSDF 거리 사용

• 전통적인 ICP는 가장 가까운 점을 찾기 위해 보통 k-d 트리와 같은 공간 분할 구조를 사용
• 그러나 이 방식은 TSDF에서 직접 계산된 거리 정보를 사용
• k-d 트리 대신 TSDF:
  • TSDF는 각 점이 표면으로부터의 거리를 나타내므로, 대응점 간의 거리를 직접 계산할 수 있습니다.
  • 이 방식은 k-d 트리를 구축할 필요가 없어 계산 효율성을 높여줍니다.

3. 이 방법의 제한사항과 장점

3.a) 제한사항

• 캡처 영역 제한:
  • 이 방법은 k-d 트리를 사용하지 않고 TSDF 거리만을 이용하기 때문에, 초기 추정치가 비교적 정확해야 합니다.
  • 잘못된 초기 추정치가 있다면, 포인트 클라우드를 제대로 정렬하지 못할 가능성이 있습니다.
• 반복(iteration)의 부재:
  • 전통적인 ICP는 여러 번 반복하면서 정렬을 점진적으로 개선하지만, 이 방법은 각 미니 배치마다 새로운 최적의 변환을 찾고 이를 반복하지 않기 때문에 전통적인 의미의 반복(iteration)이 없습니다.

b) 장점

• 병렬 처리:
  • 미니 배치를 사용하여 독립적인 계산을 할 수 있기 때문에, 병렬 처리에 적합합니다.
  • 이는 실시간 성능을 요구하는 애플리케이션에서 매우 유리합니다.
• 효율성:
  • k-d 트리 대신 TSDF 거리를 사용함으로써 계산 시간을 줄일 수 있으며, 실시간으로 대응 추정을 할 수 있습니다.

---

공개 함수

• ICP(const Config &config)

  • 두 개의 인자를 받고 정수 값을 반환하는 일반적인 멤버 함수

  매개변수

  • config: 모든 관련 ICP 파라미터를 보유하는 구조체.

• size_t runICP(const Layer<TsdfVoxel> &tsdf_layer, const Pointcloud &points, const Transformation &initial_T_tsdf_sensor, Transformation *refined_T_tsdf_sensor, const unsigned seed = std::chrono::system_clock::now().time_since_epoch().count())

  • 포인트를 tsdf_layer와 정렬하기 위해 ICP 방법을 실행합니다.

  반환값

  • 성공한 미니 배치의 수를 반환합니다.

• bool refiningRollPitch()

구조체 Config

ICP 클래스를 설정하는 데 필요한 모든 정보를 포함합니다.

공개 멤버

1. bool refine_roll_pitch = false

• 설명: 이 값은 ICP 알고리즘이 회전의 특정 축에 대해 정밀하게 조정할 것인지 여부를 결정하는 플래그입니다.
  • 사용 예시:
    • 예를 들어, 로봇이 매우 평평한 지형에서 주행 중일 때는 Roll과 Pitch 회전의 정밀한 조정이 그다지 중요하지 않을 수 있습니다.
    • 이 경우, refine_roll_pitch를 false로 설정해 불필요한 계산을 줄일 수 있습니다.
    • 반면에, 지형이 기울어져 있거나 복잡한 환경에서는 더 정밀한 회전 조정이 필요할 수 있으므로, 이 플래그를 true로 설정해 정밀도를 높일 수 있습니다.

2. int mini_batch_size = 20

• 설명: 각 정렬 단계에서 사용되는 포인트의 수를 나타냅니다. 이는 ICP 프로세스가 대량의 포인트 클라우드를 작은 부분(미니 배치)으로 나누어 처리할 수 있도록 도와줍니다.

• 상세 설명:

  • 미니 배치(Mini Batch): 미니 배치는 전체 포인트 클라우드에서 일부 점들을 선택한 작은 그룹을 의미합니다. 대량의 데이터를 한 번에 처리하기보다는, 작은 배치로 나누어 처리하는 것이 효율적이고, 병렬 처리와 같은 최적화가 가능합니다.
  • 병렬 처리와 스레딩: ICP 알고리즘은 복잡하고 많은 계산을 요구합니다.
    • mini_batch_size를 설정함으로써 전체 포인트 클라우드를 여러 개의 미니 배치로 나누어 각각의 미니 배치에서 독립적인 정렬 작업을 수행할 수 있습니다.
    • 이러한 작업들은 스레드를 사용해 병렬로 실행할 수 있으며, 이로 인해 계산 속도가 크게 향상됩니다.
  • 결과의 결합:
    • 각 미니 배치에서 얻은 정렬 결과는 개별적으로는 작은 부분에 불과하지만, 전체 포인트 클라우드를 정렬하는 데 중요한 역할
    • 최종적으로, 각 미니 배치에서 얻은 정렬 결과들은 "정보의 추정치"에 기반한 가중치를 부여받아 결합
    • 이 과정은 개별 정렬 결과가 얼마나 신뢰할 수 있는지를 반영하여 전체 정렬 결과에 기여하게 됩니다.

3. FloatingPoint min_match_ratio = 0.8

• 설명: 이 값은 할당된 복셀(Voxel) 내에 포함되는 점(point)들이 일정 비율 이상 TSDF 거리 내에 있어야 한다는 것을 의미
• 상세 설명:
  • TSDF 거리:
    • TSDF(Truncated Signed Distance Function)는 공간 내의 각 점이 표면으로부터 얼마나 떨어져 있는지를 나타내는 함수
    • 양수 값은 표면 바깥, 음수 값은 표면 안쪽을 나타냅니다.
    • 절단 거리는 이 거리가 일정 값 이상일 때 그 값을 자르는(즉, 일정 값으로 고정하는) 임계값
  • min_match_ratio:
    • 이 매개변수는 ICP 과정에서 선택된 포인트 클라우드의 점들이, 할당된 복셀 내에서 TSDF 절단 거리 내에 있어야 하는 비율을 정의
    • 예를 들어, 이 값이 0.8로 설정되어 있다면, 각 복셀 내에서 80% 이상의 점이 TSDF 절단 거리 내에 있어야 합니다.
    • 이 조건을 충족하지 못하면 해당 점은 정합에서 제외됩니다. 이는 잘못된 점들이 정합에 참여하는 것을 방지하는 역할을 합니다.

4. FloatingPoint subsample_keep_ratio = 0.5

• 설명:
  • ICP 매칭에 사용될 포인트 클라우드의 점의 비율을 나타냄
  • 이 값은 원래 데이터에서 얼마나 많은 점을 사용하여 매칭할지를 결정
• 상세 설명:
  • subsample_keep_ratio: 이 값이 0.5로 설정된 경우, 원래 포인트 클라우드 데이터의 50%만 선택하여 ICP 매칭에 사용됩니다.

5. FloatingPoint initial_translation_weighting = 100.0

• 설명:
  • 초기 추정치에서 평행 이동(Translation) 성분에 적용되는 가중치를 의미
  • 이는 초기 추정치의 신뢰도와 관련이 있음

5.1. 기본 개념: 초기 추정치와 가중치

• 초기 추정치(Initial Guess)는 포인트 클라우드를 정렬할 때 처음에 가정하는 변환(회전 및 평행 이동)입니다.
• 이 초기 추정치는 일반적으로 이전의 위치 추정 결과나 다른 센서 데이터를 바탕으로 만들어집니다.
• 평행 이동(Translation)은 포인트 클라우드를 X, Y, Z 방향으로 움직이는 변환을 말합니다.
• 가중치(Weighting)는 이 초기 추정치의 신뢰도를 나타내는 값입니다. 이 값은 알고리즘이 초기 추정치를 얼마나 믿고 따를지를 결정합니다.

5.2. 수학적 표현

• 가중치를 수학적으로 설명하기 위해서는 가우스 분포(Gaussian distribution)와 공분산 행렬(Covariance Matrix)의 개념이 필요합니다.

5.3. 1. 공분산 행렬(Covariance Matrix)

• 공분산 행렬은 데이터의 분산과 변수들 간의 상관관계를 나타냅니다.
• 만약 우리가 3D 공간에서 포인트 클라우드의 위치를 나타낸다고 하면, 그 위치의 분산(즉, X, Y, Z 좌표 각각의 변동성)을 고려해야 합니다.
• 이때 공분산 행렬 (\Sigma)는 다음과 같은 형태를 가집니다:
• 

5.5. 초기 추정치가 정렬 과정에 미치는 영향

• 이제 이 가중치가 어떻게 작용하는지 이해해봅시다. 초기 추정치와 최종 정렬 변환 (T_{\text{final}}) 간의 관계는 다음과 같이 표현할 수 있습니다:
• 

6. FloatingPoint initial_rotation_weighting = 100.0

• 설명:

  • 초기 추정치에서 회전(Rotation) 성분에 적용되는 가중치를 의미합니다. 이는 초기 추정치의 회전 성분에 대한 신뢰도를 반영합니다.

• 상세 설명:

  • 회전(Rotation): 포인트 클라우드를 특정 축을 기준으로 회전시키는 변환입니다. 3D 공간에서 X, Y, Z 축을 중심으로 회전할 수 있습니다.
  • 가중치(Weighting): 이 가중치는 초기 추정치에서 회전 성분이 얼마나 중요한지를 결정합니다. 값이 크면 초기 추정치의 회전을 거의 그대로 유지하려고 하며, 값이 작으면 이후의 최적화 과정에서 회전이 더 많이 수정될 수 있습니다.
  • 초기 평행 이동 가중치와 유사한 원리로, 이 가중치도 초기 추정치의 회전 신뢰성을 나타냅니다. 회전에 대해 얼마나 신뢰할 수 있는지를 결정하는 기준이 됩니다.

7. size_t num_threads = std::thread::hardware_concurrency()

• 설명:
  • 사용 가능한 하드웨어 스레드 수를 반환하는 함수로, ICP 알고리즘의 병렬 처리를 위해 사용