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

About_work·2024년 10월 18일

lifelong scene graph

목록 보기

33/56

1. 개요

이 코드는 로봇이 탐사해야 할 경계(프론티어)를 추출하는 작업을 수행
이를 바탕으로 로봇의 탐험 경로를 업데이트

코드의 주요 알고리즘 및 구성

1. 블록과 포인트 클라우드를 활용한 경계 탐지

TSDF(Truncated Signed Distance Field) 블록을 기반으로, 이미 탐사된 공간의 블록과 그 주변 경계를 탐색
processBlock() 함수는 각 블록의 경계에 있는 점(voxel)을 식별하고,
- 미탐사 공간과의 연결 여부를 확인해 프론티어로 분류해.

2. 클러스터링을 통한 경계 그룹화

경계점들이 단순히 점의 집합으로 남으면 의미가 없으니, 이를 클러스터링(군집화)해서 유의미한 탐사 단위로 만들기 위해 clusterFrontiers()를 사용해.
PCL 라이브러리의 KdTree와 유클리드 거리 기반의 클러스터링 기법을 활용해 탐사 지점을 그룹화.

3. 공분산과 중심 계산을 통한 경계 분할

splitFrontier() 함수는 프론티어를 두 그룹으로 나누는 작업을 수행해.
- 공분산 행렬과 주성분 분석(PCA)을 활용해, 경계점의 분포를 이해하고 두 개의 하위 그룹으로 분할.
- 이 작업은 로봇이 너무 큰 탐사 경계를 더 작은 단위로 나눌 수 있도록 도와줘.

세부 흐름 정리

1. 경계 탐색 과정: `detectFrontiers()`

TSDF 블록 탐색: processBlock() 함수는 TSDF 블록을 스캔하면서 경계에 있는 점들을 수집해.
주변 블록 탐색:

경계에 있는 점이 다른 블록과 연결될 수 있으면, 해당 블록을 추가 탐사 대상으로 큐에 추가해.

클러스터링 및 분할: 수집된 프론티어들을 클러스터링하고, 더 작은 단위로 나눌 수 있는지 분석해.

2. 블록 및 경계 관리: `updateRecentBlocks()`

최근 탐사한 블록의 위치와 상태를 추적하고, 필요하면 큐에 다시 추가해.

3. 경계 추가: `addFrontiers()`

수집된 프론티어들을 그래프에 추가하여 탐사 경로를 설정하고, 기존에 있던 프론티어는 정리해.

사용된 주요 알고리즘과 기법

TSDF와 Voxel 필터링:
- 탐사된 공간과 미탐사 공간을 Voxel(작은 셀 단위)로 구분해 분석.

2. updateRecentBlocks

frontier_places_->updateRecentBlocks(input.world_t_body, input.map().blockSize());

이 코드는 최근에 탐사된 블록을 업데이트하는 역할을 담당
- 이미 아카이브된(기록된) 블록들 중에서 로봇의 현재 위치와 가까운 블록들만 남겨두고 나머지는 정리

3. 예시

예를 들어 로봇이 (10, 10, 10) 위치에 있고, 블록 크기는 5m라고 하자.
이전에 기록된 블록이 (5, 5, 5)와 (20, 20, 20) 위치에 있다면:
- (5, 5, 5) 블록은 로봇과의 거리가 가까우므로 유지.
- (20, 20, 20) 블록은 너무 멀어 삭제.

2. 알고리즘의 동작 원리

이 함수는 탐사된 블록 중 중요한 블록만 남기고 나머지를 정리하는 역할을 함.
이를 통해 필요한 블록에만 자원을 집중하고, 불필요한 데이터로 인해 발생할 수 있는 성능 저하를 방지해.

거리 기반 필터링:
- 탐사된 블록들이 로봇의 현재 위치에서 가까운지 확인함.
- 먼 거리에 있는 블록들은 삭제하고, 가까운 블록들만 유지함.
동적 경로 관리:
- 로봇이 이동할 때마다 최근에 탐사한 블록을 동적으로 업데이트하여 최신 정보에 맞는 탐사 경로를 설정할 수 있게 함.

detectFrontiers()

이 코드는 로봇이 탐사해야 할 경계(frontier)를 탐지하는 역할을 수행해.
TSDF(Truncated Signed Distance Field)와 그래프 구조를 활용하여 경계 구역을 찾아내고, 이를 데이터 구조에 저장해둠.
로봇이 이동하면서 새로운 블록을 탐지하거나 기존에 탐사된 영역을 관리하는 과정이 포함됨

1. 코드의 주요 동작 흐름

입력 파라미터

input: ReconstructionOutput 객체로, 지도 데이터와 로봇의 위치 등의 정보를 담고 있음.
graph: DynamicSceneGraph로, 로봇의 작업 공간을 표현하는 그래프.
finder: NearestNodeFinder로, 그래프에서 가까운 place 노드를 찾는 기능을 제공.

주요 동작 요약

이전 탐사 결과 초기화
- 새로운 탐사를 시작하기 위해 frontiers_와 archived_frontiers_를 초기화함.
- 이전에 아카이브된 블록은 recently_archived_blocks_에 추가.
TSDF 블록 탐사
- TSDF 필드에 있는 모든 블록을 순회하면서 경계를 탐지하고, 필요시 이웃 블록을 큐에 추가.
추가 블록 처리
- 큐에 들어 있는 블록들을 하나씩 처리하면서, 필요한 경우 추가 블록을 큐에 넣어 탐사.
경계 처리 방식 결정
- 밀집 경계(dense) 또는 희소 경계(sparse) 방식을 선택해 탐지된 경계를 저장.

2. 로직 분석

(2) TSDF 블록 순회 및 탐사

const auto& tsdf = input.map().getTsdfLayer();
for (const auto& idx : tsdf.allocatedBlockIndices()) {
  processBlock(finder, idx, graph, input, archived_places_,
               config.max_place_radius, min_frontier_z, max_frontier_z,
               false, processed_extra_blocks, extra_blocks, cloud, archived_cloud);
}

TSDF 블록 탐사:
- TSDF에 있는 모든 할당된 블록을 순회하며 processBlock() 함수를 호출.
- frontier voxel: places(장애물과 먼 공간) 근처에 있는 voxel 중, 많이 가보지 못한 voxel들 중, 주변이 장애물이 없는 공간이 존재하는 voxel
- 각 블록이 경계인지 확인하고, 필요시 이웃 블록을 extra_blocks 큐에 추가.
- 경계 블록은 cloud와 archived_cloud에 저장됨.

(3) 추가 블록 처리 단계

while (!extra_blocks.empty()) {
  BlockIndex bix = extra_blocks.front();
  extra_blocks.pop();
  bool skip_adding_frontiers = false;

  if (std::find(recently_archived_blocks_.begin(), recently_archived_blocks_.end(), bix) != recently_archived_blocks_.end()) {
    skip_adding_frontiers = true;
  }

  processBlock(finder, bix, graph, input, archived_places_,
               config.max_place_radius, min_frontier_z, max_frontier_z,
               skip_adding_frontiers, processed_extra_blocks, extra_blocks,
               cloud, archived_cloud);
}

추가 블록 큐 처리:
- 큐에 있는 추가 블록을 순서대로 처리.
- 블록이 아카이브된 블록이라면 경계 탐지를 건너뜀.
- 여전히 탐사해야 할 블록이 있다면 다시 큐에 추가.

(4) 밀집 vs 희소 경계 결정

if (config.dense_frontiers) {
  populateDenseFrontiers(cloud, archived_cloud, tsdf.voxel_size, tsdf);
} else {
  computeSparseFrontiers(cloud, config.compute_frontier_shape, tsdf, frontiers_);
  computeSparseFrontiers(archived_cloud, config.compute_frontier_shape, tsdf, archived_frontiers_);
}

밀집 경계와 희소 경계 중 선택:
- dense_frontiers가 참이면 밀집 경계를 계산.
- 거짓이면 희소 경계를 계산하고, 필요한 경우 경계의 형태(shape)도 계산.
밀집 경계 (Dense Frontiers):
- 탐지된 모든 경계를 세밀하게 저장.
- 경계 점을 개별적인 점 클라우드로 관리.
희소 경계 (Sparse Frontiers):
- 탐지된 경계를 군집(cluster)으로 묶어 관리.
- 군집화된 경계들에 대해 중심점(centroid)과 형태(orientation)를 계산.

processBlock 알고리즘 요약

다음과 같은 흐름으로 frontier(경계)가 탐지된다.
frontier voxel: places(장애물과 먼 공간) 근처에 있는 voxel 중, 많이 가보지 못한 voxel들 중, 주변이 장애물이 없는 공간이 존재하는 voxel

핵심 흐름

TSDF 블록 내의 모든 복셀을 순회하면서 근처의 Place 노드(활성/아카이브)와의 관계를 파악한다.
- computeVoxelsInPlace()를 통해 복셀들이 활성 장소 또는 아카이브된 장소에 포함되는지 확인한다.
모든 복셀을 대상으로 삼는 게 아니라, 활성/아카이브된 Place 노드 반경 내에 포함되는 복셀만 필터링한다. (그중에서도, voxel의 weight가 1e-6 이하인 친구들만 포함한다)
- inside_place와 inside_archived_place 배열에 복셀의 상태를 기록한다.
경계(frontier) 여부를 판단:
- 복셀의 이웃이 전부 자유 공간인지 아닌지를 확인한다.
- checkFreeNeighbors() 함수에서 이웃 복셀들을 탐색해, 이웃 중 하나라도 자유 공간으로 확인되면 경계(frontier)로 판단한다.
  - 자유공간: 탐색이 완료된, 장애물이 없는 공간
경계 분류:
- 경계가 속한 블록이 아카이브되었는지 확인.
  - 아카이브된 경계는 archived_cloud에 추가.
  - 활성 경계는 cloud에 추가.

최종 요약

일정 거리 내 Place 노드에 속한 복셀들만 추린다.
그 복셀의 이웃 중 하나라도 자유 공간이면 경계(frontier)로 간주한다.
경계가 아카이브되었는지 여부에 따라 활성/아카이브 경계로 분류한다.

2. addFrontiers

새로운 경계(frontier)를 그래프(graph)에 추가하는 과정
여기서는 경계가 새롭게 탐지된 경우와 아카이브된 경우로 나누어 각각 처리하고 있어.

주요 내용 요약

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

어떤 역할을 하는 코드인가?

이 코드는 탐사 경계를 그래프에 추가하고 관리하는 역할을 해.

탐사 경계 노드와 기존의 Place 노드를 연결함으로써, 탐사 경계와 기존 공간 간의 관계를 파악할 수 있어.
그래프의 일관성을 유지하기 위해 이전에 추가된 탐사 경계를 정리하고, 새로운 경계 노드들을 추가하는 과정이 반복됨.

About_work

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

이전 포스트

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

다음 포스트

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

lifelong scene graph

1. 개요

코드의 주요 알고리즘 및 구성

1. 블록과 포인트 클라우드를 활용한 경계 탐지

2. 클러스터링을 통한 경계 그룹화

3. 공분산과 중심 계산을 통한 경계 분할

세부 흐름 정리

1. 경계 탐색 과정: `detectFrontiers()`

2. 블록 및 경계 관리: `updateRecentBlocks()`

3. 경계 추가: `addFrontiers()`

사용된 주요 알고리즘과 기법

2. updateRecentBlocks

3. 예시

2. 알고리즘의 동작 원리

detectFrontiers()

1. 코드의 주요 동작 흐름

입력 파라미터

주요 동작 요약

2. 로직 분석

(2) TSDF 블록 순회 및 탐사

(3) 추가 블록 처리 단계

(4) 밀집 vs 희소 경계 결정

processBlock 알고리즘 요약

핵심 흐름

최종 요약

2. addFrontiers

주요 내용 요약

어떤 역할을 하는 코드인가?

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

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

0개의 댓글

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

lifelong scene graph

1. 개요

코드의 주요 알고리즘 및 구성

1. 블록과 포인트 클라우드를 활용한 경계 탐지

2. 클러스터링을 통한 경계 그룹화

3. 공분산과 중심 계산을 통한 경계 분할

세부 흐름 정리

1. 경계 탐색 과정: detectFrontiers()

2. 블록 및 경계 관리: updateRecentBlocks()

3. 경계 추가: addFrontiers()

사용된 주요 알고리즘과 기법

2. updateRecentBlocks

3. 예시

2. 알고리즘의 동작 원리

detectFrontiers()

1. 코드의 주요 동작 흐름

입력 파라미터

주요 동작 요약

2. 로직 분석

(2) TSDF 블록 순회 및 탐사

(3) 추가 블록 처리 단계

(4) 밀집 vs 희소 경계 결정

processBlock 알고리즘 요약

핵심 흐름

최종 요약

2. addFrontiers

주요 내용 요약

어떤 역할을 하는 코드인가?

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

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

0개의 댓글

1. 경계 탐색 과정: `detectFrontiers()`

2. 블록 및 경계 관리: `updateRecentBlocks()`

3. 경계 추가: `addFrontiers()`