• 일단, addPointsAndSurfaces와 makePoseGraph 만 보면 됩니다.

1. 전체 코드 개요

1. Graph 클래스의 핵심 개념과 역할

• 그래프의 구성:
  그래프는 정점(Vertex)과 간선(Edge)의 집합
  • 이 구현은 메쉬의 구조나 변형 그래프를 관리하는 데 중점
  • 각 정점은 특정 위치를 나타내며, 간선은 정점 간의 연결 관계를 정의
• 그래프의 활용:
  • 이 그래프는 메쉬 데이터의 압축 및 최적화를 지원하며,
  • 정점과 간선 정보를 통해
    • 로봇의 메쉬 표현을 갱신하고,
    • 변형 메쉬를 추적
• 이 구현은 로봇의 메쉬 데이터를 실시간으로 압축, 통합 및 변형 추적하는 데 최적화

2. Graph의 주요 메소드와 로직 분석

• 정점 및 간선 추가 (addVertex, addEdge):
  • 정점이 이미 존재하는지 검사한 뒤, 중복되지 않으면 추가
  • 간선은 두 정점을 연결하며, 간선의 중복 여부도 검사
• 메쉬에서 그래프 생성 (createFromPclMesh):
  • 메쉬의 정점과 폴리곤 정보를 그래프 형태로 변환
  • 각 폴리곤을 순회하며 인접한 정점들을 간선으로 연결해 메쉬의 **구조를 그래프에 반영**
  • Bidirectional 버전:
    • 양방향 간선을 추가하여 정점 간의 양방향 탐색을 지원
• 새로운 정점과 면 추가 (addPointsAndSurfaces):
  • 새롭게 관측된 정점과 면을 그래프에 추가
  • 이때, 중복된 간선을 피하기 위해 검사한 후 새로운 간선만 추가
• 그래프 병합 (combineGraph):
  • 외부에서 입력된 그래프를 현재 그래프와 병합
  • 모든 정점과 간선을 추가하여 하나의 일관된 메쉬 구조로 만듭니다.

4. 이 알고리즘의 핵심 요소와 최적화 전략

• 메쉬 데이터의 그래프 표현:
  • 메쉬 데이터를 그래프 구조로 변환하면, 정점과 간선 기반의 탐색이 빠르고 효율적
  • 이는 특히 로봇의 실시간 변형 추적에 유리

2. addPointsAndSurfaces

1. 개요

• 메쉬 데이터를 그래프 형태로 변환
• 정점(vertices)과 면(polygons) 정보를 입력으로 받아
  • 이를 그래프의 정점과 간선으로 변환
• 정점과 면을 그래프 형태로 변환하는 목적
  • 다양한 탐색 및 최적화 작업을 수행할 수 있음
  • 메쉬 변형 추적
• 특히, 이 함수는 메쉬의 인접 정점들 간의 연결을 간선으로 표현
• new_edges를 출력 (새롭게 추가된 간선만)

2. 주요 알고리즘 단계

1. 정점 추가 (Vertices Integration):

   • 입력으로 주어진 정점 리스트에서 각 정점을 하나씩 그래프에 추가
   • 이미 존재하는 정점과 중복되지 않도록 확인한 후, 새 정점을 그래프에 삽입

2. 면 처리 및 간선 생성 (Surface Processing & Edge Creation):

   • 각 폴리곤(Polygon)은 정점의 집합으로 이루어져 있습니다.
   • 이를 순회하며 인접한 정점들 사이의 간선을 생성
   • 예를 들어, 폴리곤이 [v1, v2, v3]로 구성되어 있다면 (v1 -> v2), (v2 -> v3), (v3 -> v1)의 간선을 생성합니다.

3. 양방향 간선 생성 (Bidirectional Edge Handling):

4. 중복 간선 제거 (Duplicate Edge Prevention):

   • addEdge 함수는 간선이 이미 그래프에 존재하는지 체크한 후 중복을 피합니다.

5. 새로운 간선 저장 (Tracking New Edges):

   • 생성된 간선 중 새롭게 추가된 간선만 추적해 리스트에 저장
   • 이 리스트는 최종적으로 반환되며, 이후의 로직에서 새로 생성된 간선들을 활용할 수 있도록 합니다.

3. makePoseGraph

• makePoseGraph 함수는 메쉬 기반의 포즈 그래프(Pose Graph)를 생성하고 이를 새로운 정점들과 간선들로 업데이트하는 알고리즘

1. 포즈 그래프(Pose Graph)란?

• 포즈 그래프는 mesh의 위치(정점)와 그 위치 간의 변위 정보(edge)를 나타내는 그래프

2. 주요 알고리즘 단계

• 간선 데이터 삽입:
  • 주어진 간선 리스트(new_edges)를 순회하며 각 간선을 포즈 그래프에 추가
  • 간선은 두 개의 정점(from_node, to_node) 사이의 관계를 나타내며, 이를 통해 두 정점 간의 변위 정보를 저장
• 변위 계산:
  • 두 정점(from_node, to_node)의 위치는 주어진 포인트 클라우드(vertices)에서 가져와 변환 후 Eigen 벡터로 표현
  • p_from과 p_to는 각각 정점의 좌표를 나타내며, 이 둘의 차이가 곧 변위(Translation Vector)

2.3. 정점 정보 인코딩 (Encoding the Vertices)

• 정점 데이터 삽입:
  • 각 정점(new_indices)을 순회하며 mesh의 위치(포즈)를 그래프에 저장합니다.
  • 각 정점은 로봇의 ID, 정점의 키(key), 그리고 해당 정점의 3D 위치(pose)를 포함합니다.
  • 이때 위치 정보는 Eigen 라이브러리를 이용하여 포인트 클라우드 데이터를 3D 벡터로 변환하여 저장

2.4. 결과 반환

• 포즈 그래프를 완성한 후, 이를 반환합니다.