[Backend] hydra code - src/backend/backend_module.cpp - optimize

About_work·2024년 10월 23일

lifelong scene graph

목록 보기

47/56

핵심 요약! 이거만 보면돼

optimize 메서드는 루프 클로저가 감지되었을 때 메쉬 변형과 그래프 최적화를 수행합니다.
먼저, 장소 노드들을 변형 그래프에 추가하여 메쉬 변형 시 공간적 일관성을 유지합니다.
변형 그래프를 최적화하여 메쉬 변형에 필요한 제어점의 위치를 업데이트합니다.
최적화된 결과를 기반으로 DSG의 메쉬를 업데이트하여 환경 지도의 왜곡을 보정합니다.
업데이트된 정보를 바탕으로 에이전트와 각 레이어의 업데이트 함수를 호출하여 DSG의 노드와 엣지를 갱신합니다.
객체, 장소, 방, 건물 등의 레이어를 각각 업데이트하고, 필요한 경우 노드 병합을 수행합니다.
병합 처리와 콜백 실행을 통해 DSG의 일관성과 정확성을 유지합니다.
이를 통해 로봇의 위치 추정과 환경 모델이 최신 상태로 유지되도록 지원합니다.

BackendModule의 `optimize` 메서드 상세 분석

개요

BackendModule 클래스의 optimize 메서드는 로봇 시스템의 백엔드에서 메쉬 변형(Mesh Deformation)과 그래프 최적화(Graph Optimization)를 수행하는 핵심적인 함수입니다. 이 메서드는 루프 클로저(Loop Closure)가 감지되었을 때 호출되며, 환경의 지도를 정교화하고 오류를 보정하기 위해 메쉬와 그래프 구조를 업데이트합니다.

optimize 메서드는 다음과 같은 주요 작업을 수행합니다:

장소 노드(Place Nodes)를 변형 그래프에 추가
변형 그래프 최적화 수행
DSG(Dynamic Scene Graph) 메쉬 업데이트
업데이트된 정보를 기반으로 다른 모듈 호출

각 단계는 시스템의 정확성과 일관성을 유지하기 위해 중요한 역할을 합니다. 아래에서는 각 단계의 동작과 관련 함수들의 역할을 상세히 설명하겠습니다.

1. 장소 노드를 변형 그래프에 추가

목적

환경의 장소 노드를 변형 그래프에 추가하여, 메쉬 변형 과정에서 장소 노드의 위치 정보가 반영되도록 합니다. 이는 메쉬 변형이 공간적으로 일관되게 이루어지도록 보장하며, 장소 노드와 메쉬 간의 관계를 유지합니다.

동작

조건 확인: 설정에서 add_places_to_deformation_graph 옵션이 활성화되어 있는지 확인합니다.
함수 호출: addPlacesToDeformationGraph(timestamp_ns)를 호출하여 장소 노드를 변형 그래프에 추가합니다.

`addPlacesToDeformationGraph` 함수 상세 설명

주요 작업:

장소 레이어 접근: DSG에서 장소 노드 레이어를 가져옵니다. 만약 장소 노드가 없으면 경고를 출력하고 함수를 종료합니다.
타이머 시작: 성능 측정을 위해 타이머를 시작합니다.
변형 그래프의 임시 구조 초기화: 이전에 사용된 임시 데이터 구조를 초기화합니다.
최소 스패닝 트리(Minimum Spanning Tree, MST) 계산:
- 장소 노드 간의 MST를 계산하여, 변형 그래프의 엣지로 사용될 후보군을 생성합니다.
- MST를 통해 장소 노드 간의 최적 연결 구조를 확보합니다.
장소 노드 추가:
- 각 장소 노드에 대해, 변형 그래프에 노드를 추가합니다.
- 노드의 위치, ID, 그리고 연결 정보 등을 설정합니다.
- 변형 그래프의 노드로 추가될 때, 필요에 따라 가중치를 설정합니다.
MST 엣지 추가:
- 계산된 MST의 엣지를 변형 그래프에 추가합니다.
- 이는 장소 노드 간의 공간적 관계를 변형 그래프에 반영합니다.

세부 동작:

변형 그래프에 장소 노드 추가:
- 장소 노드 중에서 형제 노드(siblings)가 있는 노드들을 선택합니다.
- 각 노드의 위치 정보를 gtsam::Pose3 객체로 변환하여 저장합니다.
- 변형 그래프에 노드를 추가할 때, 해당 노드의 연결 정보(valences)를 설정합니다.
변형 그래프에 엣지 추가:
- MST에서 얻은 엣지를 변형 그래프의 엣지로 추가합니다.
- 각 엣지는 소스 노드와 타겟 노드의 위치 정보를 사용하여 상대 변환을 계산합니다.
- 엣지의 가중치나 정보 행렬은 설정에서 지정한 값을 사용합니다.

2. 변형 그래프 최적화 수행

목적

변형 그래프를 최적화하여 메쉬 변형에 필요한 제어점(Control Points)의 위치를 업데이트합니다. 이를 통해 루프 클로저로 인해 발생한 지도의 왜곡을 보정하고, 전체적인 일관성을 유지합니다.

동작

타이머 시작: 최적화 과정의 실행 시간을 측정합니다.
최적화 수행: deformation_graph_->optimize()를 호출하여 변형 그래프를 최적화합니다.

`DeformationGraph::optimize` 함수 상세 설명

주요 작업:

새로운 팩터와 변수 강제 업데이트:
- pgo_->forceUpdate(new_factors_, new_values_);를 호출하여, 새로운 팩터(factors)와 변수(values)를 그래프에 강제로 적용합니다.
재계산 플래그 확인:
- 만약 force_recalculate_ 플래그가 설정되어 있으면, 정점의 재계산이 필요함을 표시하는 recalculate_vertices_를 true로 설정합니다.
최적 추정치 계산:
- values_ = pgo_->calculateEstimate();를 호출하여, 최적화된 변수들의 값을 계산합니다.
내부 상태 업데이트:
- 현재의 팩터 그래프와 변수들을 업데이트하고, 임시 변수들을 초기화합니다.
- new_factors_와 new_values_를 초기화하여, 다음 최적화를 준비합니다.

참고 사항:

pgo_ 객체: Pose Graph Optimization을 수행하는 객체로, GTSAM 라이브러리를 기반으로 합니다.
강제 업데이트: 새로운 팩터와 변수를 그래프에 즉시 적용하여, 최적화에 반영합니다.

3. DSG 메쉬 업데이트

목적

최적화된 변형 그래프의 결과를 바탕으로 DSG의 메쉬를 업데이트하여, 환경의 지도를 최신 상태로 유지합니다. 이는 메쉬 변형을 통해 지도의 왜곡을 보정하고, 노드 간의 공간적 관계를 일관성 있게 유지합니다.

동작

메쉬 업데이트 여부 결정:
- force_mesh_update가 false이고 have_new_mesh_가 false이면, 업데이트를 수행하지 않습니다.
- 메쉬가 존재하지 않거나 비어 있는 경우 함수를 종료합니다.
- force_mesh_update가 false이고 루프 클로저가 발생하지 않은 경우에도 업데이트를 생략합니다.
타이머 시작: 메쉬 변형 과정의 실행 시간을 측정합니다.
메쉬 변형 수행:
- deformation_graph_->deformPoints를 호출하여 메쉬의 정점들을 변형합니다.
- 입력으로 원본 메쉬 정점(original_vertices_)과 타임스탬프(vertex_stamps_)를 제공합니다.
- 최적화된 변수 값(getGtsamValues())을 사용하여 변형을 적용합니다.
아카이브된 정점 수 업데이트:
- prev_num_archived_vertices_를 현재의 num_archived_vertices_로 업데이트하여, 다음 변형 시 사용할 수 있도록 합니다.

`DeformationGraph::deformPoints` 함수 상세 설명

주요 작업:

제어점 탐색을 위한 공간 인덱싱 구조 생성:
- 제어점(Control Points)을 기반으로 Octree를 생성하여, 공간적 탐색을 효율화합니다.
각 정점에 대한 변형 계산:
- 메쉬의 각 정점에 대해, 주변의 제어점을 탐색하여 변형을 계산합니다.
- 주변의 k개 제어점을 사용하여 가중치를 계산하고, 정점의 새로운 위치를 결정합니다.
시간적 유효성 검사:
- 정점과 제어점의 타임스탬프를 비교하여, 시간적으로 유효한 제어점만을 사용합니다.
- 이는 오래된 제어점이 현재 정점의 변형에 영향을 미치지 않도록 합니다.

세부 동작:

제어점 추가 및 제거:
- 정점의 타임스탬프에 따라, 유효한 제어점을 Octree에 추가하거나 제거합니다.
가중치 계산 및 위치 업데이트:
- 제어점과의 거리 및 시간 차이를 기반으로 가중치를 계산합니다.
- 가중치를 적용하여 정점의 새로운 위치를 계산하고 업데이트합니다.

참고 사항:

Octree: 3차원 공간에서의 빠른 근접 탐색을 위해 사용되는 자료 구조입니다.
시간적 고려: 변형 과정에서 시간적 일관성을 유지하기 위해, 정점과 제어점의 타임스탬프를 고려합니다.

4. 업데이트된 정보를 기반으로 다른 모듈 호출

목적

최적화된 결과를 기반으로 DSG(Dynamic Scene Graph)의 구조와 속성을 업데이트하고, 관련된 모듈이나 함수들을 호출하여 시스템 전체의 일관성을 유지합니다. 특히, 각 레이어의 업데이트 함수와 에이전트 업데이트 함수를 호출하여 그래프의 각 구성 요소를 갱신합니다.

동작 상세 분석

코드 상의 흐름

void BackendModule::optimize(size_t timestamp_ns) {
  // ... 이전 단계 코드 생략 ...

  // 4. 업데이트된 정보를 기반으로 다른 모듈 호출
  callUpdateFunctions(timestamp_ns,
                      deformation_graph_->getGtsamTempValues(),
                      deformation_graph_->getGtsamValues(),
                      have_new_loopclosures_);
  have_new_loopclosures_ = false;
}

optimize 메서드의 마지막 부분에서 callUpdateFunctions 함수를 호출합니다. 이 함수가 다른 모듈들을 호출하여 DSG를 업데이트하는 핵심 부분입니다.

`callUpdateFunctions` 함수의 상세 동작

void BackendModule::callUpdateFunctions(size_t timestamp_ns,
                                        const gtsam::Values& places_values,
                                        const gtsam::Values& pgmo_values,
                                        bool new_loop_closure,
                                        const UpdateInfo::LayerMerges& given_merges) {
  // 타이머 시작
  ScopedTimer spin_timer("backend/update_layers", timestamp_ns);

  // 병합 활성화 여부 결정
  const bool enable_merging = given_merges.empty() ? config.enable_node_merging : false;

  // 에이전트 값 준비
  gtsam::Values complete_agent_values;
  // ... 에이전트 값 설정 코드 생략 ...

  // UpdateInfo 객체 생성
  UpdateInfo::ConstPtr info(new UpdateInfo{&places_values,
                                           &pgmo_values,
                                           new_loop_closure,
                                           timestamp_ns,
                                           enable_merging,
                                           given_merges,
                                           &complete_agent_values});

  // 그래프 병합 수행
  GraphMergeConfig config;
  config.previous_merges = &private_dsg_->merges;
  config.update_dynamic_attributes = false;
  private_dsg_->graph->mergeGraph(*unmerged_graph_, config);

  // 에이전트 업데이트 함수 호출
  if (agent_functor_) {
    agent_functor_->call(*unmerged_graph_, *private_dsg_, info);
  }

  // 레이어별 업데이트 함수 호출 및 병합 처리
  std::list<LayerCleanupFunc> cleanup_hooks;
  for (const auto& [layer, functor] : layer_functors_) {
    if (!functor) {
      continue;
    }

    const auto merges = functor->call(*unmerged_graph_, *private_dsg_, info);
    const auto hooks = functor->hooks();
    merge_tracker.applyMerges(*unmerged_graph_, merges, *private_dsg_, hooks.merge);
    if (hooks.cleanup) {
      cleanup_hooks.push_back(hooks.cleanup);
    }
  }

  // 콜백 실행
  launchCallbacks(cleanup_hooks, info, private_dsg_.get());
}

1. 그래프 병합 수행

역할: 최신의 DSG 상태를 유지하기 위해, unmerged_graph_를 private_dsg_->graph에 병합합니다.
동작:
- GraphMergeConfig를 설정하여 병합 과정에서의 세부 동작을 제어합니다.
- mergeGraph 함수를 호출하여 그래프를 병합합니다.

2. 에이전트 업데이트 함수 호출 (`agent_functor_`)

역할: 에이전트 레이어를 업데이트하고, 에이전트 관련 노드 및 엣지를 처리합니다.
호출 조건: agent_functor_가 설정되어 있을 때 호출됩니다.
동작:
- agent_functor_->call(*unmerged_graph_, *private_dsg_, info);를 호출하여 에이전트 레이어를 업데이트합니다.

3. 레이어별 업데이트 함수 호출 및 병합 처리 (`layer_functors_`)

역할: 각 레이어에 대한 특화된 업데이트를 수행합니다.
동작:
- layer_functors_에 저장된 각 레이어의 업데이트 함수를 순회하며 호출합니다.
- functor->call(*unmerged_graph_, *private_dsg_, info);를 통해 레이어를 업데이트합니다.
- 업데이트 결과로 얻은 병합 정보를 merge_tracker.applyMerges를 통해 적용합니다.
- 필요한 경우, 정리 작업을 위한 콜백 함수를 수집합니다.

4. 콜백 실행

역할: 업데이트 과정에서 수집된 정리 작업이나 후속 처리를 수행합니다.
동작:
- launchCallbacks(cleanup_hooks, info, private_dsg_.get());를 호출하여 콜백 함수를 실행합니다.

호출되는 모듈 및 업데이트 함수들

에이전트 업데이트 함수 (`agent_functor_`)

역할: 에이전트 레이어의 노드와 엣지를 업데이트합니다.
설정:
- BackendModule 생성 시 또는 외부에서 agent_functor_를 설정합니다.

관련 코드:

if (agent_functor_) {
  agent_functor_->call(*unmerged_graph_, *private_dsg_, info);
}

레이어별 업데이트 함수 (`layer_functors_`)

역할: DSG의 각 레이어를 업데이트하고, 필요한 경우 노드 병합 등을 수행합니다.
설정된 기본 업데이트 함수들:
- 객체 레이어 (DsgLayers::OBJECTS):
  - 사용되는 업데이트 함수: UpdateObjectsFunctor
  - 역할: 객체 노드의 속성 업데이트 및 병합 제안.
- 장소 레이어 (DsgLayers::PLACES):
  - 사용되는 업데이트 함수: UpdatePlacesFunctor
  - 역할: 장소 노드의 위치 업데이트 및 병합 제안.
- 방 레이어 (DsgLayers::ROOMS):
  - 사용되는 업데이트 함수: UpdateRoomsFunctor
  - 역할: 방을 식별하고, 방 노드를 생성 및 업데이트.
- 건물 레이어 (DsgLayers::BUILDINGS):
  - 사용되는 업데이트 함수: UpdateBuildingsFunctor
  - 역할: 건물 노드를 생성하고, 방 노드와의 계층 구조를 설정.

관련 코드:

for (const auto& [layer, functor] : layer_functors_) {
  if (!functor) {
    continue;
  }

  const auto merges = functor->call(*unmerged_graph_, *private_dsg_, info);
  // 병합 처리 및 콜백 수집
}

각 업데이트 함수의 동작 요약

깊은 이해: https://velog.io/@jk01019/Backend-UpdateObjectsFunctor-UpdatePlacesFunctor-UpdateRoomsFunctor-UpdateBuildingsFunctor

UpdateObjectsFunctor:
- 역할: 객체 노드의 속성(예: 위치, 메쉬 연결 등)을 업데이트하고, 병합 가능한 노드를 제안합니다.
- 동작:
  - 객체 노드의 메쉬 연결 정보를 사용하여 기하학적 속성을 업데이트합니다.
  - 인접한 노드 중 병합 기준에 부합하는 노드를 찾아 병합을 제안합니다.
UpdatePlacesFunctor:
- 역할: 장소 노드의 위치 정보를 업데이트하고, 병합 가능한 노드를 제안합니다.
- 동작:
  - 최적화된 위치 값을 사용하여 장소 노드의 위치를 업데이트합니다.
  - 근처의 다른 장소 노드와의 거리 및 속성을 비교하여 병합을 제안합니다.
UpdateRoomsFunctor:
- 역할: 방을 식별하고, 방 노드를 생성 및 업데이트합니다.
- 동작:
  - 장소 노드 정보를 기반으로 방 탐색 알고리즘을 실행합니다.
  - 발견된 방에 대해 방 노드를 생성하고, 해당 장소 노드와의 관계를 설정합니다.
UpdateBuildingsFunctor:
- 역할: 건물 노드를 생성하고, 방 노드와의 계층 구조를 설정합니다.
- 동작:
  - 방 노드들의 위치를 기반으로 건물의 중심 위치를 계산합니다.
  - 건물 노드를 생성하거나 업데이트하고, 방 노드들과 부모-자식 관계를 설정합니다.

병합 처리 및 콜백 실행

병합 처리:
- 각 업데이트 함수로부터 반환된 병합 제안(MergeList)을 merge_tracker.applyMerges를 통해 적용합니다.
- 병합 과정에서 노드의 속성 업데이트나 삭제 등이 수행됩니다.
콜백 실행:
- 업데이트 함수들이 반환한 콜백 함수들을 실행하여, 추가적인 정리 작업이나 후속 처리를 수행합니다.

요약

BackendModule::optimize 메서드의 4번 단계에서, DSG의 각 레이어를 업데이트하기 위해 다양한 모듈들이 호출됩니다.
에이전트 업데이트 함수와 레이어별 업데이트 함수가 주요한 호출 대상이며, 각각 에이전트 레이어와 다른 레이어들의 업데이트를 담당합니다.
각 업데이트 함수는 레이어별로 특화된 로직을 적용하여 노드와 엣지를 갱신하고, 필요에 따라 노드 병합 등을 수행합니다.
병합 처리 및 콜백 실행을 통해 업데이트 과정에서 발생한 변경 사항을 DSG에 반영하고, 시스템의 일관성을 유지합니다.

이로써 BackendModule::optimize 메서드의 '4. 업데이트된 정보를 기반으로 다른 모듈 호출' 단계에서 어떤 다른 모듈이 호출되는지에 대한 상세한 설명을 제공하였습니다.

About_work

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

이전 포스트

[Backend] hydra code - src/backend/backend_module.cpp - updatePrivateDsg

다음 포스트

[Backend] hydra code - src/backend/backend_module.cpp - optimize

lifelong scene graph

핵심 요약! 이거만 보면돼

BackendModule의 `optimize` 메서드 상세 분석

개요

1. 장소 노드를 변형 그래프에 추가

목적

동작

`addPlacesToDeformationGraph` 함수 상세 설명

2. 변형 그래프 최적화 수행

목적

동작

`DeformationGraph::optimize` 함수 상세 설명

3. DSG 메쉬 업데이트

목적

동작

`DeformationGraph::deformPoints` 함수 상세 설명

4. 업데이트된 정보를 기반으로 다른 모듈 호출

목적

동작 상세 분석

코드 상의 흐름

`callUpdateFunctions` 함수의 상세 동작

1. 그래프 병합 수행

2. 에이전트 업데이트 함수 호출 (`agent_functor_`)

3. 레이어별 업데이트 함수 호출 및 병합 처리 (`layer_functors_`)

4. 콜백 실행

호출되는 모듈 및 업데이트 함수들

에이전트 업데이트 함수 (`agent_functor_`)

레이어별 업데이트 함수 (`layer_functors_`)

각 업데이트 함수의 동작 요약

병합 처리 및 콜백 실행

요약

[Backend] hydra code - src/backend/backend_module.cpp - updatePrivateDsg

[Backend] UpdateObjects/Places/Rooms/Functor

0개의 댓글

[Backend] hydra code - src/backend/backend_module.cpp - optimize

lifelong scene graph

핵심 요약! 이거만 보면돼

BackendModule의 optimize 메서드 상세 분석

개요

1. 장소 노드를 변형 그래프에 추가

목적

동작

addPlacesToDeformationGraph 함수 상세 설명

2. 변형 그래프 최적화 수행

목적

동작

DeformationGraph::optimize 함수 상세 설명

3. DSG 메쉬 업데이트

목적

동작

DeformationGraph::deformPoints 함수 상세 설명

4. 업데이트된 정보를 기반으로 다른 모듈 호출

목적

동작 상세 분석

코드 상의 흐름

callUpdateFunctions 함수의 상세 동작

1. 그래프 병합 수행

2. 에이전트 업데이트 함수 호출 (agent_functor_)

3. 레이어별 업데이트 함수 호출 및 병합 처리 (layer_functors_)

4. 콜백 실행

호출되는 모듈 및 업데이트 함수들

에이전트 업데이트 함수 (agent_functor_)

레이어별 업데이트 함수 (layer_functors_)

각 업데이트 함수의 동작 요약

병합 처리 및 콜백 실행

요약

[Backend] hydra code - src/backend/backend_module.cpp - updatePrivateDsg

[Backend] UpdateObjects/Places/Rooms/Functor

0개의 댓글

BackendModule의 `optimize` 메서드 상세 분석

`addPlacesToDeformationGraph` 함수 상세 설명

`DeformationGraph::optimize` 함수 상세 설명

`DeformationGraph::deformPoints` 함수 상세 설명

`callUpdateFunctions` 함수의 상세 동작

2. 에이전트 업데이트 함수 호출 (`agent_functor_`)

3. 레이어별 업데이트 함수 호출 및 병합 처리 (`layer_functors_`)

에이전트 업데이트 함수 (`agent_functor_`)

레이어별 업데이트 함수 (`layer_functors_`)