• update 함수 보면 됨
• const auto packet = tracker_->update(input.timestamp_ns, input.world_T_body());

1. pose_graph_from_odom.cpp

• 이 코드는 로봇의 이동 경로를 오도메트리(odometry) 데이터를 기반으로 포즈 그래프로 생성하고 업데이트
• 그래프 형태로 구성하여 경로를 추적

1. 전체 코드의 동작 개요

• 포즈 그래프(Pose Graph):
  • 시간에 따른 로봇의 위치(포즈)와 그 사이의 변환 정보를 노드와 간선의 형태로 기록
• 오도메트리 기반 그래프 생성:
  • 두 위치(이전 포즈와 현재 포즈) 사이의 상대적인 변환을 간선으로 기록

makePoseGraph 함수

• 두 개의 스탬프된 포즈(curr_pose, prev_pose)를 이용해 포즈 그래프를 생성합니다.
• 노드와 간선을 추가하며, 로봇의 경로를 그래프 형태로 기록

세부 로직:

1. 그래프 생성:
   • 새로운 포즈 그래프 객체를 생성
2. 노드 추가:
   • 이전 포즈와 현재 포즈를 각각 노드로 그래프에 추가합니다.
   • 노드는 포즈의 시간, 로봇 ID, 노드의 키(key), 3D 위치(pose) 정보를 포함
3. 간선 추가:
   • 두 포즈 간의 변환(body_i_T_body_j)을 계산해 그래프의 간선으로 추가
   • 간선은 이전 노드와 현재 노드 사이의 상대적 변환을 나타내며, 이는 오도메트리 정보를 기반으로 함
4. 결과 반환: 생성된 그래프를 반환

addNode 함수

• 포즈 데이터를 기반으로 새로운 노드를 그래프에 추가합니다.
• 각 노드는 시간, 로봇 ID, 키(key), 포즈 정보로 구성됩니다.

addEdge 함수

• 두 포즈 사이의 상대적인 변환을 그래프의 간선으로 추가합니다.
• 간선은 이동 변환(오도메트리 정보)과 두 노드의 연결 정보를 포함합니다.
• 간선의 타입은 ODOM(오도메트리 변환)으로 설정됩니다.

update 함수

• 로봇의 현재 위치와 이전 위치를 바탕으로 포즈 그래프를 갱신합니다.
• 첫 번째 호출에서는 그래프를 생성하지 않고 포즈만 저장하며, 이후 호출부터 이전 포즈와 현재 포즈를 연결하는 그래프를 생성
• 갱신된 포즈 그래프를 포함한 PoseGraphPacket 형태를 반환

3. 로직의 핵심 흐름과 알고리즘

1. 포즈 수신 및 저장

   • 로봇의 새로운 위치가 수신될 때마다 타임스탬프와 3D 위치를 포함한 포즈 객체를 생성합니다.

2. 포즈 그래프 갱신 및 생성

   • 첫 번째 포즈 이후로는 이전 포즈와 현재 포즈 사이의 변환을 계산하고 이를 그래프로 추가합니다.

3. 포즈 간의 간선 연결

   • 두 포즈 사이의 상대적 변환(오도메트리 정보)은 그래프의 간선으로 저장됩니다.
   • 이 간선은 나중에 경로 최적화나 SLAM과 같은 후처리에서 중요한 역할을 합니다.

4. 멀티 노드 관리와 확장성

   • 각 노드는 고유한 키(key)를 가지며, 이 키는 노드와 간선 연결 시 참조됩니다.

2. include/hydra/odometry/pose_graph_tracker.h

1. PoseGraphPacket 구조체의 역할

• PoseGraphPacket은 로봇의 포즈 그래프 갱신 데이터를 포함하는 패킷

  • 이를 통해 매번 갱신 시점에 추가된 정보가 백엔드로 전달

• pose_graphs:

  • pose_graph_tools::PoseGraph의 포인터 배열로, 추적 중인 로봇의 포즈 그래프들을 포함합니다.
  • 여러 그래프를 관리할 수 있어 멀티로봇 환경이나 다양한 경로를 추적하는 데 유용합니다.

• external_priors:

  • 외부에서 제공되는 최적화된 포즈 정보입니다. 이 외부 정보는 포즈 그래프의 정합성을 개선하는 데 사용됩니다.
  • 예를 들어, SLAM이나 루프 클로저 탐지 후 제공된 외부 최적화 값을 포즈 그래프에 반영할 수 있습니다.