1. processIncrementalMeshGraph

processIncrementalMeshGraph(*input.deformation_graph, timestamps_, unconnected_nodes_)

• 로봇이 움직이면서 수집한 메쉬(graph)(frontend 모듈의 deformation_graph)와 로봇의 위치 정보(pose graph)(backend의 factor_graph) 를 동기화하고 연결하는 역할
  • input.deformation_graph은 새로 수집한 mesh 정보를 담고 있는데, 이를 backend의 factor graph에 반영한다.
    • 어떻게 반영하냐면,
      • 새로 얻은 (input.deformation_graph의) mesh node 각각을 시간적으로 가장 가까운 (backend의 factor graph의) pose node를 찾아서 연결하는 방식으로 업데이트 한다.

함수의 핵심 로직

1. input 메쉬 데이터 정리 및 초기화

• 하나의 로봇의 데이터만 포함된다고 가정
• 노드와 엣지를 별도로 수집하여 저장할 준비를 한다.
• 메쉬에 포함된 엣지를 순회하면서, 각 엣지가 같은 로봇에 의해 수집된 것인지 확인
  • 다른 로봇 데이터가 혼재되어 있으면 경고를 출력
  • 유효한 엣지는 목록에 추가
• 메쉬에 포함된 노드(메쉬의 각 지점)를 확인하면서,
  • 같은 로봇의 노드인지 확인
  • 노드의 위치와 타임스탬프를 저장하고, 중복된 노드는 무시

---

2. addNewMeshEdgesAndNodes

• 수집한 엣지와 노드들(mesh 정보)을 backend의 변형 그래프(deformation graph)에 추가
  • 추가할 때, edge에 noise 모델을 설정한다.
• https://velog.io/@jk01019/Backend-kimera-PGMO-code-kimerapgmosrcdeformationgraph.cpp

3. Pose 노드-포인트 연결 및 최적화 개념 정리

• 메쉬 포인트와 포즈 노드를 연결함으로써, 최적화 시 로봇의 경로와 환경의 변형된 상태를 정확하게 반영할 수 있다.
• 연결되지 않은 데이터가 있을 경우 이를 파악해 경고 메시지를 출력

2. 세부 설명

2.1 메쉬 포인트와 포즈 노드의 매칭 과정

• unconnectednodes:
  아직 메쉬와 연결되지 않은 로봇의 경로 포즈(노드)들을 저장하는 대기열(큐)
  • 메쉬 포인트를 해당 포즈 노드와 시간적으로 가장 근접하게 연결하는 것이 목표
  • 메쉬 포인트의 타임스탬프(timestamp)와 포즈 노드의 타임스탬프를 비교하여 시간 차이를 최소화하는 노드를 찾는다.

2.2. 매칭 정보 저장: node_valences 맵

• node_valences:
  각 로봇 포즈 노드(시간적 위치)와 연결된 메쉬 포인트들을 매칭해 저장하는 데이터 구조
  • Key: 각 포즈 노드의 ID
  • Value: 해당 포즈 노드에 연결된 메쉬 포인트들의 리스트

3. Deformation Graph에 연결 추가

• 각 포즈 노드와 연결된 메쉬 포인트들을 backend의 Deformation Graph(변형 그래프)에 반영

1. addNodeValence 메서드를 호출해, 각 포즈 노드와 연결된 메쉬 포인트를 그래프에 추가
   • https://velog.io/@jk01019/Backend-kimera-PGMO-code-kimerapgmosrcdeformationgraph.cpp
   • 이때, 포즈와 메쉬 포인트 간의 변형 관계가 정의되고, 그래프에 반영된다.

---

3. processIncrementalPoseGraph

processIncrementalPoseGraph(*msg, trajectory_, timestamps_, unconnected_nodes_);

• 로봇의 이동 경로(node와 edge)를 정확하게 deformationgraph 에 기록
• 추가적으로 loop closure 정보도 deformationgraph 에 기록
• 들어가기전에 이 글 꼭 읽어라
  • https://velog.io/@jk01019/Backend-hydra-용어-정리-sparse-Keyframe-unconnectednodes

핵심만 요약

• frontend output 중 poses 를 처리
• Sparse Keyframe에 새로운 pose 정보를 node/edge로 반영
  • 이는 deformation graph에 반영됨
• 위 과정에서, loop closure이 감지되면 변수에 저장

---

1. 첫 번째 노드 초기화

• 처음으로 deformation_graph_에 포즈 그래프를 생성할 때는 첫 번째 노드를 초기화해야 합니다.

2. 초기화 작업 수행:
   • input의 첫 번째 노드의 포즈 정보를 가져옵니다.
   • 변형 그래프(deformation graph)에 이 포즈를 추가합니다.
   • 희소 프레임(Sparse Keyframe)을 생성하여 노드와 메쉬의 연결을 관리할 준비를 합니다.
   • 초기 경로(initial_trajectory)와 타임스탬프(node_timestamps)에 해당 정보를 저장합니다.
   • 나중에 메쉬 포인트와 연결하기 위해 이 노드를 연결되지 않은 노드 큐(unconnected_nodes)에 추가합니다.

---

2. 포즈 그래프 엣지 처리

로봇의 이동 정보는 포즈 그래프의 엣지로 표현됩니다. 각 엣지는 이전 노드와 현재 노드 사이의 관계를 나타냅니다.

3.1 엣지 정보 추출

• 각 엣지(pg_edge)에 대해 다음 정보를 가져옵니다:
  • 측정된 포즈 변환(measure)
  • 이전 노드(prev_node)와 현재 노드(current_node)
  • 로봇의 ID (robot_from, robot_to)

3.2 엣지 타입에 따른 처리

• Odometry 엣지 (로봇의 연속적인 이동) 이면:
  • 두 노드가 같은 로봇의 것인지 확인합니다. 그렇지 않으면 오류입니다.
  • 이전 노드가 존재하지 않으면 에러를 출력하고 처리를 중단합니다.
  • 이미 처리된 엣지인 경우 경고를 출력하고 다음 엣지로 넘어갑니다.
• 루프 클로저 엣지 이면:
  • 이후 5. 루프 클로저 처리에서 자세히 다룹니다.

---

4. 희소 프레임(Sparse Keyframe) 관리

• 희소 프레임은 모든 포즈를 저장하는 대신 중요한 포즈만 선택적으로 저장하여 메모리 사용을 줄이고 연산 효율을 높이는 방법입니다.
  • https://velog.io/@jk01019/Backend-hydra-용어-정리-sparse-Keyframe

4.1 희소 프레임에 엣지 추가

• 현재 희소 프레임에 새로운 엣지를 추가할 수 있는지 확인합니다.
• 조건에 맞으면 기존 희소 프레임에 엣지를 추가하고, 그렇지 않으면 새로운 희소 프레임을 생성합니다.

4.2 새로운 희소 프레임 생성

• 새로운 희소 프레임을 생성하고 초기화합니다.
• 희소 프레임 맵(sparse_frames_)에 추가합니다.
• 해당 노드와 타임스탬프를 저장하고, 경로에도 추가합니다.
• 이 노드를 연결되지 않은 노드 큐에 추가하여 메쉬와의 연결을 대기합니다.

---

5. 루프 클로저 처리

루프 클로저는 로봇이 이전에 방문했던 위치를 다시 방문했을 때 발생합니다. 이를 정확히 처리하면 경로의 누적 오차를 보정할 수 있습니다.

5.2 루프 클로저 처리

• 엣지의 타입이 루프 클로저인지 확인합니다.
• 두 노드 사이의 변환 관계를 계산합니다.
• 이 변환을 변형 그래프에 추가하여 경로 최적화에 반영합니다.
• 이미 처리된 루프 클로저인지 확인하고, 중복 처리를 방지합니다.
• 루프 클로저 정보를 로그에 기록하고, 총 루프 클로저 수를 증가시킵니다.

---

6. 에러 처리와 최종 결과 반환

• 처리 과정에서 예외가 발생하면 에러 메시지를 출력하고 적절한 상태를 반환합니다.
• 모든 처리가 성공적으로 완료되면 ProcessPoseGraphStatus::SUCCESS를 반환하여 다음 단계로 진행합니다.

---