[Frontend] hydra code - src/frontend/frontend_module.cpp

About_work·2024년 10월 15일

lifelong scene graph

목록 보기

16/56

SpinOnce() 중요

0. 핵심! 이것만 보면돼!

0.1. updateMesh

reconstruction에서 새로 검지한 mesh를 압축한 후 , DSG에 반영
DSG mesh 변경사항에 따른
- object-mesh 간 연결(edge) 수정
- mesh와 연결되지 않은 object 제거
사견 & 궁금증
- 새 mesh를 기존 DSG에 merge 하면,
  - 기존에 DSG에 있었던 mesh 지만 새 mesh에는 사라졌다고 인식된 mesh를 제거하는 로직이 있는가?
  - 로직이 있는 것 같긴 한데, 문제가 생기면 확인 필요

0.1.1. updateObjects

updateMesh 수행 후 실행됨
DSG mesh 변경사항에 따른
- mesh 기반 object clustering 실시 후 -> DSG의 기존 Object와의 병합 혹은 추가
  - label이 같으면서, 거리적으로 가까우면 병합
- 업데이트 된 object - place 간 연결 수정 (가장 가까운 place를 찾아 연결)
사견 & 궁금증
- 아마, 새 input 영역들만 한정해서 로직을 돌리지 않을까 싶음
- object-place 연결: 빈 공간 a는 어떤 Object들과 연결되어 있는지를 매칭시킴

0.1.2. updatePlaces2d

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 개념으로 관리하며, 중첩이 많이 될수록 값이 커집니다.

0.2. updateDeformationGraph

kimera_pgmo::MeshCompression(deformation_compression_) 에서, 오래된 mesh 제거
reconstruction input 의 요소인 mesh를 kimera_pgmo::MeshCompression(deformation_compression_)에 추가한 후, 압축
kimera_pgmo::Graph (deformation_graph_)에, 위 과정에서 새로 생성된 압축된 mesh 면(삼각형, 꼭지점)들을 그래프 형태(노드 + 엣지)로 변환하여 추가해줌
kimera_pgmo::Graph (deformation_graph_)에 추가된 새 압축된 edge들을 이용해서, PoseGraph를 만듭니다.
- 이 PoseGraph는 새롭게 추가된 mesh 정보들을 담고 있습니다.
  - node: mesh 꼭지점
  - edge: 꼭지점들간 연결 관계 (두 꼭지점간 거리 관계)

0.3. updatePoseGraph

과정 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와 해당 단어의 가중치로 구성

0.4. updatePlaces

reconstruction 모듈이 전달한 TSDF를 기반으로 GVD 알고리즘을 돌려 place nodes를 도출(graph 형태로 도출)
도출한 graph(place nodes)를 원래 그래프에 반영
Place(3) - Agent(2) 엣지를 새로 조정하고, Place(3)-object(2) 엣지를 새로 조정합니다.

0.5. updateFrontiers

프론티어 노드를 감지하고 DSG 그래프에 반영하는 것
프론티어는 알려진 영역과 미지의 영역 경계에 해당

0.1. 로직

`frontier_places_.updateRecentBlocks`

로봇의 현재 위치와 가까운 블록들만 남겨두고 나머지는 정리

`freespace_places_.getActiveNodes`

places: 3번째 계층
places 중 활성화된 node를 가져온다.

`frontier_places_.detectFrontiers`

frontier voxel: places(장애물과 먼 공간) 근처에 있는 voxel 중, 많이 가보지 못한 voxel들 중, 주변이 장애물이 없는 공간이 존재하는 voxel
frontier voxel들을 아래 2가지 중 하나의 방식으로 관리
- Dense Frontiers
  - 경계 점을 개별적인 점 클라우드로 관리.
- Sparse Frontiers
  - 탐지된 경계를 군집(cluster)으로 묶어 관리.

`frontier_places_.addFrontiers`

이전에 탐색된 Frontiers 노드들을 DSG 그래프의 place node에서 제거해.
새롭게 탐지된 경계 추가
- 새롭게 탐지된 활성 경계점들을 그래프에 Place 노드로 추가
  - 경계점의 위치, 크기, 방향, 복셀 개수 등의 정보가 포함된 노드를 생성
  - 이 노드가 실제 장소(Place)가 아닌, 탐사 경계임을 표시
- 해당 경계 노드를 기존의 Place 노드와 연결하여 그래프에 엣지를 추가
- 추가된 경계 노드의 ID를 nodes_to_remove_ 목록에 저장해, 다음 탐색에서 정리될 수 있도록 준비해.
아카이브된 경계 추가
- 아카이브된 경계도 마찬가지로 그래프에 Place 노드로 추가해.
  - 경계점의 위치와 기타 속성들을 설정함.
  - 이 노드가 아카이브된 경계임을 표시함.
- 역시 해당 경계 노드를 Place 노드와 연결하고 그래프에 엣지를 추가해.

0.6. updatePlaceMeshMapping

장소(Place) 노드와 메쉬 데이터 간의 연결을 유지하고, 일치하지 않는 부분을 수정
최근 메쉬 업데이트 정보와 기존의 메쉬-블록 매핑을 바탕으로 메쉬와 장소의 연속성을 유지
일치하지 않는 메쉬 정보가 있는 경우 이를 탐지하고 경고 메시지를 남깁니다.
PlaceMeshConnector 객체를 생성하여,
- 활성화된 장소(Place) 노드와 가까운 메쉬 포인트를 연결
- 각 장소(place) 노드에 변형(deformation) 연결 정보와 레이블(semantics)을 할당
- 연결되지 않은 노드를 카운트하여 누락된 연결 수를 반환

0.7 그 후

백엔드 그래프(state_->backend_graph) 에 프론트엔드에서 처리한 현재 그래프(dsg_ ->graph)를 병합
LCD 그래프에 프론트엔드 그래프를 병합
아래 output들을 2개 모듈로 전달
backend_input_
- mesh (1)
- deformation_graph (1)
- agent_updates (2)
lcd_input_
- archived_places (3)
- new_agent_nodes (2)

2. spinOnce 메서드 설명

이 메서드는 ReconstructionOutput 데이터 input을 처리하는 데 중요한 역할

2.1. 주요 흐름 요약:

데이터 폴링 및 처리:
- 입력 큐에서 데이터를 가져와 처리하며, 각 입력 데이터에 대해 3. spinOnce 함수를 호출
초기화 및 업데이트:
- 처음 호출 시 콜백을 초기화하며, 각 입력 데이터를 처리하고 관련 상태나 그래프를 업데이트
백엔드 및 LCD 처리:
- 데이터를 백엔드와 LCD에 전달
후속 처리 (Sink 호출):
- Sink를 통해 데이터를 전달하고, 후속 작업을 실행

2.2. 메서드 깊이 있게 설명

이 함수는 queue_(입력 대기열)에서 데이터를 폴링하여 ReconstructionOutput을 처리하는 메서드

bool FrontendModule::spinOnce() {
  bool has_data = queue_->poll();
  if (!has_data) {
    return false;
  }

  ReconstructionOutput::Ptr input = queue_->front();
  processNextInput(*input);
  queue_->pop();

  spinOnce(input);
  return true;
}

queue_->poll(): 큐에서 데이터를 기다리며, 데이터가 있으면 true를 반환하고 없으면 false를 반환합니다.
queue_->front(): 큐의 첫 번째 요소, 즉 가장 최근의 ReconstructionOutput 데이터를 가져옵니다.
processNextInput(): 아무것도 구현되어 있지 않음.
queue_->pop(): 큐에서 첫 번째 입력을 제거합니다. 입력이 처리되었으므로 큐에서 제거하는 것입니다.
spinOnce(input): 입력 데이터를 더 구체적으로 처리하기 위해 spinOnce 함수의 또 다른 오버로드된 버전을 호출
- 이 함수는 실제로 ReconstructionOutput에 대한 처리를 진행합니다.

3. `void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg)` - 코드 상

이 함수는 앞서 언급한 ReconstructionOutput을 실제로 처리하는 핵심 로직을 담고 있습니다.

void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg) {
  if (!initialized_) {
    initCallbacks();
  }

  VLOG(5) << "[Hydra Frontend] Popped input packet @ " << msg->timestamp_ns << " [ns]";
  std::lock_guard<std::mutex> lock(mutex_);
  ScopedTimer timer("frontend/spin", msg->timestamp_ns);

  backend_input_.reset(new BackendInput());
  backend_input_->timestamp_ns = msg->timestamp_ns;
  if (state_->lcd_queue) {
    lcd_input_.reset(new LcdInput());
    lcd_input_->timestamp_ns = msg->timestamp_ns;
  }

  updateImpl(msg);

초기화:
- 아직 콜백이 초기화되지 않았다면 3.1. initCallbacks()를 호출하여 콜백을 설정
backend_input_ 초기화:
- BackendInput 객체를 새로 생성하고, 타임스탬프 값을 설정
  - 이 객체는 후속 모듈에 전달되는 데이터를 저장하는 역할
lcd_input_ 초기화 (옵션):
- LCD(Loop Closure Detection) 처리를 위해 필요할 경우,
  - LcdInput 객체를 생성하고 타임스탬프를 설정
    - LcdInput는 place와 agent node 정보를 담음
- LCD는 루프 클로저 탐지를 통해 로봇의 위치나 경로를 수정하는 과정

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

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

3.2. updateImpl():
- 3.2. updateImpl(msg) 함수는 현재 입력 데이터를 사용하여 내부 상태를 업데이트하고, 필요한 모든 처리를 실행

3.1. initCallbacks()

콜백 함수들을 초기화하는 역할을 수행
여러 메서드들을 콜백 형태로 등록하여, 적절한 시점에 호출될 수 있도록 준비하는 과정

콜백 함수 등록 (`input_callbacks_`)

input_callbacks_ 리스트에 5개의 주요 콜백 함수를 추가

(1) `updateMesh` 등록

https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-detail
updateMesh가 끝나면 (+ updateObjects & updatePlaces2d) 이 실시된다.

콜백 함수 등록 (`post_mesh_callbacks_`)

메쉬 업데이트 이후 처리해야 할 추가적인 작업을 수행하는 콜백 함수 2개를 등록합니다.

(1) `updateObjects` 등록

https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-detail

(2) `updatePlaces2d` 등록

https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-detail

(2) `updateDeformationGraph` 등록

updateDeformationGraph()는 변형 그래프를 업데이트하는 함수
- 변형 그래프: 새롭게 업데이트(압축+추가/오래된 것 삭제) 된 mesh 정보를 그래프로 표현한 것
- https://velog.io/@jk01019/hydra-용어-정리-deformation-Graph
- https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-updateDeformationGraph

(3) `updatePoseGraph` 등록

https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-updatePoseGraph

(4) `updatePlaces` 등록

https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-updatePlaces

(5) `updateFrontiers` 등록

updateFrontiers()는 로봇이 탐지한 경계 지역(Frontiers)을 업데이트하는 함수
- 경계는 로봇이 아직 탐색하지 않은 미지의 영역과 탐색한 영역 사이의 경계를 나타냄
- 이 함수는 로봇의 탐사 경로를 설정하는 데 중요한 역할
https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-updateFrontiers

3.2. updateImpl

재구성된 출력 데이터를 처리하고 적절한 콜백 함수들을 실행하는 역할

3.2.1. 정리

input_dispatches_ 초기화:

input_callbacks_에 있는 콜백 함수들을 디스패치 함수 형태로 input_dispatches_에 저장하여, 동적으로 호출할 수 있도록 준비

콜백 함수 실행:

launchCallbacks()를 통해 각 콜백 함수를 실행하여,
- 입력된 메시지 데이터를 처리하고, 로봇의 환경 인식 정보, 위치, 경계 등을 업데이트

메쉬와 장소 간의 매핑 갱신:
- updatePlaceMeshMapping
- https://velog.io/@jk01019/Frontend-hydra-code-srcfrontendfrontendmodule.cpp-updatePlaceMeshMapping

4. `void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg)` - 코드 하

4.1. 백엔드 및 LCD 처리 (데이터 복사):

 // TODO(nathan) ideally make the copy lighter-weight
  // we need to copy over the latest updates to the backend and to LCD
  // no fancy threading: we just mark the update time and copy all changes in one go
  {  // start critical section
    std::unique_lock<std::mutex> lock(state_->backend_graph->mutex);
    ScopedTimer merge_timer("frontend/merge_graph", msg->timestamp_ns);
    state_->backend_graph->last_update_time = msg->timestamp_ns;
    state_->backend_graph->graph->mergeGraph(*dsg_->graph);
  }  // end critical section

  if (state_->lcd_queue) {
    // n.b., critical section in this scope!
    std::unique_lock<std::mutex> lock(state_->lcd_graph->mutex);
    ScopedTimer merge_timer("frontend/merge_lcd_graph", msg->timestamp_ns);
    state_->lcd_graph->last_update_time = msg->timestamp_ns;
    state_->lcd_graph->graph->mergeGraph(*dsg_->graph);
  }

전체 흐름 요약

이 코드는 프론트엔드에서 처리된 그래프 데이터를 백엔드와 LCD에 병합

백엔드 그래프와의 동기화

첫 번째 블록에서는 백엔드 그래프(state_->backend_graph)에 프론트엔드에서 처리한 현재 그래프(dsg_->graph)를 병합
mergeGraph 함수는
- 백엔드 그래프에 프론트엔드 그래프의 상태를 병합하는 역할
- 백엔드 그래프는 프론트엔드에서 수집된 데이터의 복사본을 유지하게 되며, 백엔드에서 추가적으로 필요한 정보들을 포함하게 됨
- TODO: mergeGraph 스터디

LCD 그래프와의 동기화

두 번째 블록은 LCD 그래프(state_->lcd_graph)가 있는 경우, 동일한 방식으로 LCD 그래프에 프론트엔드 그래프를 병합
mergeGraph 함수는 LCD 그래프에 프론트엔드에서 전달된 최신 그래프 데이터를 병합
- 이때, 프론트엔드 그래프에서 수집된 데이터가 LCD 모듈에서 재사용되며,
  - 이를 통해 루프 클로저 탐지나 위치 재탐색과 같은 작업이 수행됨
- TODO: mergeGraph 스터디

4.2. 백엔드 큐 및 로그 처리:

  backend_input_->mesh_update = last_mesh_update_;
  state_->backend_queue.push(backend_input_);
  if (state_->lcd_queue) {
    state_->lcd_queue->push(lcd_input_);
  }

  if (logs_) {
    // mutex not required because nothing is modifying the graph
    frontend_graph_logger_.logGraph(dsg_->graph);
  }

  if (dsg_->graph && backend_input_) {
    ScopedTimer sink_timer("frontend/sinks", msg->timestamp_ns);
    Sink::callAll(sinks_, msg->timestamp_ns, *dsg_->graph, *backend_input_);
  }

정리

이 코드의 전반적인 역할은 프론트엔드에서 처리된 메쉬 업데이트 정보와 기타 데이터들을 백엔드와 로컬 루프 클로저로 전달하고,
- 그래프 상태를 로그로 기록한 뒤,
- 등록된 Sink 콜백 함수들을 호출하여 데이터를 후속 처리
backend_input_
- mesh
- deformation_graph
- agent_updates
lcd_input_
- archived_places
- new_agent_nodes

`backend_input_`에 메쉬 업데이트 정보 저장

backend_input_->mesh_update = last_mesh_update_;
- backend_input_ 객체는 백엔드로 전달될 입력 데이터를 담고 있습니다.
  - 이 라인에서는 last_mesh_update_ 값을 backend_input_의 mesh_update 필드에 할당
- last_mesh_update_는 이전 메쉬 업데이트 데이터로,
  - 현재 시점에서 처리된 최신 메쉬 업데이트 정보를 담고 있음
- 이 과정은 메쉬 데이터의 상태가 백엔드 모듈로 전파될 수 있도록 준비하는 단계

`backend_queue`에 데이터 푸시

state_->backend_queue.push(backend_input_);
- backend_queue는 백엔드 모듈로 전달될 데이터를 저장하는 큐
  - backend_input_ 객체를 큐에 푸시(push)하여 백엔드 모듈에서 나중에 해당 데이터를 사용할 수 있도록 함
  - mesh, deformation graph, agent 가 backendinput에 포함되어 있음

`lcd_queue`에 데이터 푸시 (옵션)

lcd_queue는 로컬 루프 클로저(Local Loop Closure, LCD) 큐
- 로봇의 루프 클로저 관련 데이터를 처리하는 큐로, lcd_input_ 객체를 푸시
  - 장소 node와 agent node가 lcd_queue에 들어가 있는듯함
- 이는 루프 클로저가 활성화된 경우에만 발생
이를 통해 로컬 루프 클로저 기능이 필요한 경우 해당 데이터를 처리할 수 있도록 준비

Sink 콜백 함수 호출

dsg_->graph와 backend_input_이 유효한지 확인한 뒤, Sink::callAll() 함수를 통해 모든 등록된 sinks_ 콜백 함수를 호출합니다.
이 함수는 등록된 Sink(출력 모듈)에 데이터를 전파하는 역할
- dsg_->graph는 현재 그래프 상태,
- backend_input_은 백엔드로 전달될 데이터를 포함
Sink는 다양한 모듈들이 이 데이터를 활용하여 추가 작업을 수행할 수 있도록 하는 구조

About_work

새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.

이전 포스트

hydra 코드 분석 - README

다음 포스트

[Frontend] hydra code - src/frontend/frontend_module.cpp

lifelong scene graph

0. 핵심! 이것만 보면돼!

0.1. updateMesh

0.1.1. updateObjects

0.1.2. updatePlaces2d

0.2. updateDeformationGraph

0.3. updatePoseGraph

0.4. updatePlaces

0.5. updateFrontiers

0.1. 로직

frontier_places_.updateRecentBlocks

freespace_places_.getActiveNodes

frontier_places_.detectFrontiers

frontier_places_.addFrontiers

0.6. updatePlaceMeshMapping

0.7 그 후

2. spinOnce 메서드 설명

2.1. 주요 흐름 요약:

2.2. 메서드 깊이 있게 설명

3. void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg) - 코드 상

3.1. initCallbacks()

콜백 함수 등록 (input_callbacks_)

(1) updateMesh 등록

콜백 함수 등록 (post_mesh_callbacks_)

(1) updateObjects 등록

(2) updatePlaces2d 등록

(2) updateDeformationGraph 등록

(3) updatePoseGraph 등록

(4) updatePlaces 등록

(5) updateFrontiers 등록

3.2. updateImpl

3.2.1. 정리

4. void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg) - 코드 하

4.1. 백엔드 및 LCD 처리 (데이터 복사):

전체 흐름 요약

백엔드 그래프와의 동기화

LCD 그래프와의 동기화

4.2. 백엔드 큐 및 로그 처리:

정리

backend_input_에 메쉬 업데이트 정보 저장

backend_queue에 데이터 푸시

lcd_queue에 데이터 푸시 (옵션)

Sink 콜백 함수 호출

hydra 코드 분석 - README

hydra code - src/utils/mesh_utilities.cpp

0개의 댓글

`frontier_places_.updateRecentBlocks`

`freespace_places_.getActiveNodes`

`frontier_places_.detectFrontiers`

`frontier_places_.addFrontiers`

3. `void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg)` - 코드 상

콜백 함수 등록 (`input_callbacks_`)

(1) `updateMesh` 등록

콜백 함수 등록 (`post_mesh_callbacks_`)

(1) `updateObjects` 등록

(2) `updatePlaces2d` 등록

(2) `updateDeformationGraph` 등록

(3) `updatePoseGraph` 등록

(4) `updatePlaces` 등록

(5) `updateFrontiers` 등록

4. `void FrontendModule::spinOnce(const ReconstructionOutput::Ptr& msg)` - 코드 하

`backend_input_`에 메쉬 업데이트 정보 저장

`backend_queue`에 데이터 푸시

`lcd_queue`에 데이터 푸시 (옵션)