1. header

• 이 .h 파일은 ICP (Iterative Closest Point) 알고리즘을 통해 포인트 클라우드와 TSDF 맵을 정렬(또는 맞추는)하는 클래스를 정의
• 이 파일에서는
  • ICP의 작동 방식을 정의
  • 정렬을 위해 필요한 여러 가지 설정 및 기능을 제공

1. ICP::Config 구조체

• 역할: ICP 알고리즘의 설정값들을 저장
• 주요 설정:
  • refine_roll_pitch: 롤과 피치를 정밀하게 조정할지 여부.
  • mini_batch_size: 각 정렬 단계에서 사용할 점의 수.
  • min_match_ratio: 일치하는 점의 최소 비율.
  • subsample_keep_ratio: ICP 매칭에 사용할 점의 비율.
  • inital_translation_weighting: 초기 추정에서의 변환에 대한 가중치.
  • inital_rotation_weighting: 초기 추정에서의 회전에 대한 가중치.
  • num_threads: 사용할 스레드의 수.

2. ICP 클래스

• 역할: 포인트 클라우드와 TSDF 맵을 정렬하여 변환을 계산

• 주요 메소드:

  • ICP(const Config& config):

    • ICP 객체를 초기화하는 생성자
    • 설정값을 받아서 초기화

  • size_t runICP:

    • 역할:
      • 주어진 포인트 클라우드와 TSDF 맵 간의 최적 변환을 계산하는 메인 함수
    • 세부 사항:
      • 여러 스레드를 사용해 병렬 처리로 ICP를 수행하며, 최종적으로 정렬된 변환을 반환

  • runThread:

    • 역할:
      • runICP 함수에서 호출되어 각 스레드에서 변환을 계산
    • 세부 사항:
      • 포인트 클라우드의 일부분을 처리하며, 변환을 점진적으로 개선

  • matchPoints:

    • 역할:
      • 포인트 클라우드에서 TSDF 맵과 일치하는 점을 찾기
    • 세부 사항:
      • 주어진 변환을 사용하여 TSDF 맵에서 일치하는 점을 찾고,
        • 이 정보를 기반으로 포인트 클라우드와 TSDF 맵 간의 매칭을 생성

  • stepICP:

    • 역할: ICP 알고리즘의 한 단계를 수행
    • 세부 사항: 매칭된 점들에 대해 변환을 계산하고, 이를 통해 변환을 업데이트

  • getRotationFromMatchedPoints:

    • 역할: 주어진 매칭된 점들 간의 최적 회전을 계산
    • 세부 사항: 회전 행렬을 계산하기 위해 SVD(특이값 분해)를 사용

  • getTransformFromMatchedPoints:

    • 역할: 주어진 매칭된 점들 간의 최적 변환(회전 및 이동)을 계산
    • 세부 사항: 매칭된 점들로부터 변환을 계산하고 이를 반환

  • addNormalizedPointInfo:

    • 역할: 점과 법선 벡터를 기반으로 정보 벡터를 업데이트
    • 세부 사항: 변환을 계산할 때 필요한 가중치를 제공

2. cc 파일

1. ICP::getTransformFromMatchedPoints 함수

• 이 함수는 두 점군 간의 변환을 계산합니다. 두 점군의 매칭된 점들(src와 tgt)이 주어지면, 이 점들 사이의 회전(R)과 이동(t)을 계산하여 최적의 변환(Transformation)을 찾습니다.

• 수학적 배경:

  • 각 점군의 중심을 계산한 후, 중심이 원점에 위치하도록 점들을 조정
  • 이 조정된 점들에 대해 SVD(특이값 분해)를 사용해 최적의 회전 행렬 ( R )을 구합니다.
  • 회전이 적용된 점군의 중심과 대상 점군의 중심 간의 차이를 통해 이동 벡터 ( t )를 구합니다.
  • 최종적으로 변환 행렬 ( T_{tgt_src} = [R, t] )를 반환합니다.

2. ICP::addNormalizedPointInfo 함수

• 주어진 점과 그 점에 대한 법선 벡터를 사용하여,
  • 정렬(또는 정합) 과정에서의 정보 벡터(info_vector)를 누적하는 역할
  • 이는 나중에 변환행렬(회전과 이동)을 계산하는 데 필요한 가중치를 제공
• 이 함수는 특히 점의 위치와 해당 점에서의 표면 법선(normal vector)을 사용하여
  • 변환의 회전과 평행이동 성분에 대한 정보 벡터(info_vector)를 수집
• 

3. ICP::matchPoints 함수

• 이 함수는 주어진 점군과 TSDF 맵에서 일치하는 점을 찾습니다.
• 각 점에 대해 TSDF 맵에서의 위치를 찾고, 그 위치에서의 거리와 법선 벡터(gradient)를 계산
  • 이를 통해 소스 점(src)과 대상 점(tgt)을 생성
• 

4. ICP::stepICP 함수

• 이 함수는 ICP 알고리즘의 한 단계를 수행

• 이전 단계에서 계산된 변환을 바탕으로 점군을 TSDF 맵에 맞추고, 그 결과를 기반으로 새로운 변환을 계산

• 수학적 배경:

  • 매칭된 점들을 통해 새로운 변환을 계산하고, 이를 이전 변환과 결합하여 더 정확한 변환을 만듭니다.
  • 이 과정은 반복적으로 수행되어 점군과 TSDF 맵이 점점 더 잘 맞아지게 됩니다.

5. ICP::runThread 함수

• 이 함수는 다중 스레드를 사용해 ICP 계산을 병렬 처리
• 각 스레드는 주어진 점군의 일부를 처리하여 변환을 계산하고, 이를 통해 전체 변환을 점진적으로 개선
• 수학적 배경:
  • 각 스레드는 독립적으로 점군의 일부를 처리하며, 전체 변환은 스레드 간의 결과를 통합하여 계산
  • 이 과정에서 변환 간의 가중치(weight)를 계산하여, 각 변환이 전체 변환에 미치는 영향을 조절

6. ICP::runICP 함수

• 이 함수는 전체 ICP 프로세스를 수행하는 메인 루틴
  • 초기 변환을 주어진 상태에서 시작하여,
    • 여러 스레드를 통해 반복적으로 점군을 TSDF 맵에 맞추는 과정을 거침
• 수학적 배경:
  • 점군을 무작위로 섞어서 알고리즘의 안정성을 높임
  • 각 스레드는 독립적으로 작업을 수행하며, 최종적으로 모든 스레드의 결과를 통합하여 최적의 변환을 계산합니다.
  • 이 최적의 변환이 점군을 TSDF 맵에 가장 잘 맞추는 변환으로 사용됩니다.