1. 클래스 설명

• 이 클래스 GradICPOdometryProvider는 두 점 클라우드(Pointclouds) 사이의 상대적인 변환(강체 변환(회전 및 이동))을 계산하는 방법으로,
  • GradICP (Gradient-based Iterative Closest Point) 알고리즘을 사용
  • 이는 점 클라우드 데이터에서 중요한 객체 추적 및 위치 추정 작업에 사용

1.1. 클래스의 주요 개념

1. GradICP 알고리즘:

   • GradICP는 일반적인 ICP (Iterative Closest Point) 알고리즘의 변형
   • ICP는 두 점 클라우드를 정렬하기 위해 반복적으로 가장 가까운 점을 찾고, 이를 통해 최적의 변환을 계산
   • GradICP는 이 과정에서 Gradient-based Levenberg-Marquardt(gradLM) 최적화 방법을 사용
     • 이는 더 효율적이고 안정적인 수렴을 보장

2. Levenberg-Marquardt(gradLM) 최적화:

   • gradLM은 비선형 최소자승 문제를 해결하기 위한 방법
     • 이 방법은 일반적인 선형 최소자승 방법과 비선형 최적화의 장점을 결합
   • gradLM은 각 반복(iteration)마다 댐핑 계수와 잔차를 조정하여 최적의 변환을 찾아냄

1.2. 클래스의 구성 요소

1. 초기화 메서드 (__init__):

   • numiters: 최적화 반복 횟수
   • damp: 초기 댐핑 계수
   • dist_thresh: 거리 임계값
   • lambda_max, B, B2, nu: gradLM 알고리즘의 매개변수

2. 변환 계산 메서드 (provide):

   • maps_pointclouds: 기준이 되는 점 클라우드
   • frames_pointclouds: 정렬해야 하는 점 클라우드
   • 이 메서드는 GradICP를 사용하여 두 점 클라우드 사이의 변환을 계산

2.

2.1. 개요

• 소스 점 클라우드(src_pc)를 타겟 점 클라우드(tgt_pc)와 맞추는(정렬하는) 것입니다.
• point_to_plane_gradICP 함수는 점 대 평면(Point-to-Plane) 오류 메트릭을 사용하여 두 점 클라우드 사이의 강체 변환을 계산하는 알고리즘
  • 이 알고리즘은 gradLM 방법을 사용하여 반복적으로 최적의 변환 행렬을 찾음
• gradLM 방법은 Levenberg-Marquardt(LM) 알고리즘의 개선된 버전으로, 보다 안정적이고 빠른 수렴을 위해 설계

2.2. 알고리즘 단계

잔차

잔차는 현재 변환된 소스 점 클라우드와 타겟 점 클라우드 사이의 차이를 나타내요. 잔차가 작을수록 두 점 클라우드가 잘 맞춰진다는 의미에요.

1. 초기화:

   • src_pc는 이동시켜야 하는 점 클라우드이고, tgt_pc는 기준이 되는 점 클라우드에요.
   • tgt_normals는 타겟 점 클라우드의 각 점에서 표면이 어떻게 생겼는지(법선 벡터)를 나타내요.
   • initial_transform은 처음에 적용할 변환 행렬이에요. 주어지지 않으면 기본값인 단위 행렬(아무 변환도 없는 상태)로 시작해요.

2. 반복(iteration) 과정:

   • 주어진 횟수(numiters)만큼 반복해서 최적의 변환을 찾아요.
   • 각 반복에서 현재 변환 행렬(transform)을 사용하여 소스 점 클라우드를 변환해요.

3. 가우스-뉴턴 방법을 사용한 선형 시스템 형성 및 잔차 계산:

   • 변환된 소스 점 클라우드를 사용하여 현재 상태에서의 오차를 계산 - 이 과정에서 gauss_newton_solve 함수는
     • 선형 시스템 행렬(A)과 잔차 벡터(b), 그리고
     • 소스 점 클라우드의 각 점에 대해 가장 가까운 타겟 점 클라우드의 점 인덱스(chamfer_indices)를 반환

4. 선형 시스템 해결 및 변환 계산:

   • 잔차를 줄이기 위해 선형 시스템을 해결하여 변환(residual_transform)을 계산
   • 이 변환을 현재 변환 행렬에 적용하여 소스 점 클라우드를 업데이트
   • 잔차와 새로운 잔차를 비교하여 변환이 개선되었는지 확인

5. 그라디언트 업데이트:

   • gradLM 방법에 따라 그라디언트를 사용하여 댐핑 계수(damp)를 업데이트
   • 오류 차이(errdiff)를 사용하여 새로운 댐핑 계수를 계산하고, 이를 통해 잔차 변환을 조정
   • 최종적으로 소스 점 클라우드에 변환을 적용하여 새로운 변환 행렬을 갱신