0. 이거만 보면됨! 핵심요약

0.1. updateMesh

• reconstruction에서 새로 검지한 mesh를 압축한 후 , DSG에 반영
• DSG mesh 변경사항에 따른
  • object-mesh 간 연결(edge) 수정
  • mesh와 연결되지 않은 object 제거

0.2. UpdateObjects

• updateMesh 수행 후 실행됨
• DSG mesh 변경사항에 따른
  • mesh 기반 object clustering 실시 후 -> DSG의 기존 Object와의 병합 혹은 추가
  • 업데이트 된 object - place 간 연결 수정 (가장 가까운 place를 찾아 연결)

0.3. UpdatedPlaces2D

• class label별로 clustering을 통해, 2d places를 도출
  • object와 다른점은
    • 2d 라는 점이다.
    • 자유공간을 의미(places)하는게 아니고, object(2d places)를 의미함
• 이를 DSG에 업데이트 (추가/수정/제거)
  • 2d places - mesh 연결 업데이트
  • 2d places 상태 관리
    • 제거 조건
      • boundary(edge) 3개 이하일 때 (2d places간 연결된 edge를 의미하는듯)
        • TODO: 왜 제거하지? 잘못 알고 있는 것 같기도..
      • 연결된 mesh가 없을 때
  • 2d place 간 edge(연결관계) 관리
    • 2d place끼리 중첩되고, 높이 차이가 적으면 -> edge로 연결
    • edge를 weight 개념으로 관리하며, 중첩이 많이 될수록 값이 커집니다.

---

1. updateMesh

void FrontendModule::updateMesh(const ReconstructionOutput& input) {
  {  // start timing scope
    ScopedTimer timer("frontend/mesh_archive", input.timestamp_ns, true, 1, false);
    VLOG(5) << "[Hydra Frontend] Clearing " << input.archived_blocks.size()
            << " blocks from mesh";
    if (!input.archived_blocks.empty()) {
      mesh_compression_->clearArchivedBlocks(input.archived_blocks);
    }
  }  // end timing scope

  // TODO(nathan) prune archived blocks from input?

  const auto& input_mesh = input.map().getMeshLayer();
  {
    ScopedTimer timer("frontend/mesh_compression", input.timestamp_ns, true, 1, false);
    mesh_remapping_ = std::make_shared<kimera_pgmo::HashedIndexMapping>();
    auto mesh = getActiveMesh(input_mesh, input.archived_blocks);
    VLOG(5) << "[Hydra Frontend] Updating mesh with " << mesh->numBlocks() << " blocks";
    auto interface = PgmoMeshLayerInterface(*mesh);
    last_mesh_update_ =
        mesh_compression_->update(interface, input.timestamp_ns, mesh_remapping_.get());
  }  // end timing scope

  {  // start timing scope
    // TODO(nathan) we should probably have a mutex before modifying the mesh, but
    // nothing else uses it at the moment
    ScopedTimer timer("frontend/mesh_update", input.timestamp_ns, true, 1, false);
    last_mesh_update_->updateMesh(*dsg_->graph->mesh());
    invalidateMeshEdges(*last_mesh_update_);
  }  // end timing scope

  ScopedTimer timer(
      "frontend/launch_postmesh_callbacks", input.timestamp_ns, true, 1, false);
  launchCallbacks(post_mesh_callbacks_, input);
}

void FrontendModule::updateObjects(const ReconstructionOutput& input) {
  if (!last_mesh_update_) {
    LOG(ERROR) << "Cannot detect objects without valid mesh";
    return;
  }

  const auto clusters =
      segmenter_->detect(input.timestamp_ns, *last_mesh_update_, std::nullopt);

  {  // start dsg critical section
    std::unique_lock<std::mutex> lock(dsg_->mutex);
    segmenter_->updateGraph(input.timestamp_ns,
                            clusters,
                            last_mesh_update_->getTotalArchivedVertices(),
                            *dsg_->graph);
    addPlaceObjectEdges(input.timestamp_ns);
  }  // end dsg critical section
}

정리

• 이 함수는 메쉬 데이터를 관리하고 최신 상태로 유지하는 역할
  • 불필요한 메쉬 블록을 삭제 + 압축된 메쉬 데이터를 생성
  • 메쉬 데이터가 그래프에 제대로 반영되도록 함,
    • object와 mesh간 변경된 연결(엣지)을 처리하여 일관성을 유지
  • 마지막으로, 후속 콜백 함수들을 실행하여
    • 객체 인식, 장소 탐지 등 추가 작업을 처리

1.1. 메쉬 블록 삭제 처리

• input.archived_blocks는 이전 프레임에서 사용되지 않거나 불필요하게 된 메쉬 블록을 의미
  • 이 블록들을 mesh_compression_ 객체를 사용해 삭제
    • 이는 clearArchivedBlocks() 함수로 처리됨
  • TODO
    • clearArchivedBlocks 메서드 내용 확인 (하는 법 공부)
    • input.archived_blocks 은, 누가 어떤 기준으로 채우는가?

1.2. 메쉬 업데이트 준비

• const auto& input_mesh = input.map().getMeshLayer();
  • 입력된 메쉬 데이터를 가져와 메쉬 레이어(input_mesh)로 설정
  • 이 정보를 기반으로 후속 작업을 진행합니다.
• 메쉬 리매핑(mesh_remapping_)
  • std::make_shared<kimera_pgmo::HashedIndexMapping>()로 리매핑 정보를 저장할 객체를 생성
  • 리매핑 작업은 메쉬 데이터가 업데이트되거나 재구성되는 동안, 인덱스가 어떻게 변했는지 추적하는 역할
• getActiveMesh(input_mesh, input.archived_blocks)
  • 이 함수는 사용 가능한(active) 메쉬 블록을 필터링하여 반환
  • 즉, 삭제되지 않은 블록만을 추출해 메쉬 업데이트에 활용할 준비를 합니다.
  • https://velog.io/@jk01019/hydra-code-srcutilsmeshutilities.cpp
• auto interface = PgmoMeshLayerInterface(*mesh);
  • https://velog.io/@jk01019/hydra-code-srcutilspgmomeshinterface.cpp
• last_mesh_update_ = mesh_compression_->update(interface, input.timestamp_ns, mesh_remapping_.get());
  • 이 단계에서는 압축된 메쉬 데이터를 생성하고, 리매핑 정보를 갱신 (압축시킨다는 뜻)
  • TODO: mesh_compression_->update 구현 부분을 찾아서 공부

1.3. 메쉬 업데이트 적용

• 이 단계에서는 last_mesh_update_에 저장된 압축된 메쉬 업데이트 정보를 실제로 그래프(dsg_)에 반영
  • last_mesh_update_->updateMesh(*dsg_->graph->mesh())는 업데이트된 메쉬 데이터를 그래프에 적용하는 작업
    • TODO: last_mesh_update_->updateMesh 에 대해 찾아서 공부해보기
  • 이후, invalidateMeshEdges()는 변경된 메쉬에 연결된 엣지(edge)들을 무효화하여 유효하지 않은 연결을 제거

1.3.1. invalidateMeshEdges

• 이 함수는 메쉬 업데이트(메쉬 삭제 및 변경)에 따라 오브젝트 노드의 메쉬 연결 상태를 무효화하고, 최소 메쉬 연결 수를 만족하지 못하는 오브젝트 노드를 그래프에서 제거

---

---

1.4. 후속 콜백 실행

• ScopedTimer timer("frontend/launch_postmesh_callbacks", input.timestamp_ns, true, 1, false);
  • 마지막으로 후속 작업(post-mesh callbacks)에 걸리는 시간을 측정합니다.
• launchCallbacks(post_mesh_callbacks_, input)
  • 이 단계에서는 메쉬 업데이트 후 실행되어야 할 후속 작업들을 실행합니다.
  • FrontendModule::updateObjects, FrontendModule::updatePlaces2d

---

2. updateObjects

void FrontendModule::updateObjects(const ReconstructionOutput& input) {
  if (!last_mesh_update_) {
    LOG(ERROR) << "Cannot detect objects without valid mesh";
    return;
  }

  const auto clusters =
      segmenter_->detect(input.timestamp_ns, *last_mesh_update_, std::nullopt);

  {  // start dsg critical section
    std::unique_lock<std::mutex> lock(dsg_->mutex);
    segmenter_->updateGraph(input.timestamp_ns,
                            clusters,
                            last_mesh_update_->getTotalArchivedVertices(),
                            *dsg_->graph);
    addPlaceObjectEdges(input.timestamp_ns);
  }  // end dsg critical section
}

핵심 요약

1. 메쉬 데이터가 유효한지 확인 후, 메쉬에서 객체를 클러스터링
2. 클러스터링된 객체 정보를 기반으로 DSG에 객체 노드를 추가하고, 해당 객체를 장소 노드와 연결

세부 로직

2. 클러스터링을 통한 객체 탐지

   const auto clusters =
       segmenter_->detect(input.timestamp_ns, *last_mesh_update_, std::nullopt);

   • https://velog.io/@jk01019/frontend-hydra-code-srcfrontendmeshsegmenter.cpp
   • segmenter_ 객체는 메쉬를 기반으로 객체를 클러스터링
     • 클러스터링은 메쉬에서 군집화된 점들을 의미하며, 이 군집들은 특정 객체를 나타냄
   • detect() 함수는 클러스터링된 객체 정보(clusters)를 반환

3. DSG(Graph) 업데이트 (Critical Section)

   {  // start dsg critical section
     std::unique_lock<std::mutex> lock(dsg_->mutex);
     segmenter_->updateGraph(input.timestamp_ns,
                             clusters,
                             last_mesh_update_->getTotalArchivedVertices(),
                             *dsg_->graph);
     addPlaceObjectEdges(input.timestamp_ns);
   }  // end dsg critical section

   • DSG 업데이트
   • segmenter_->updateGraph():
     • https://velog.io/@jk01019/frontend-hydra-code-srcfrontendmeshsegmenter.cpp
     • 클러스터링된 객체 정보를 기반으로 객체 노드(Object Nodes)를 DSG에 추가
     • last_mesh_update_->getTotalArchivedVertices()는 메쉬의 아카이브된 전체 버텍스 수를 반환
   • 마지막으로, addPlaceObjectEdges(input.timestamp_ns) 함수를 호출하여 객체와 장소 노드 간의 연결을 추가
   • 이는 탐지된 객체가 DSG 내의 특정 장소(places)와 연관성을 가질 수 있도록 관계를 설정하는 단계

addPlaceObjectEdges

• 객체 노드와 장소 노드 사이의 부모-자식 관계를 정의하는 엣지를 추가하는 작업을 수행
• 주로 객체와 장소 간의 공간적 인접성을 기반으로 부모-자식 관계를 설정

전체 알고리즘 요약

1. 활성화된 객체들을 가져옵니다.
2. 각각의 객체 노드에 대해, 해당 노드가 그래프에 유효한지 확인
3. 객체의 위치를 기준으로 가장 가까운 장소 노드를 찾습니다.
4. 찾은 장소 노드를 부모, 객체 노드를 자식으로 하는 부모-자식 엣지를 그래프에 추가

알고리즘 로직 및 역할 상세 설명

3. 활성화된 객체 노드 가져오기

• 객체 노드는 segmenter_에서 활성화된 노드들을 반환하는 getActiveNodes() 함수로부터 가져옵니다. 이 함수는 현재 메쉬에서 인식된 활성 객체들의 ID를 반환합니다.

  for (const auto& object_id : segmenter_->getActiveNodes()) {

5. 장소와 가장 가까운 부모 노드 찾기

• 장소 노드를 찾기 위해, 객체 노드의 위치(node->attributes().position)를 기준으로 가장 가까운 장소 노드를 탐색
• places_nn_finder_는 K-최근접 이웃(KNN) 알고리즘을 사용해, 주어진 객체 위치에서 가장 가까운 장소 노드를 찾습니다.
  • 여기서 탐색 개수는 1로 설정되어, 하나의 가까운 노드만 찾습니다.
• find 함수는 탐색된 장소 노드에 대해 콜백 함수를 실행합니다.
  • 콜백 함수는 찾은 장소 노드의 ID(place_id)와 해당 장소 노드를 부모 노드로,
  • 객체 노드를 자식 노드로 하는
  • 부모-자식 관계(엣지)를 그래프에 추가하는 역할을 합니다.

  places_nn_finder_->find(
      node->attributes().position, 1, false, [&](NodeId place_id, size_t, double) {
        dsg_->graph->insertParentEdge(place_id, object_id);
      });

6. 부모-자식 엣지 추가

• 콜백 함수 내에서, 장소 노드와 객체 노드를 연결하는 부모-자식 엣지가 추가됩니다.
• insertParentEdge(place_id, object_id) 함수는 place_id를 부모로, object_id를 자식으로 설정하여, 그래프에 부모-자식 관계를 정의합니다.
• 이를 통해 객체가 어느 장소에 속하는지 또는 해당 장소 주변에 있는지에 대한 공간적 관계가 그래프에 반영됩니다.

  dsg_->graph->insertParentEdge(place_id, object_id);

---

---

3. updatePlaces2d

• 이 함수는 2D 장소(places)를 탐지하고, 메쉬 데이터와 DSG(Dynamic Scene Graph)를 업데이트하는 역할

핵심 요약

• 이 함수는 메쉬 데이터를 기반으로 2D 장소(places)를 탐지하고 이를 DSG에 반영합니다.
  • 동시에, 더 이상 활성화되지 않은 장소들을 아카이브 상태로 변경

로직 설명

3. 장소 탐지 (surface_places_->detect)

   NodeIdSet active_nodes;
   
   {  // start timing scope
     ScopedTimer timer("frontend/places_2d", input.timestamp_ns, true, 1, false);
     surface_places_->detect(input, *last_mesh_update_, *dsg_->graph);

   • surface_places_->detect() 함수는 현재 메쉬 데이터(last_mesh_update_)와 DSG를 기반으로 2D 장소를 탐지
     • https://velog.io/@jk01019/hydra-code-srcfrontendplace2dsegmenter.cpp
   • 탐지된 장소들은 나중에 DSG에 반영됩니다.

4. DSG 업데이트 (Critical Section)

   {  // start graph critical section
     std::unique_lock<std::mutex> graph_lock(dsg_->mutex);
   
     surface_places_->updateGraph(input.timestamp_ns, input, *dsg_->graph);

   • surfaceplaces->updateGraph() 함수는 탐지된 장소 데이터를 DSG에 추가하거나 업데이트합니다.
   • https://velog.io/@jk01019/hydra-code-srcfrontendplace2dsegmenter.cpp

5. 아카이브 처리 및 활성 장소 업데이트

   archivePlaces2d(active_nodes);
   previous_active_places_2d_ = active_nodes;

   • archivePlaces2d(active_nodes) 함수는 활성 상태에서 벗어난 장소를 아카이브 상태로 변경하는 작업을 수행
   • 즉, 더 이상 활성화되지 않은 장소들은 아카이브(archive) 상태로 전환되어 이후 처리에서 제외될 수 있습니다.
   • 현재 활성 장소 집합(active_places)과 이전 활성 장소 집합(previous_active_places_2d_)을 비교
   • 이전에 활성화되었지만 현재는 활성화되지 않은 장소를 식별하여 비활성화 대상으로 지정합니다.
   • 마지막으로 previous_active_places_2d_에 현재 활성 노드 목록을 저장하여 다음 업데이트에서 비교할 수 있도록 합니다.