핵심요약! 이거만 보면돼

• updateFactorGraph 함수는 백엔드의 변형 그래프를 최신 데이터로 업데이트하는 핵심 역할을 합니다. 주요 동작은 다음과 같습니다:
• 1. 메쉬 그래프 처리: 프론트엔드에서 전달된 새로운 메쉬 데이터를 받아 변형 그래프에 통합하여 로봇의 포즈와 메쉬 간의 관계를 유지합니다.
• 2. 포즈 그래프 처리: 로봇의 새로운 경로 데이터를 변형 그래프에 반영하고, 루프 클로저가 발생했는지 감지하여 기록합니다.
• 3. 외부 위치 정보 반영: 외부에서 제공된 추가적인 위치 정보를 그래프에 적용하여 암묵적인 루프 클로저를 처리하고 그래프를 보강합니다.
• 4. 루프 클로저 상태 업데이트: 새로운 루프 클로저 발생 여부를 확인하고, 루프 클로저 수와 그래프의 상태 정보를 업데이트하여 이후의 최적화에 대비합니다.

---

2. updateFactorGraph

2. 함수의 주요 동작

2.2. 그래프 업데이트와 처리

1. 증분 메쉬 그래프 처리
   • 새로운 mesh 데이터를 backend의 deformation graph에 반영해 -> 로봇의 pose와 mesh 간의 관계를 유지
2. 로봇의 경로(포즈) 그래프 처리
   • 새로운 Pose 데이터를 backend의 deformation graph에 반영.
   • 만약 경로에서 루프 클로저(중복된 경로)가 탐지되면, 이를 인식해 기록
     • 이 함수에서 경로 optimize 까지 하는건 아님
3. 외부 위치 정보(External Priors) 반영
   • 만약 외부에서 제공된 추가적인 위치 정보가 있다면, 이를 반영해 그래프를 보강
   • 이로 인해 암묵적인 루프 클로저도 반영될 수 있어.

2.3. 루프 클로저 감지 및 상태 관리

• 새로운 루프 클로저 발생 여부를 확인
  • 만약 새로운 루프 클로저가 탐지되었다면, 이를 기록하고 다음 단계에서 사용할 수 있도록 준비
• 루프 클로저와 그래프에 대한 state(상태)를 업데이트해:
  • 루프 클로저 수와 전체 경로 길이 저장
  • 그래프의 노드와 엣지 수 저장

---

2.1. processIncrementalMeshGraph(*input.deformation_graph, timestamps_, unconnected_nodes_)

• 새로 얻은 mesh 데이터(frontend deformation_graph)를, backend의 factor graph에 반영
  • 이때, agent pose node와 mesh를 연결시킨다. (시간적 근접성을 기준으로 연결시킴)
• https://velog.io/@jk01019/Backend-kimera-PGMO-code-kimerapgmosrckimerapgmointerface.cpp

2.2. hasLoopClosure(*msg)

inline bool hasLoopClosure(const PoseGraph& graph) {
  for (const auto& edge : graph.edges) {
    if (edge.type == pose_graph_tools::PoseGraphEdge::Type::LOOPCLOSE) {
      return true;
    }
  }
  return false;
}

• msg: frontend output 중, pose graph
• 루프 클로저 탐지 로직:
  • input의 pose graph의 각 그래프의 엣지를 순회하며, 엣지 유형이 ‘LOOPCLOSE’인 경우를 찾는다.
  • 해당 유형의 엣지가 하나라도 발견되면, true를 반환해 루프 클로저가 존재한다고 판단한다.
• 발견되면 print를 한다. (별거 안하네? 왜 하지 이걸)

2.3. processIncrementalPoseGraph(*msg, trajectory_, timestamps_, unconnected_nodes_)

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

• https://velog.io/@jk01019/Backend-kimera-PGMO-code-kimerapgmosrckimerapgmointerface.cpp

2.4. logIncrementalLoopClosures

logIncrementalLoopClosures(*msg);

설명:

• 목적: 주어진 포즈 그래프 메시지(msg)에서 루프 클로저 엣지를 찾아서 loop_closures_ 리스트에 저장
• 동작:
  1. 엣지 순회:
     • 메시지의 모든 엣지를 순회하면서, 엣지의 타입이 LOOPCLOSE인지 확인합니다.
  2. 루프 클로저 처리:
     • 로봇의 프리픽스(prefix)를 가져와서 노드의 키를 생성합니다.
     • gtsam::Symbol을 사용하여 소스 노드(src_key)와 목적지 노드(dest_key)의 키를 생성
     • 포즈 변환(pose)을 가져옵니다.
     • 루프 클로저 정보를 loop_closures_ 리스트에 추가
• 참고 사항:
  • loop_closures_ 리스트: 루프 클로저 이벤트를 저장하여 나중에 경로 최적화에 사용

2.4. updateAgentNodeMeasurements(*input.agent_updates.external_priors)

전체적인 흐름과 연관 관계

• 외부 프라이어 처리 및 루프 클로저 적용:
  • 외부 프라이어가 제공되면 이를 변형 그래프에 업데이트하고, 루프 클로저가 발생한 것으로 간주
• 노드 측정값 업데이트 (updateAgentNodeMeasurements):
  • 외부에서 제공된 노드의 포즈 정보를 변형 그래프에 반영
  • 기존의 프라이어를 제거하고 새로운 측정값으로 대체

외부 프라이어 처리

if (input.agent_updates.external_priors) {
  updateAgentNodeMeasurements(*input.agent_updates.external_priors);
  // Think of it as "implicit" loop closures
  have_loopclosures_ = true;
  have_new_loopclosures_ = true;
}

설명:

• "암묵적" 루프 클로저: 외부 프라이어를 적용하는 것은 실제로 루프 클로저를 적용하는 것과 동일한 효과를 가집니다.
• 플래그 설정: have_loopclosures_ 및 have_new_loopclosures_를 true로 설정하여 루프 클로저 처리가 필요함을 나타냅니다.

---

3.2 updateAgentNodeMeasurements 함수

void BackendModule::updateAgentNodeMeasurements(
    const pose_graph_tools::PoseGraph& meas) {
  deformation_graph_->removePriorsWithPrefix(
      GlobalInfo::instance().getRobotPrefix().key);
  std::vector<std::pair<gtsam::Key, gtsam::Pose3>> agent_measurements;
  for (const auto& node : meas.nodes) {
    agent_measurements.push_back(
        {gtsam::Symbol(GlobalInfo::instance().getRobotPrefix().key, node.key),
         gtsam::Pose3(node.pose.matrix())});
  }
  deformation_graph_->addNodeMeasurements(agent_measurements);
}

설명:

• 목적: 외부에서 받은 노드 측정값을 변형 그래프에 업데이트합니다.

• 동작:

  1. 기존 프라이어 제거:

     • 현재 로봇의 프리픽스를 가진 모든 프라이어(prior)를 변형 그래프에서 제거합니다.
     • 이는 새로운 측정값으로 업데이트하기 위한 준비 단계입니다.

  2. 노드 측정값 생성:

     • 메시지(meas)의 모든 노드를 순회하며, 각 노드의 키와 포즈를 가져옵니다.
     • agent_measurements 벡터에 키와 포즈의 쌍을 추가합니다.

  3. 변형 그래프 업데이트:

     • 생성된 노드 측정값을 변형 그래프에 추가합니다.
     • 이를 통해 변형 그래프는 최신의 노드 정보를 갖게 됩니다.

• 참고 사항:

  • 프리픽스 사용: 로봇의 고유 식별자를 사용하여 노드 키를 생성하고 관리합니다.
  • 프라이어(prior): 이전에 알고 있던 노드의 상태 정보를 의미합니다.