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

About_work·2024년 10월 17일
0

lifelong scene graph

목록 보기
29/56
  • detect()와 updateGraph() 파악을 위함

1. 개요

이 코드는 GVD(Generalized Voronoi Diagram)를 사용해 로봇 탐색을 위한 장소(places)를 추출하고, DSG를 업데이트

1. GVD를 이용한 장소 추출의 핵심 흐름

  • GVD(GVD Place Extractor)는 로봇의 주변 환경을 거리 기반 필드(TSDF)로 분석해 장애물과 자유 공간 간의 경계와 연결된 장소들을 추출하는데 활용돼.

2. 주요 동작 흐름과 로직

2.1. TSDF 거리 필드 사용과 다운샘플링

  1. 입력 메시지에서 TSDF 데이터를 추출:
  2. 필요할 경우 TSDF를 다운샘플링:
    • TSDF 데이터가 너무 많아 계산이 비효율적일 경우, 인터폴레이션을 통해 필요한 부분만 추출.
    • 이 과정에서 갱신된 블록을 효율적으로 선택해 처리 시간을 줄임.

2.2. GVD 초기화 및 위치 추적

  1. GVD와 GVD 통합기(integrator) 초기화:
    • GvdLayer는 TSDF 데이터로부터 각 지점의 거리 정보를 계산해 경계를 정의
    • GvdIntegrator는 이 레이어를 활용해 환경에서 새로운 장소와 경계 정보를 갱신
  2. 로봇의 최신 위치 정보 갱신:
    • 메시지에 포함된 월드 좌표계와 로봇의 변환 정보를 사용해 현재 위치를 추적.

2.3. GVD 통합과 장소 업데이트

  1. GVD 정보 갱신:

    • GVD 통합기가 TSDF에서 경계를 계산하고, 새로운 장소와 경계를 업데이트.
    • 블록 단위로 데이터를 보관해 이전 블록이 삭제되거나 오래된 경우, 이를 아카이빙 처리함.

  2. 장소 탐지 후 후속 작업:

    • GVD가 계산된 장소를 기반으로 그래프 추출기(graph extractor)를 통해 DSG에 노드와 엣지를 추가.

2.4. 장소와 경계 정보의 유효성 필터링

  1. 유효하지 않은 노드 필터링:

    • NaN 값이 포함된 노드나 위치가 올바르지 않은 노드들은 제거.
    • 각 노드의 거리, 위치, 메쉬 연결 정보가 정확한지 검사함.

  2. 고립된 노드 제거:

    • 다른 노드들과 연결되지 않은 고립된 노드들은 그래프에서 제거.
    • 이 과정에서 연결 성분 분석을 통해 고립된 구성 요소를 감지.

  3. 지면에 너무 가까운 노드 필터링:

    • 로봇의 높이 기준으로 지면과 너무 가까운 노드들은 유효하지 않은 것으로 판단하고 제거.

2.5. 새로운 장소와 엣지 추가

  1. 새로운 엣지 생성:

    • 필터링된 노드들 간에 유효한 거리 내에 있는 노드들 사이에 엣지를 생성.
    • 이 과정에서 최대 거리 제한과 최소 간격 기준을 충족하는 엣지만 연결.

  2. 동적 장면 그래프(DSG)에 반영:

    • 새롭게 탐지된 장소와 이를 연결하는 엣지들을 DSG에 추가해 환경 모델을 업데이트.

3. 이 코드의 의미와 중요성

  • 이 코드는 GVD를 통해 장애물과 자유 공간의 경계를 효율적으로 계산하고, 로봇의 탐색 경로를 최적화해.
  • 또한 장소 정보를 동적으로 관리하면서, 새로운 장소가 탐지되거나 고립된 장소가 생기는 상황에 빠르게 대응할 수 있어.

4. 추가로 깊이 있게 살펴볼 부분

  • GvdIntegrator와 관련된 코드를 보면 GVD가 어떻게 계산되고 업데이트되는지 더 자세히 이해할 수 있을 거야.
  • TSDF 인터폴레이션 로직을 살펴보면, 어떻게 데이터를 효율적으로 샘플링하고 관리하는지 알 수 있어.
  • graph_extractor를 통해 각 장소가 어떻게 그래프 형태로 추출되고 연결되는지도 분석하면 좋아.

이해가 잘 되었길 바라! 더 궁금한 부분이 있으면 말해줘.

detect()

4. 결론

  • 이 코드는 TSDF와 GVD를 활용해 로봇의 탐색 및 경계 관리를 효율적으로 수행하는 핵심 로직을 구현
  • 특히 동적 그래프를 관리하면서 장애물과 자유 공간 간의 경계를 실시간으로 계산하고, 이를 다양한 후속 작업에 연결하는 모듈 간 통합이 잘 이뤄져 있어.

2. 함수의 동작을 단계별로 분석

2.1 TSDF 데이터 준비

  • TSDF 레이어 추출: msg 객체로부터 TSDF 레이어를 가져와 환경 정보를 로드해.
  • TSDF 다운샘플링:
    • tsdf_interpolator_가 활성화된 경우, 업데이트된 블록만 선택해 인터폴레이션을 수행해.

2.2 GVD 초기화 및 업데이트 준비

  • 만약 GVD와 통합기(integrator)가 초기화되지 않았다면, 새로운 GvdLayer 객체를 생성해.
  • GVD 통합기(GvdIntegrator)는 TSDF를 사용해 새로운 장소와 경계를 계산하고, 이를 그래프에 반영해.
  • 로봇의 월드 좌표계에서의 위치(world_T_body)를 최신 값으로 갱신해 로봇의 이동 경로를 반영해.

2.3 TSDF를 기반으로 GVD 업데이트

  • GVD 통합기에서 환경 정보 업데이트:
    • TSDF 데이터로부터 새로운 경계를 계산하고, 장소와 장애물 간의 경계를 추출해.
    • 통합 과정에서 오래된 블록은 아카이브 처리해, 더 이상 사용되지 않는 데이터의 메모리 점유를 최소화해.

updateGraph()

  • 이 함수는 GVD 데이터에서 생성된 장소 정보(place information)를 동적 장면 그래프(Dynamic Scene Graph, DSG)에 통합하고, 실시간으로 변화하는 노드와 엣지를 관리

1. Graph Extractor에서 활성 노드와 그래프 데이터 가져오기

  • graph_extractor_는 GVD를 기반으로 현재 활성 노드들과 해당 노드들로 구성된 그래프를 제공해.
  • 이 노드들은 현재 로봇 주변에서 사용 가능한 장소 정보를 포함하며, 이는 로봇이 경로를 계획할 때 중요한 요소가 돼.
  • 이후 filterInvalidNodes를 사용해 유효하지 않은 노드들을 걸러냄으로써 그래프의 정확성을 보장해.

2. 삭제된 노드와 엣지 처리

  • 노드 및 엣지 삭제 처리:
    • 삭제된 노드는 그래프에서 제거되며, 그와 연결된 형제 노드(sibling nodes)도 네트워크에서 관리돼.
    • 이웃 노드와 엣지를 통해 연결된 활성 이웃(neighborhood)이 유지되도록 보장해.
  • 엣지 삭제:
    • 삭제된 엣지는 그래프 구조에서 제거되며, 이 과정에서 연결 관계가 끊어지는 것을 막기 위해 이웃 노드 간의 연결을 재조정해.
    • 삭제 작업은 노드 간의 위상 연결(Topology)을 반영하여 그래프의 일관성을 유지해.

3. 활성 노드와 엣지 추가 및 갱신

  • 새 노드 추가 또는 기존 노드 갱신:

    • active_nodes_에 있는 노드들은 DSG에 추가되거나 갱신돼.
    • 각 노드는 활성 상태로 표시되고(is_active = true), 해당 노드의 마지막 갱신 시간이 기록돼(last_update_time_ns).

  • 형제 노드와의 엣지 갱신:

    • 각 노드가 다른 노드들과 어떻게 연결되는지 추적하기 위해 형제 노드와의 엣지도 갱신돼.
    • 엣지 정보도 복제(clone)하여 최신 상태를 유지하며, 노드 간의 위상 정보를 일관되게 유지해.

4. 선택적 필터링: Ground 및 Isolated 노드 필터링

  • Ground 필터링:

    • filterGround는 일정 높이 이하에 있는 노드를 필터링해. 이는 로봇의 위치와 관련해 불필요한 지면 노드를 제거하여 경로 탐색을 최적화하기 위함이야.

  • Isolated 노드 필터링:

    • filterIsolated는 고립된 노드를 감지해 제거해. 고립된 노드는 로봇이 탐색 중 활용할 수 없는 노드들이기 때문에, 연결된 구성 요소로서의 의미를 잃은 경우 제거돼.

5. 그래프 데이터 초기화와 정리

  • 모든 처리 과정이 끝난 후 graph_extractor_->clearDeleted()를 호출해 삭제된 데이터들을 초기화해.
  • 이를 통해 불필요한 메모리 사용을 방지하고, 새로운 데이터로의 원활한 갱신을 보장해.

결론: 실시간 장소 정보 갱신의 핵심

이 함수는 실시간 환경 변화에 적응할 수 있도록 장소 정보를 효율적으로 관리해.
1. 삭제와 추가를 반복하며 노드와 엣지를 최신 상태로 유지하고,
2. 필터링을 통해 불필요한 노드와 엣지를 제거하여 탐색을 최적화하며,
3. 장소 간 연결을 재조정함으로써 그래프의 일관성을 유지해.

이러한 구조는 로봇이 동적 환경에서도 정확한 경로를 탐색하고 의사결정을 내릴 수 있도록 도와줘. DSG 기반의 동적 그래프는 다양한 상황에서 로봇이 효율적으로 동작하도록 지원하는 중요한 데이터 구조야.

1. filterGround

GvdPlaceExtractor::filterGround 알고리즘 심층 분석

이 함수는 로봇이 인식한 장소 데이터에서 지면(ground)와 관련된 불필요한 노드와 엣지를 걸러내어 그래프를 최적화하는 역할을 수행해. 이는 로봇의 탐색 성능을 개선하고 효율적인 경로 계획을 가능하게 만드는 중요한 기능이야. 아래는 이 알고리즘의 각 핵심 개념과 단계에 대한 깊이 있는 설명이야.

1. 지면 임계값 계산 (max_z)

  • max_z는 로봇의 현재 위치(높이)를 기준으로 계산된 최대 허용 높이를 의미해.
    • 이 값은 로봇의 높이(config.robot_height)와 허용 오차(config.node_tolerance)를 반영해 설정돼.
    • max_z보다 낮은 노드만 유효한 장소로 간주돼, 로봇이 탐색 가능한 공간을 결정하는 중요한 기준이야.

2. 유효하지 않은 노드와 엣지 탐색

노드 필터링

  • 각 활성 노드를 순회하며 해당 노드의 z-축 좌표에서 distance를 뺀 값(curr_min_z)가 max_z보다 큰지 확인해.
    • curr_min_z > max_z일 경우, 해당 노드는 유효하지 않은 노드(invalid)로 간주하고 제거 목록에 추가돼.
    • 이는 지면에 너무 가까운 장애물이나 탐색할 필요 없는 노드를 제거하여 탐색 효율을 높이기 위함이야.

엣지 필터링

  • 각 노드에 연결된 형제 노드(sibling)와의 엣지도 검사해.
    • 엣지의 양 끝점 중 하나라도 max_z보다 높다면, 해당 엣지를 제거 목록에 추가해.
    • 이는 높은 장애물에 대한 경로를 제거하여, 로봇이 불필요한 경로를 탐색하지 않도록 한다는 의미야.

3. 유효하지 않은 노드와 엣지 제거

  • 유효하지 않은 노드와 엣지는 제거되고, 이는 그래프에서 실시간으로 반영돼.
    • 노드를 제거할 때, 해당 노드와 연결된 모든 엣지도 함께 제거돼.
    • 제거된 노드는 activenodes 집합에서도 삭제하여 그래프의 일관성을 유지해.

4. 새로운 엣지 생성: Free Space Edge 탐색

  • 필터링 이후, freespace(자유 공간) 엣지를 재탐색하고 추가해.
    • 이 과정은 places::findFreespaceEdges를 통해 수행되며, 이는 노드 간 새로운 연결을 탐색하고 생성해.
    • 새로운 엣지를 추가할 때에도 max_z 조건을 확인하여 높이가 허용되는 엣지만 추가해.

5. 최적화된 경로 탐색 지원

  • 이 필터링 과정은 불필요한 노드와 엣지를 제거하여 그래프의 밀도를 낮추고, 로봇이 탐색할 필요가 없는 경로를 줄여줘.
  • 이로써 로봇은 보다 적은 계산 자원으로 효율적인 경로를 탐색할 수 있게 돼.

결론: 탐색 효율성 극대화

  • 이 함수는 동적으로 변화하는 환경에서 로봇이 탐색할 필요가 없는 지면 근처의 장애물과 노드를 걸러내고,
    • 자유 공간(free space) 엣지를 재탐색하여 그래프의 일관성과 효율성을 유지해.
  • 이러한 과정은 경로 계획 최적화와 탐색 성능 향상에 큰 기여를 하며, 로봇이 동적 환경에서도 신속하게 대응할 수 있도록 도와줘.

filterIsolated

GvdPlaceExtractor::filterIsolated 알고리즘 심층 분석

이 함수는 그래프에서 고립된 작은 구성 요소를 찾아 제거하는 것을 목표로 해. 이는 탐색에 필요 없는 경로와 장소를 줄여 그래프의 복잡성을 최소화하고 탐색 성능을 향상하는 중요한 과정이야. 이 알고리즘이 수행하는 주요 작업과 그 의의를 단계별로 설명할게.

1. 활성 이웃 노드 정제 (Active Neighborhood Filtering)

  • active_neighborhood 집합에 포함된 노드들이 실제로 존재하는지 확인하여 불필요한 항목을 제거해.
    • 이 과정은 이전 업데이트 이후 삭제된 노드나 잘못된 노드가 포함되는 것을 방지해.
    • 이로써 그래프의 상태가 일관성 있게 유지되며, 이후 단계에서 오류를 피할 수 있어.
  • 이 단계에서는 활성 장소 노드들과 관련된 서브그래프에서 연결된 구성 요소를 탐색해.
    • graph_utilities::getConnectedComponents 함수를 통해, 모든 활성 노드 및 관련된 아카이브된 노드들을 포함해 연결된 서브그래프를 찾는다.
    • 이때, 최소 허용 구성 요소 크기(config.min_component_size)를 기준으로 탐색을 제한하여 불필요한 계산을 줄여.

이 단계의 목적

  • 탐색 효율성 향상: 고립된 작은 구성 요소는 탐색에 유용하지 않으므로 제거한다.
  • 연속성 보장: 이전에 활성화된 아카이브 노드들과 연결된 구성 요소는 탐색에 유용할 수 있어 유지한다.

3. 고립된 구성 요소 제거 (Removing Isolated Components)

  • 각 구성 요소의 크기를 확인하고, 최소 허용 크기보다 작은 구성 요소는 제거해.
    • 작은 구성 요소는 로봇의 탐색 경로에 유의미하지 않은 정보일 가능성이 높기 때문이야.
    • to_delete 리스트에 추가된 노드는 그래프에서 제거되고, 동시에 activenodes 집합에서도 삭제돼.

4. 최적화된 그래프 유지

  • 이 과정을 통해 불필요한 노드와 엣지를 제거하여 그래프의 복잡도를 낮추고 탐색 성능을 최적화해.
  • 특히 탐색 가능한 경로를 유지하면서도, 고립된 구성 요소를 정리해 경로 탐색과 관련된 리소스를 절약할 수 있어.

결론: 탐색 경로의 간소화

  • 고립된 작은 구성 요소는 탐색 효율을 저하시킬 수 있으므로 제거해야 해.
  • 이 함수는 탐색 성능과 효율성을 최적화하기 위해, 그래프에서의 일관성 유지와 불필요한 노드 제거를 균형 있게 수행하는 중요한 역할을 해.
  • 이로써, 로봇은 실제 탐색에 필요한 경로만 고려하여 빠르고 신뢰성 있는 탐색을 수행할 수 있게 돼.
profile
About_work
새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.
이전 포스트

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

다음 포스트

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

0개의 댓글