void FrontendModule::updatePoseGraph(const ReconstructionOutput& input) {
  if (!tracker_) {
    LOG_FIRST_N(WARNING, 1)
        << "PoseGraphTracker disabled, no agent layer will be created";
    return;
  }

  std::unique_lock<std::mutex> lock(dsg_->mutex);
  ScopedTimer timer("frontend/update_posegraph", input.timestamp_ns);
  const auto& prefix = GlobalInfo::instance().getRobotPrefix();
  const auto& agents = dsg_->graph->getLayer(DsgLayers::AGENTS, prefix.key);

  if (lcd_input_) {
    lcd_input_->new_agent_nodes.clear();
  }

  const auto packet = tracker_->update(input.timestamp_ns, input.world_T_body());
  if (backend_input_) {
    backend_input_->agent_updates = packet;
  }

  for (const auto& pose_graph : packet.pose_graphs) {
    if (!pose_graph || pose_graph->nodes.empty()) {
      continue;
    }

    for (const auto& node : pose_graph->nodes) {
      if (node.key < agents.numNodes()) {
        continue;
      }

      Eigen::Vector3d position = node.pose.translation();
      Eigen::Quaterniond rotation(node.pose.linear());

      // TODO(nathan) implicit assumption that pgmo ids are sequential starting at 0
      // TODO(nathan) implicit assumption that gtsam symbol and dsg node symbol are
      // same
      NodeSymbol pgmo_key(prefix.key, node.key);

      const std::chrono::nanoseconds stamp(node.stamp_ns);
      VLOG(5) << "[Hydra Frontend] Adding agent " << agents.nodes().size() << " @ "
              << stamp.count() << " [ns] for layer " << agents.prefix.str()
              << " (key: " << node.key << ")";
      auto attrs = std::make_unique<AgentNodeAttributes>(rotation, position, pgmo_key);
      if (!dsg_->graph->emplaceNode(
              agents.id, agents.prefix, stamp, std::move(attrs))) {
        VLOG(1) << "[Hydra Frontend] repeated timestamp " << stamp.count()
                << "[ns] found";
        continue;
      }

      // TODO(nathan) save key association for lcd
      const size_t last_index = agents.nodes().size() - 1;
      agent_key_map_[pgmo_key] = last_index;
      if (lcd_input_) {
        lcd_input_->new_agent_nodes.push_back(agents.prefix.makeId(last_index));
      }
    }
  }

  addPlaceAgentEdges(input.timestamp_ns);
  assignBowVectors(agents);
}

1. updatePoseGraph

0. 핵심만 정리!

• 과정 1
  • 로봇의 현재 위치와 이전 위치를 바탕으로 포즈 그래프를 갱신
  • 갱신된 포즈 그래프를 포함한 PoseGraphPacket 형태를 반환하여
    • backend의 input(agent_updates 맴버 변수)으로 전달
• 과정 2
  • DSG에 새로운 에이전트 노드 추가
  • 추가된 agent node를 lcd_input_(new_agent_nodes 맴버 변수)에 추가
• 과정 3: addPlaceAgentEdges
  • DSG의 Agent node(2 layer)와 Place node(3 layer)의 edge 업데이트
• 과정 4: assignBowVectors
  • agent node에 Bag-of-words 벡터를 할당
    • Bag-of-words: 특정 단어 ID와 해당 단어의 가중치로 구성

---

주요 작업 순서 및 로직

3. 로봇의 프리픽스 및 AGENT 레이어 추출

• 로봇의 ID(Prefix)를 가져와 에이전트 레이어(AGENTS)를 찾습니다.
• Loop Closure Detection(lcd_input_)이 활성화된 경우,
  • 새로운 에이전트 노드 목록을 초기화

---

4. 트래커 업데이트 및 백엔드 전달

const auto packet = tracker_->update(input.timestamp_ns, input.world_T_body());

• tracker_->update() 호출을 통해 로봇의 현재 포즈를 업데이트
  • 트래커: 에이전트(로봇)의 위치를 추적하며, 이를 통해 포즈 그래프(pose_graphs)를 갱신
• 트래커가 반환한 packet에 있는 포즈 그래프를 백엔드(backend_input_)로 전달
• tracker_ 와 update() -> https://velog.io/@jk01019/Frontend-hydra-code-includehydraodometryposegraphtracker.h
• TODO
  • pose_graphs: pose_graph_tools::PoseGraph 공부하기

5. DSG에 새로운 에이전트 노드 추가

• 포즈 그래프(pose_graphs) 내 각 노드를 순회하며 에이전트 노드를 추가
  1. 중복 노드 필터링: 노드의 key가 기존 노드 수보다 작다면 이미 존재하는 노드로 간주하고 건너뜁니다.
  2. 노드의 위치와 회전 정보 추출:
     • 3D 위치(translation)와 회전(rotation)을 Eigen 형식으로 변환합니다.
  3. 노드 생성:
     • pgmo_key를 생성해 해당 노드의 키를 관리
     • AgentNodeAttributes 객체로 에이전트 노드 속성을 설정하고, DSG 그래프에 노드를 추가
  4. 에이전트 노드 인덱스 저장:
     • 노드가 추가되면, agent_key_map_에 노드의 키와 인덱스를 저장합니다.
     • 만약 LCD(루프 클로저)가 활성화된 경우, 새로 추가된 노드들을 lcd_input_에 기록

struct LcdInput {
  using Ptr = std::shared_ptr<LcdInput>;

  uint64_t timestamp_ns;
  NodeIdSet archived_places;
  std::vector<NodeId> new_agent_nodes;
};

• TODO
  • AgentNodeAttributes 공부하기
  • agentkey_map 공부하기

---

6. 에이전트와 장소 간의 연결

• addPlaceAgentEdges() 함수를 호출해 장소 노드와 에이전트 간의 연결을 추가
  • 이를 통해 로봇의 위치와 주행 경로가 그래프 내 장소 노드와 연계

7. Bow 벡터 할당

• assignBowVectors()를 호출해 에이전트에 Bag-of-Words(BOW) 벡터를 할당
  • BOW 벡터는 루프 클로저 탐지나 장소 인식을 위한 특징 벡터로 사용

---

---

addPlaceAgentEdges

3. 코드의 주요 목적

• 에이전트 노드와 장소 노드 간의 관계를 최신화하는 것
  1. 새로 추가된 에이전트 노드를 추적하고, 그들과 가장 가까운 장소 노드를 찾음
  2. 부모-자식 연결을 추가하여 DSG의 관계를 유지합니다.
  3. 부모 노드가 비활성화된 경우에는 해당 에이전트 노드를 비활성화합니다.

assignBowVectors

assignBowVectors 함수의 알고리즘 및 로직 심층 분석

• 이 함수는 BoW(Bag-of-Words) 벡터를 에이전트 노드에 할당하여 (특징점 기반의 환경 인식(인코딩 벡터))
  • BoW: 특정 단어 ID와 해당 단어의 가중치로 구성
  • 그래프의 각 에이전트가 해당 위치에서 수집한 데이터와 연결되도록 관리
• 이 작업은 아래 작업에 중요한 역할
  • DSG(Dynamic Scene Graph)에서 정보 통합
  • 로봇의 데이터 일치(Loop Closure Detection) 작업
• 이 함수는 BoW 벡터를 사용해 에이전트 노드에 시각적 정보를 부여하고, 이를 통해 위치 인식과 루프 클로저를 강화
• 루프 클로저 감지:
  • 로봇이 과거의 위치를 재인식할 때 BoW 벡터를 사용

---

---

2. 세부 로직 분석

1) BoW 큐 처리: 메시지 수집과 캐시 업데이트

• 큐에서 메시지를 꺼내 캐시에 저장:
  • 로봇이 수신한 BoW 메시지를 배치 처리하기 위해
  • 큐(queue)에서 모든 메시지를 꺼내 cached_bow_messages_에 저장
• 이전과 현재 메시지의 관리:
  • BoW 메시지는 스핀(spin) 주기마다 수집되며, 처리가 완료된 메시지는 캐시에서 제거

2) 로봇 간 메시지 필터링

• BoW 메시지의 로봇 ID가 현재 로봇의 ID와 일치하는지 확인
• 다른 로봇의 BoW 벡터는 삭제하여 잘못된 노드에 할당되지 않도록 합니다.
  • 이 단계는 멀티 에이전트 환경에서 데이터 충돌을 방지하기 위한 필수 과정

---

3) 에이전트 노드와 BoW 벡터의 매핑

• BoW 메시지의 포즈 ID와 일치하는 에이전트 노드를 찾음:
  • 각 메시지에 포함된 포즈 ID를 agent_key_map_에서 찾아 해당 노드를 식별합니다.
  • 만약 해당 ID와 일치하는 노드를 찾지 못하면, 해당 메시지는 건너뜁니다.