hydra 코드 분석 - backend

About_work·2024년 10월 11일
0

lifelong scene graph

목록 보기
4/56

0. 상세 설명

-1. 정리

  • Frontend에서 구축한 DSG, deformation graph(mesh)를 input으로 받아,
    • 로봇의 경로 최적화 및 환경의 3D 메쉬 변형을 관리
  • 로직
    • 디포메이션 그래프(Deformation Graph)를 사용하여 루프 클로저로 인한 누적 오류를 수정하고,
    • 동적 씬 그래프(Dynamic Scene Graph, DSG)를 계층적으로 관리하며,
      • 이 과정에서, 환경에 대한 객체(2), 장소(3), 방(4) 등의 공간 정보를 실시간으로 업데이트
  • semantic object 업데이트 로직?
    • loop closure 감지 -> mesh 오류 수정 최적화
    • 기존 object들이 어떤 mesh vertices와 연결되어있는지가 저장되어 있으므로,
      • mesh 오류가 수정되면 -> object의 위치들도 자동으로 업데이트
    • object 의 위치가 업데이트 된 후 -> object간 병합 로직을 수행
      • 같은 semantic label 중에서, bounding box간 overlap이 기준 이상이면 -> object 병합

1. src/backend/backend_module.cpp

2. 주요 구성 요소 및 데이터 구조

  • FactorGraph는 실시간으로 업데이트되지만, 그 결과가 DSG에 간헐적으로 반영됩니다.

2.1. Deformation Graph

  • 목적: 로봇의 경로와 환경 메쉬의 일관성을 유지하면서 루프 클로저(loop closure)로 인한 누적 오류를 수정
  • 구성 요소:
    • 노드(Node): 로봇의 포즈(2), 메쉬의 컨트롤 포인트(1), 장소 노드(3) 등이 포함
    • 엣지(Edge): 노드 간의 관계를 나타내며, 상대 변환 또는 제약 조건을 인코딩
  • 동작 원리:
    • 최적화 과정을 통해 노드들의 포즈를 조정하여 전체 메쉬와 로봇의 경로를 일관되게 변형

3. 메인 루프 (spin 함수)

  • 동작 과정:
    1. 입력 데이터 처리: state_->backend_queue에서 입력 데이터를 가져옴
    • mesh(1), deformation graph, agent(2) 가 포함되어 있는 것으로 파악
    1. 팩터 그래프 업데이트: updateFactorGraph를 호출하여
    • mesh(1), 로봇의 경로(2), 루프 클로저 등의 정보를 디포메이션 그래프에 반영
    1. 루프 클로저 처리:
    • updateFromLcdQueue를 통해 새로운 루프 클로저를 확인하고 처리
    1. 메쉬 업데이트:
    • 필요 시 copyMeshDelta를 호출하여 메쉬의 변경 사항을 반영
    1. DSG 업데이트 결정:
    • 현재 시점에서 DSG를 업데이트할지 여부를 판단
    1. 최적화 수행:
    • 루프 클로저가 발생한 경우 optimize를 호출하여 디포메이션 그래프를 최적화하고, DSG의 각 레이어를 업데이트
    1. 결과 반영:
    • 최적화 결과를 DSG 및 메쉬에 반영하고, 필요한 경우 다른 모듈로 데이터를 전송

3.2. 팩터 그래프 업데이트 (updateFactorGraph 함수)

  • mesh, 로봇의 경로, 루프 클로저 정보 를 factor graph에 추가
    • 변수 node
      • 파란색 원(x₁, x₂, x₃, ...): 각 로봇의 포즈(위치와 자세)를 나타내는 변수
      • 녹색 원(l₁, l₂, ...): 환경 내 랜드마크의 위치를 나타내는 변수들
      • 빨간색 원(K): 카메라의 내재 매개변수(intrinsic parameters)
    • factor node
      • u₁, u₂: 로봇의 연속된 포즈 간의 관계를 나타내는 오도메트리 팩터(odometry factors)
      • v₁, v₂, v₃, v₄: 로봇 포즈와 랜드마크 위치 간의 관계를 나타내는 카메라 관측 팩터(visual factors)
      • c₁, c₂: 루프 클로저(loop closure)를 나타내는 팩터들
  • 목적: 새로운 팩터(제약 조건)와 노드를 디포메이션 그래프에 추가하여 최적화에 필요한 정보를 갱신합니다.
  • 동작 과정:
    1. 입력 데이터에서 팩터 추출: 오도메트리, 루프 클로저 등의 정보를 추출
    2. 디포메이션 그래프 업데이트: processIncrementalMeshGraph와 processIncrementalPoseGraph를 호출하여 mesh/pose에 대한 새로운 노드와 엣지를 디포메이션 그래프에 추가
    3. 루프 클로저 처리:
    • 새로운 루프 클로저가 있는지 확인하고, 있다면 addLoopClosure를 통해 디포메이션 그래프에 추가
    1. 상태 업데이트:
    • 새로운 팩터의 수, 루프 클로저의 수 등을 상태 변수에 반영

3.3. 루프 클로저 처리(updateFromLcdQueue)

  • 목적: 루프 클로저가 감지되면 -> 디포메이션 그래프와 DSG에 이를 반영하여 누적된 오류를 수정
  • 동작 과정:
    1. 루프 클로저 수신: state_->backend_lcd_queue에서 루프 클로저 결과를 가져옴
    2. 디포메이션 그래프에 추가: addLoopClosure를 호출하여 루프 클로저를 디포메이션 그래프에 엣지로 추가
    3. 상태 업데이트: 새로운 루프 클로저의 수를 상태 변수에 반영

3.4. 메쉬 업데이트 및 변형

  • 목적: 루프 클로저로 인한 환경의 변화에 대응하여 메쉬를 변형
  • 동작 과정:
    1. 메쉬 델타 복사: copyMeshDelta를 통해 프론트엔드에서 전달된 메쉬 업데이트를 백엔드의 DSG 메쉬에 반영
    2. 디포메이션 그래프를 통한 변형: updateDsgMesh를 호출하여 디포메이션 그래프의 최적화 결과를 메쉬에 적용
    3. 메쉬의 일관성 유지: 메쉬의 버텍스 좌표를 디포메이션 그래프의 변환을 통해 업데이트하여, 루프 클로저로 인한 오류를 수정

3.6. 최적화 수행 (optimize 함수)

  • 목적: 디포메이션 그래프를 최적화하여 로봇의 경로(pose)와 메쉬(mesh)의 변형을 동시에 일관되게 조정
  • 동작 과정:
    1. 장소 노드 추가: addPlacesToDeformationGraph를 통해 장소 노드들을 디포메이션 그래프에 추가
      • 최소 스패닝 트리(MST) 생성: 장소 노드 간의 연결성을 유지하기 위해 MST를 생성하고, 이를 디포메이션 그래프에 반영
    2. 최적화 실행: 디포메이션 그래프의 optimize 함수를 호출하여 최적화를 수행
      • 사용된 알고리즘: 일반적으로 비선형 최적화 기법(예: Gauss-Newton 방법)을 사용하여 에러 함수를 최소화
    3. 메쉬 업데이트: 최적화 결과를 바탕으로 updateDsgMesh를 호출하여 메쉬를 변형
    4. DSG의 레이어 업데이트: callUpdateFunctions를 통해 DSG의 각 레이어를 업데이트하는 함수들을 호출합니다.

3.7. DSG 레이어 업데이트 (callUpdateFunctions 함수)

  • 목적: DSG의 각 레이어(객체(2), 장소(3), 방(4), 건물(5) 등)를 최적화 결과에 따라 업데이트합니다.
  • 동작 과정:
    1. 업데이트 정보 생성: 최적화 결과와 현재 상태를 포함하는 UpdateInfo 객체를 생성
    2. 레이어별 업데이트 함수 호출: layer_functors_에 등록된 각 레이어의 업데이트 함수를 호출합니다.
      • 객체 레이어: 객체 노드들의 위치 및 속성을 업데이트하고, 필요에 따라 병합합니다.
      • 장소 레이어: 장소 노드들의 위치를 업데이트하고, 인접한 노드들을 병합하여 그래프의 간소화를 수행
      • 방 및 건물 레이어: 공간적 구조를 분석하여 새로운 방이나 건물을 생성하거나 업데이트합니다.
    3. 병합 추적 및 처리: 노드 병합으로 인한 변경 사항을 추적하고, DSG에 반영합니다.

4. 세부 알고리즘 및 최적화 기법

한 줄 요약

  • 디포메이션 그래프 최적화는 로봇이 맵을 정확히 인식하도록 오류를 줄이는 과정
  • MST는 가장 짧은 경로로 장소들을 연결해, 불필요한 경로를 없애고 효율적으로 이동하도록 도와줘.

4.1. 디포메이션 그래프 최적화 (Deformation Graph Optimization)

  • 쉽게 설명하자면:
    • 로봇이 공간을 탐사하다 보면, 같은 장소를 여러 번 방문할 때가 있어.
    • 하지만 센서가 정확하지 않을 수 있기 때문에, 방문할 때마다 조금씩 다른 위치로 인식하게 돼.
    • 이게 누적되면 맵에 오류가 생길 수 있어.
    • 그래서 디포메이션 그래프 최적화는 이런 오류를 최소화해 맵을 정확하게 수정하는 과정
  • 데이터 적합성:
    • 그래프에서 각 노드(로봇의 위치나 메쉬의 점들)가 서로 어떻게 움직였는지 측정한 데이터와 최대한 일치하도록 맞추는 거야.
  • 정규화 항목:
    • 인접한 노드들끼리 갑자기 멀어지거나 이상하게 움직이지 않도록, 부드럽게 연결되도록 조정
  • 어떻게 최적화하나?
    • 비선형 최소제곱 문제라는 수학적 방법으로 오류를 줄이는 거야.
    • 이를 위해 Gauss-Newton 방법이나 Levenberg-Marquardt 방법 같은 기술을 사용해.
    • 쉽게 말해, 오차를 점점 줄여가며 가장 좋은 위치를 찾아내는 과정이야.

4.2. 최소 스패닝 트리(MST)로 장소 노드 연결

  • 쉽게 설명하자면:

    • 여러 장소를 연결할 때, 가장 짧은 거리만으로 연결하는 방법이 있어.
    • 이걸 최소 스패닝 트리(MST)라고 불러.
    • 목표는 모든 장소를 연결하되 불필요한 경로를 최소화하는 거야.

  • 어떻게 연결하나?

    • Prim 알고리즘: 한 노드에서 출발해, 점점 가까운 노드를 연결해 나가.
    • Kruskal 알고리즘: 모든 가능한 경로를 정렬해서, 가장 짧은 경로들부터 연결해.

  • 엣지 가중치란?

    • 두 장소 간의 유클리드 거리(직선 거리)를 가중치로 사용해.
      • 예를 들어, 가까운 장소들은 작은 값, 먼 장소들은 큰 값을 부여해.

  • 왜 필요할까?

    • 그래프의 복잡도를 줄이고, 로봇이 빠르고 효율적으로 장소를 탐색하도록 돕기 위해서야.
    • 이렇게 만든 MST를 디포메이션 그래프에 추가하면, 장소 간 이동 경로가 더 효율적으로 관리돼.

4.3. 메쉬 변형

  • 목적: 최적화 결과를 메쉬에 반영하여 환경의 3D 표현을 업데이트합니다.
  • 동작 과정:
    • 버텍스 좌표 업데이트: 디포메이션 그래프의 노드 변환을 보간하여 메쉬의 버텍스 좌표를 변형
    • 보간 방법: 각 버텍스에 대해 인접한 디포메이션 그래프 노드들의 변환을 가중치 합하여 적용합니다.
      • 가중치는 노드와 버텍스 사이의 거리 등에 기반합니다.
    • 일관성 유지: 메쉬의 변형이 부드럽고 일관되게 이루어지도록 보장합니다.

4.4. 노드 병합 및 그래프 간소화

  • 목적: 그래프의 복잡도를 줄이고 계산 효율성을 향상시킴
  • 알고리즘:
    • 유사성 기준: 위치, 속성, 의미적 정보 등을 고려하여 유사한 노드들을 식별
    • 병합 조건: 일정 거리 이내에 있고, 속성이 유사한 노드들을 병합
    • 병합 처리: 병합된 노드의 속성을 업데이트하고, 그래프의 구조를 수정
  • 추적 및 취소:
    • 병합 추적기: 병합된 노드들의 정보를 추적하여 필요 시 병합을 취소할 수 있도록 함
    • 이벤트 처리: 새로운 정보나 루프 클로저로 인해 병합이 부적절해진 경우 병합을 취소하고 그래프를 재구성

2. object(semantic) 관련 내용

profile
About_work
새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.
이전 포스트

hydra 용어 정리 - Factor Graph

다음 포스트

hydra 용어 정리 - deformation Graph

0개의 댓글