ICPOdometryProvider

About_work·2024년 8월 4일

global mapper

목록 보기

30/37

ICPOdometryProvider 클래스는
- Iterative Closest Point (ICP) 알고리즘을 사용하여
- 두 포인트클라우드(Pointclouds) 객체 간의 상대적인 변환을 계산하는 역할
이 클래스는 특히 point-to-plane 오류 메트릭을 사용하여 포인트클라우드 정합을 수행
아래는 이 클래스의 목적과 방법에 대한 자세한 설명입니다.

1. 목적:

목적:
- 두 포인트클라우드 객체(맵 포인트클라우드와 프레임 포인트클라우드) 간의 상대적인 변환 행렬을 계산하여,
- 라이브 프레임의 포인트를 맵 포인트와 정렬시키는 것.
주요 사용 사례:
- SLAM 시스템에서 현재 프레임의 위치 및 자세를 추정하는 데 사용

주요 구성 요소:

초기화 (__init__ 메서드):
- numiters:
  - ICP 최적화를 수행할 반복 횟수
  - 기본값은 20
- damp:
  - 비선형 최소 제곱 문제를 위한 댐핑 계수
  - 기본값은 1e-8
- dist_thresh:
  - 소스 포인트클라우드와 타겟 포인트클라우드 간의 거리가 이 값보다 큰 경우
  - 해당 포인트를 제거하기 위한 거리 임계값
  - 기본값은 None
변환 계산 (provide 메서드):
- maps_pointclouds:
  - 맵 포인트클라우드를 포함하는 Pointclouds 객체
- frames_pointclouds:
  - 라이브 프레임 포인트클라우드를 포함하는 Pointclouds 객체
- point_to_plane_ICP 함수를 사용하여 각 배치에 대해 상대적인 변환을 계산

동작 방식:

입력 검증:
- maps_pointclouds와 frames_pointclouds가 Pointclouds 타입인지 확인
- maps_pointclouds에 노멀 벡터가 포함되어 있는지 확인
- 두 포인트클라우드 객체의 배치 크기가 동일한지 확인
초기 변환 설정:
- initial_transform으로 4x4 단위 행렬을 설정합니다. 이는 초기 변환 행렬로 사용됩니다.
ICP 알고리즘 수행:
- 각 배치에 대해 point_to_plane_ICP 함수를 호출하여, 상대적인 변환 행렬을 계산
- point_to_plane_ICP 함수는 다음과 같은 인자를 사용:
  - 프레임 포인트클라우드의 포인트들
  - 맵 포인트클라우드의 포인트들
  - 맵 포인트클라우드의 노멀 벡터들
  - 초기 변환 행렬
  - 반복 횟수(numiters), 댐핑 계수(damp), 거리 임계값(dist_thresh)
결과 반환:
- 최종적으로 상대적인 변환 행렬을 반환합니다.

예시:

다음은 ICPOdometryProvider 클래스의 사용 예시입니다:

icp_provider = ICPOdometryProvider(numiters=30, damp=1e-8, dist_thresh=0.1)
maps_pointclouds = Pointclouds(points=[...], normals=[...])
frames_pointclouds = Pointclouds(points=[...])

relative_transformation = icp_provider.provide(maps_pointclouds, frames_pointclouds)
print(relative_transformation)

이 예시에서는 ICP 알고리즘을 사용하여 맵 포인트클라우드와 프레임 포인트클라우드 간의 상대적인 변환 행렬을 계산
이 변환 행렬은 맵 포인트클라우드와 프레임 포인트클라우드를 정렬하는 데 사용될 수 있음

2. point-to-plane ?

포인트-플레인(point-to-plane) 오류 메트릭을 사용하여 포인트 클라우드 사이의 강체 변환(rigid transformation)을 계산하는 과정

2.1. 용어 설명

강체 변환(Rigid Transformation): 점들을 이동하고 회전시키는 과정입니다. 예를 들어, 하나의 포인트 클라우드를 다른 포인트 클라우드에 맞추기 위해서 사용됩니다.
포인트-플레인(Point-to-Plane) 오류 메트릭:

각 점이 목표 점 클라우드에서 대응하는 점의 평면에 얼마나 가까운지 측정
목표 점 클라우드의 평면은 그 점의 노멀(수직 벡터, tgt_normals)로 정의

2.2. tgt_normals (목표 점 클라우드의 노멀 벡터)란?

노멀 벡터(Normal Vector):
- 점의 표면이 어느 방향을 향하고 있는지를 나타내는 벡터
- 예를 들어, 평평한 책상 위의 점들은 모두 위쪽을 향하는 노멀 벡터를 가질 것
- https://medium.com/@daekwanko123/point-clound-estimation-normal-vector-eb61ab8f434e

2.3. 알고리즘의 과정

초기화:
- 처음에는 소스 점 클라우드를 그대로 둡니다. 즉, 초기 변환 행렬은 단위 행렬(아무 변환도 하지 않음)입니다.
반복(iteration) 과정:
- 여러 번 반복하면서 소스 점 클라우드를 목표 점 클라우드에 맞추는 변환을 점점 더 잘 찾아갑니다.
1. 가장 가까운 점 찾기:
  - 소스 점 클라우드의 각 점에 대해 목표 점 클라우드에서 가장 가까운 점을 찾습니다.
2. 오차 계산:
  - 각 소스 점과 대응하는 목표 점 사이의 거리를 계산합니다.
  - 하지만, 단순한 점 사이의 거리가 아니라, 소스 점에서 목표 점의 평면까지의 거리를 계산합니다.
  - 이 평면은 목표 점의 노멀 벡터로 정의됩니다.
    - 예를 들어, 소스 점이 목표 점의 평면에서 멀리 떨어져 있다면, 큰 오차를 갖게 됩니다.
3. 변환 계산:
  - 오차를 줄이기 위해 소스 점 클라우드를 어떻게 회전하고 이동할지 계산합니다. 이 과정은 반복해서 더 나은 변환을 찾기 위해 여러 번 실행됩니다.
4. 변환 적용:
  - 계산된 변환을 소스 점 클라우드에 적용합니다.
5. 오차 비교:
  - 변환을 적용한 후의 오차가 이전보다 작아졌는지 확인합니다. 작아졌다면 변환을 유지하고, 그렇지 않다면 다른 변환을 시도합니다.

2.4. 최종 결과

소스 점 클라우드를 목표 점 클라우드에 맞추는 변환 행렬을 찾게 됩니다.
이 변환 행렬을 소스 점 클라우드에 적용하면 두 점 클라우드가 잘 맞게 됩니다.

3. point_to_plane_ICP

3.0. 요약:

point_to_plane_ICP 함수는 두 개의 3D 포인트 클라우드 간의 최적 강체 변환을 계산하는 데 사용
이 과정에서 point-to-plane 오류 메트릭과 Levenberg-Marquardt 알고리즘을 적용하여,
- 소스 포인트 클라우드(live_frame)를 타겟 포인트 클라우드(prev_frame)에 맞추어 최적화
최종적으로 계산된 변환 행렬은 소스 클라우드를 타겟 클라우드에 일치시키는 데 사용되며,
- 변환 과정에서 필터링된 포인트들의 매칭 인덱스도 함께 반환

3.1. 세부 설명:

3.2.1. 입력 인자:

src_pc (torch.Tensor):
- 변환이 필요한 소스 포인트 클라우드(current_frame)입니다.
- 이 포인트 클라우드는 형태가 (1, N_s, 3)으로, 3차원 좌표를 가진 N_s개의 포인트를 포함합니다.
tgt_pc (torch.Tensor):
- 소스 포인트 클라우드가 정렬될 타겟 포인트 클라우드(prev_frame)입니다.
- 이 클라우드도 형태가 (1, N_t, 3)로, 3차원 좌표를 가진 N_t개의 포인트를 포함합니다.
tgt_normals (torch.Tensor):
- 타겟 포인트 클라우드의 각 포인트에 대한 법선 벡터(normal vector)를 포함
- 이 벡터들은 (1, N_t, 3)의 형태를 가지며, point-to-plane 메트릭을 계산하는 데 사용
- TODO: pointcloud의 normal vector은 어떻게 구하나?
initial_transform (torch.Tensor or None):
- 소스 포인트 클라우드(current_frame)를 초기 정렬시키는 변환 행렬
- 만약 주어지지 않으면(기본값은 None), 단위 행렬(identity matrix)이 사용됩니다. 이 행렬은 형태가 (4, 4)입니다.
numiters (int):
- 최적화를 수행할 반복(iteration) 횟수
- 기본값은 20
damp (float):
- Levenberg-Marquardt 알고리즘에서 사용되는 댐핑 계수
  - 이 값은 변환의 안정성을 조정하며, 기본값은 1e-8
dist_thresh (float or int or None):
- 두 포인트 클라우드 간의 거리 임계값
- 이 임계값보다 큰 소스 포인트(current_frame)는 필터링되어 변환에서 제외
  - 기본값은 None으로, 필터링을 수행하지 않습니다.

과정:

초기화 단계:
- initial_transform을 통해 초기 변환된 소스 포인트 클라우드는 (1, N_s, 3)의 형태로, 이후 반복 과정에서 타겟 포인트 클라우드와 정렬될 것입니다.
반복 최적화 과정:
- 함수는 최적화 과정에서 numiters번의 반복을 수행
- 각 반복(iteration)마다 다음 단계들이 진행
1. 선형 시스템 구성:
  - gauss_newton_solve 함수를 호출하여, 현재 소스 포인트 클라우드와 타겟 포인트 클라우드 간의 선형 시스템을 구성
  - 이 선형 시스템은 A 행렬과 b 벡터로 표현되며, 각각 변환 매개변수를 계산하기 위한 방정식의 계수와 잔차를 나타냅니다.
2. 선형 시스템 풀기:
  - 선형 시스템을 풀어 변환 매개변수 xi를 구합니다.
  - 이 과정에서 Levenberg-Marquardt 알고리즘을 사용하여 댐핑 계수 damp를 적용합니다.
3. 변환 행렬 업데이트:
  - xi를 이용해 변환 행렬을 지수 함수(exponential) 형태로 계산하고, 이를 기존의 변환 행렬에 적용하여 업데이트합니다.
4. 오류 계산:
  - 현재 변환된 소스 포인트 클라우드와 타겟 포인트 클라우드 간의 오류를 계산합니다.
  - 이 오류는 최적화의 수렴성을 판단하는 데 사용됩니다.
5. Lookahead 최적화:
  - 변환된 소스 포인트 클라우드를 이용해 "미리 보기" 방식으로 한 단계 더 나아가 최적화를 수행합니다.
  - 이 과정에서 한 단계 더 나아간 변환이 현재보다 더 나은지를 평가하여,
    - 필요하면 변환을 업데이트하고 댐핑 계수를 조정합니다.
최종 결과 반환:
- 최적화가 완료되면,
  - 최종 변환 행렬과
  - 소스 포인트 클라우드의 각 포인트에 대해
    - 타겟 포인트 클라우드에서 가장 가까운 포인트의 인덱스를 반환

출력:

transform (torch.Tensor):
- 최종 계산된 변환 행렬로, 형태는 (4, 4)
- 이 행렬은 소스 포인트 클라우드를 타겟 포인트 클라우드에 정렬하는 데 사용
chamfer_indices (torch.Tensor):
- 소스 포인트 클라우드의 각 포인트에 대해, 타겟 포인트 클라우드에서 가장 가까운 포인트의 인덱스를 포함하는 텐서입니다. 이 텐서는 필터링된 포인트들에 대해서만 반환되며, 형태는 (1, N_sf)입니다.

4. gauss_newton_solve

`gauss_newton_solve` 함수 설명

목적:

gauss_newton_solve 함수는 Gauss-Newton 방법을 사용하여
- 두 개의 3D 포인트 클라우드 간의 변환을 계산하기 위해
  - 필요한 선형 시스템을 구성하는 것을 목적으로 합니다.
이 선형 시스템은
- 소스 포인트 클라우드(src_pc)를 타겟 포인트 클라우드(tgt_pc)에 맞추기 위한 최적의 강체 변환을 찾는 과정에서 사용
함수는 주어진 거리 임계값(dist_thresh)을 사용하여 타겟 포인트 클라우드와 너무 먼 소스 포인트들을 필터링합니다.

내용:

1. Gauss-Newton 방법 개요:

Gauss-Newton 방법은 비선형 최소제곱 문제를 풀기 위한 방법 중 하나입니다. 이 방법은 비선형 함수를 선형 근사화하여, 반복적으로 선형 시스템을 풀어 최적의 해를 구합니다. 이 함수는 이러한 Gauss-Newton 방법을 사용하여 두 포인트 클라우드 사이의 변환 매개변수를 추정하는 데 사용됩니다.

2. 입력 인자:

src_pc (torch.Tensor):
- 변환이 필요한 소스 포인트 클라우드입니다. 형태는 (1, N_s, 3)으로, N_s개의 3차원 포인트를 포함합니다.
tgt_pc (torch.Tensor):
- 소스 포인트 클라우드가 정렬될 타겟 포인트 클라우드입니다. 형태는 (1, N_t, 3)으로, N_t개의 3차원 포인트를 포함합니다.
tgt_normals (torch.Tensor):
- 타겟 포인트 클라우드의 각 포인트에 대한 법선 벡터입니다. 형태는 (1, N_t, 3)으로, 각 포인트에 대응하는 3차원 법선 벡터를 포함합니다.
dist_thresh (float or int or None):
- 소스 포인트와 타겟 포인트 간의 거리 임계값입니다. 이 임계값보다 큰 거리를 가진 소스 포인트들은 필터링됩니다. 기본값은 None으로, 필터링을 수행하지 않습니다.

3. 과정 설명:

입력 데이터 검증:
- 함수는 입력 데이터(src_pc, tgt_pc, tgt_normals)의 타입과 형상을 검증하여, 데이터가 올바른 형태로 제공되었는지 확인합니다. 데이터의 차원(ndim)과 크기(shape)가 예상과 다를 경우 오류를 발생시킵니다.
거리 기반 필터링:
- knn_points 함수를 사용하여, 소스 포인트 클라우드의 각 포인트에 대해 타겟 포인트 클라우드에서 가장 가까운 포인트를 찾습니다. 이 과정에서 계산된 거리가 dist_thresh보다 크면 해당 포인트는 필터링됩니다.
- 필터링된 후, 가장 가까운 포인트들의 인덱스를 chamfer_indices로 저장합니다.
선형 시스템 구성:
- 각 소스 포인트에 대해 타겟 포인트 클라우드에서 가장 가까운 포인트의 좌표(dx, dy, dz)와 그에 대응하는 법선 벡터(nx, ny, nz)를 계산합니다.
- 이어서, 각 소스 포인트와 가장 가까운 타겟 포인트 간의 차이를 계산하고, 이를 통해 선형 시스템의 계수 행렬 A와 잔차 벡터 b를 구성합니다.

4. 수식적 설명:

목표:
- 우리는 소스 포인트 클라우드의 각 포인트 ( \mathbf{p}_i = (x_i, y_i, z_i) )를 타겟 포인트 클라우드에 맞추기 위한 최적의 변환을 찾고자 합니다. 이 때, 포인트 간의 거리를 최소화하는 것이 목표입니다.
- 목표 함수는 다음과 같습니다:
  [
  \text{minimize } \sum_{i=1}^{N_sf} \left( \mathbf{n}_i^\top \left( \mathbf{T}(\mathbf{p}_i) - \mathbf{d}_i \right) \right)^2
  ]
  여기서 ( \mathbf{n}_i )는 타겟 포인트 클라우드의 법선 벡터이고, ( \mathbf{d}_i )는 타겟 포인트 클라우드에서 가장 가까운 포인트입니다. ( \mathbf{T}(\mathbf{p}_i) )는 현재 추정된 변환을 적용한 소스 포인트 클라우드입니다.
선형 시스템 구성:
- 소스 포인트 ( \mathbf{p}_i )에 대해, 우리는 다음과 같은 선형 시스템을 구성합니다:
  [
  \mathbf{A}_i = \left[ \mathbf{n}_i^\top, \mathbf{n}_i^\top \mathbf{S}_i \right]
  ]
  여기서 ( \mathbf{S}_i )는 소스 포인트 ( \mathbf{p}_i )의 교차곱 행렬입니다.
  [
  \mathbf{S}_i = \begin{bmatrix} 0 & -z_i & y_i \ z_i & 0 & -x_i \ -y_i & x_i & 0 \end{bmatrix}
  ]
- 잔차 벡터 ( b_i )는 다음과 같습니다:
  [
  b_i = \mathbf{n}_i^\top (\mathbf{d}_i - \mathbf{p}_i)
  ]
결과 반환:
- 함수는 최종적으로 구성된 선형 시스템의 계수 행렬 ( A ), 잔차 벡터 ( b ), 그리고 필터링된 포인트의 인덱스 ( \text{chamfer_indices} )를 반환합니다.

5. 출력:

A (torch.Tensor):
- 선형 시스템의 계수 행렬로, 형태는 (N_sf, 6)입니다. 여기서 N_sf는 필터링된 소스 포인트의 수입니다.
b (torch.Tensor):
- 선형 시스템의 잔차 벡터로, 형태는 (N_sf, 1)입니다.
chamfer_indices (torch.Tensor):
- 소스 포인트 클라우드의 각 포인트에 대해, 타겟 포인트 클라우드에서 가장 가까운 포인트의 인덱스를 포함하는 텐서입니다. 형태는 (1, N_sf)입니다.

요약:

gauss_newton_solve 함수는 Gauss-Newton 방법을 사용하여 두 포인트 클라우드 간의 최적 변환을 추정하기 위해 필요한 선형 시스템을 구성하는 역할을 합니다. 이 함수는 소스 포인트 클라우드의 각 포인트에 대해 타겟 포인트 클라우드에서 가장 가까운 포인트를 찾고, 이를 바탕으로 선형 시스템의 계수 행렬과 잔차 벡터를 계산하여 반환합니다. 이 과정은 최종적으로 두 포인트 클라우드 간의 변환을 최적화하는 데 사용됩니다.

About_work

새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.

이전 포스트

run_slam_rgb.py

다음 포스트

ICPOdometryProvider

global mapper

1. 목적:

주요 구성 요소:

동작 방식:

예시:

2. point-to-plane ?

2.1. 용어 설명

2.2. tgt_normals (목표 점 클라우드의 노멀 벡터)란?

2.3. 알고리즘의 과정

2.4. 최종 결과

3. point_to_plane_ICP

3.0. 요약:

3.1. 세부 설명:

3.2.1. 입력 인자:

과정:

출력:

4. gauss_newton_solve

`gauss_newton_solve` 함수 설명

목적:

내용:

1. Gauss-Newton 방법 개요:

2. 입력 인자:

3. 과정 설명:

4. 수식적 설명:

5. 출력:

요약:

run_slam_rgb.py

GradICPOdometryProvider

0개의 댓글

ICPOdometryProvider

global mapper

1. 목적:

주요 구성 요소:

동작 방식:

예시:

2. point-to-plane ?

2.1. 용어 설명

2.2. tgt_normals (목표 점 클라우드의 노멀 벡터)란?

2.3. 알고리즘의 과정

2.4. 최종 결과

3. point_to_plane_ICP

3.0. 요약:

3.1. 세부 설명:

3.2.1. 입력 인자:

과정:

출력:

4. gauss_newton_solve

gauss_newton_solve 함수 설명

목적:

내용:

1. Gauss-Newton 방법 개요:

2. 입력 인자:

3. 과정 설명:

4. 수식적 설명:

5. 출력:

요약:

run_slam_rgb.py

GradICPOdometryProvider

0개의 댓글

`gauss_newton_solve` 함수 설명