[frontend] hydra code - src/frontend/mesh_segmenter.cpp

About_work·2024년 10월 16일

lifelong scene graph

목록 보기

21/56

detect, updateGraph 위주로 보기

핵심 요약

MeshSegmenter는 3D 메쉬 데이터를 기반으로 특정 라벨에 대한 객체 클러스터를 탐지하고, 이를 동적 씬 그래프(DSG)에 반영하는 모듈
기존 객체는 업데이트하고, 새로운 객체는 DSG에 추가하며, 동일 객체는 병합하여 관리
Bounding box를 사용해 객체의 기하학적 정보를 계산하고, 메쉬와 연결된 포인트 정보를 추적

주요 구성 요소

detect 함수: 메쉬에서 객체를 감지하는 역할.
updateGraph 함수: 감지된 객체 클러스터를 DSG에 업데이트하거나, 새로운 노드를 추가
mergeActiveNodes 함수: 비슷한 객체 노드를 병합.
getActiveNodes 함수: 현재 활성화된 노드의 ID를 반환.
updateNodeInGraph 함수: 기존 노드를 업데이트.
addNodeToGraph 함수: 새로운 노드를 DSG에 추가.

각 구성 요소에 대한 로직을 자세히 설명합니다.

1. `MeshSegmenter::detect`

pos 가 주어지면 -> 로봇 근처의 vertices만 봅니다! (7m)
하지만, 코드를 보니 주어지지 않았습니다.

LabelClusters MeshSegmenter::detect(uint64_t timestamp_ns,
                                    const kimera_pgmo::MeshDelta& delta,
                                    const std::optional<Eigen::Vector3d>& pos) {}

결론

detect 함수는 메쉬 데이터를 기반으로 특정 라벨에 해당하는 클러스터를 탐지하는 역할
이 과정에서 먼저 활성화된 메쉬 인덱스를 가져오고, 라벨별로 메쉬 포인트들을 필터링한 후, 유클리드 클러스터링을 통해 라벨별 클러스터를 탐지
탐지된 클러스터들은 라벨을 기준으로 사전(LabelClusters)에 저장되며, 이를 외부에서 사용할 수 있도록 반환

주요 단계와 알고리즘 로직 설명

2. 활성화된 메쉬 인덱스 가져오기 (`getActiveIndices`)

const auto indices = getActiveIndices(delta, pos, config.active_index_horizon_m);

getActiveIndices() 함수를 사용하여 활성화된 메쉬 포인트의 인덱스를 가져옵니다.
- delta: 메쉬의 변화(delta) 정보를 담고 있으며, 이는 현재 탐지하려는 메쉬 데이터를 나타냄
- pos: 탐지 범위의 중심점입니다. 만약 이 값이 존재하면, 그 주변에 있는 메쉬 포인트들을 활성화된 인덱스로 간주합니다.
- config.active_index_horizon_m: 탐지 범위를 나타냅니다. pos가 주어지면, 이 범위 내에 있는 포인트들만 활성화된 인덱스로 처리
indices는 활성화된 메쉬 포인트들의 인덱스 리스트이며, 나중에 클러스터링에 사용

4. 라벨별로 인덱스를 필터링 (`getLabelIndices`)

const auto label_indices = getLabelIndices(config, delta, indices);

getLabelIndices() 함수를 통해, 활성화된 인덱스 중에서 라벨에 해당하는 메쉬 포인트들을 필터링
- delta: 메쉬 데이터의 변화 정보로, 각 메쉬 포인트가 어떤 라벨(semantic label)을 가지고 있는지 정보를 포함
- indices: 활성화된 메쉬 인덱스 목록
- config.labels: 감지하려는 대상 라벨들의 리스트입니다. 예를 들어, 자동차, 사람 등 특정 객체 라벨에 해당하는 포인트들만 감지
label_indices는 각 라벨별로 활성화된 메쉬 포인트들의 인덱스를 저장한 사전(딕셔너리) 형태

6. 라벨별 클러스터 탐색

for (const auto label : config.labels) {
  if (!label_indices.count(label)) {
    continue;
  }

  if (label_indices.at(label).size() < config.min_cluster_size) {
    continue;
  }

  const auto clusters = findClusters(config, delta, label_indices.at(label));

  VLOG(2) << "[Mesh Segmenter]  - Found " << clusters.size()
          << " cluster(s) of label " << static_cast<int>(label);
  label_clusters.insert({label, clusters});
}

탐지 대상인 라벨들(config.labels)을 순회하며, 라벨에 해당하는 메쉬 포인트들을 클러스터링

클러스터 탐지 로직

최소 클러스터 크기 조건 확인: 특정 라벨에 해당하는 메쉬 포인트 수가 config.min_cluster_size(최소 클러스터 크기)보다 작다면, 해당 라벨에 대한 클러스터링을 수행하지 않고 건너뜁니다.
클러스터링 수행: findClusters() 함수를 호출하여 해당 라벨에 대한 메쉬 포인트들로 클러스터를 생성

이는 유클리드 거리 기반의 클러스터링 알고리즘을 사용하여, 서로 가까운 포인트들을 하나의 클러스터로 묶음

결과 저장: 탐지된 클러스터들은 label_clusters라는 사전에 라벨별로 저장됩니다.

즉, 각 라벨에 대해 하나 이상의 클러스터가 있을 수 있음

2. `MeshSegmenter::updateGraph` 함수

void MeshSegmenter::updateGraph(uint64_t timestamp_ns,
                                const LabelClusters& clusters,
                                size_t num_archived_vertices,
                                DynamicSceneGraph& graph) {}

탐지된 클러스터 데이터를 기반으로 동적 씬 그래프(Dynamic Scene Graph, DSG)를 업데이트하는 역할
이 함수는 메쉬 클러스터를 노드로 변환하여 그래프에 추가하거나, 기존 노드를 갱신하고 병합하는 작업을 수행

함수의 전체 흐름

이전 노드 아카이브: 활성화되지 않은 오래된 노드들을 비활성화 처리
클러스터 순회: 탐지된 클러스터들에 대해 순회하며 처리
기존 노드와의 일치 여부 검사:

각 클러스터가 기존 그래프에 이미 있는 노드와 일치하는지 확인하고, 일치하면 해당 노드를 갱신

새로운 노드 추가: 일치하는 노드가 없다면, 새로 노드를 추가
노드 병합: 중복되는 노드가 없도록, 경계 상자가 겹치는 노드들을 병합

주요 단계와 알고리즘 로직 설명

2. 이전 노드 아카이브 처리 (`archiveOldNodes`)

archiveOldNodes(graph, num_archived_vertices);

archiveOldNodes() 함수는 그래프의 이전 노드들 중에서 더 이상 활성화되지 않은 노드를 아카이브 처리
- graph: 현재 동적 씬 그래프
- num_archived_vertices: 이 값은 아카이브 처리 기준을 결정하며,
  - 일정 수 이상의 메쉬 포인트를 처리한 이후에 해당하는 노드들을 더 이상 활성 노드로 간주하지 않게 만듭니다.
이 단계는 그래프 내에서 오래되거나 불필요해진 노드를 비활성화하고 제거하는 과정

3. 탐지된 클러스터 순회

for (auto&& [label, clusters_for_label] : clusters) {

clusters는 라벨별로 탐지된 클러스터들을 저장한 구조
- label: 각 클러스터에 해당하는 라벨을 나타냅니다.
- clusters_for_label: 해당 라벨에 속하는 클러스터들의 리스트
함수는 이 클러스터들을 순회하면서, 그래프에 추가하거나 기존 노드를 갱신하는 작업을 수행

4. 각 클러스터에 대해 처리

for (const auto& cluster : clusters_for_label) {

라벨에 해당하는 모든 클러스터들을 순회하며, 각 클러스터를 처리

5. 기존 노드와의 일치 여부 검사

bool matches_prev_node = false;
std::vector<NodeId> nodes_not_in_graph;
for (const auto& prev_node_id : active_nodes_.at(label)) {
  const auto& prev_node = graph.getNode(prev_node_id);
  if (nodesMatch(cluster, prev_node)) {
    updateNodeInGraph(graph, cluster, prev_node, timestamp_ns);
    matches_prev_node = true;
    break;
  }
}

현재 클러스터와 이전에 그래프에 존재하는 노드가 일치하는지 검사하는 과정

노드 일치 검사 과정

active_nodes_.at(label)를 통해 해당 라벨에 해당하는 이전에 추가된 노드들을 가져옵니다.
각 노드를 순회하며 nodesMatch() 함수를 사용해 클러스터와 노드가 일치하는지 확인
- nodesMatch() 함수는
  - 클러스터의 중심점이 이전 노드의 경계 상자(bounding box) 내에 포함되는지 여부를 확인
만약 일치하는 노드가 발견되면, 해당 노드를 갱신합니다.
- updateNodeInGraph(graph, cluster, prev_node, timestamp_ns) 함수를 호출해, 기존 노드의 메쉬 포인트 연결 및 기타 속성들을 갱신
  - https://velog.io/@jk01019/frontend-hydra-code-srcfrontendmeshsegmenter.cpp-detail
  - target node에, 이번에 검지된 label node의 mesh를 병합하고, bb와 center을 업데이트 한다는 내용이다.
- 이후, matches_prev_node를 true로 설정하고, 더 이상의 검사를 중단합니다(break).

6. 새로운 노드 추가

if (!matches_prev_node) {
  addNodeToGraph(graph, cluster, label, timestamp_ns);
}

만약 현재 클러스터와 일치하는 기존 노드가 없었다면, 새로운 노드를 그래프에 추가합니다.
addNodeToGraph() 함수는 새로운 노드를 생성하고, 해당 클러스터와 라벨, 그리고 현재 타임스탬프를 기반으로 그래프에 추가
- https://velog.io/@jk01019/frontend-hydra-code-srcfrontendmeshsegmenter.cpp-detail

7. 노드 병합 (`mergeActiveNodes`)

mergeActiveNodes(graph, label);

그래프에 추가된 노드들은 mergeActiveNodes() 함수를 통해 병합됩니다.
같은 라벨을 가진 노드들 중에서 경계 상자가 겹치거나 중복되는 노드들을 하나로 병합합니다.
https://velog.io/@jk01019/frontend-hydra-code-srcfrontendmeshsegmenter.cpp-detail

archiveOldNodes

void MeshSegmenter::archiveOldNodes(const DynamicSceneGraph& graph,
                                    size_t num_archived_vertices) {}

동적 씬 그래프(Dynamic Scene Graph, DSG) 내에서 더 이상 활성 상태로 유지되지 않는 오래된 노드들을 아카이브(제거)하는 역할

전체 흐름

라벨별로 활성 노드를 순회하여,

노드가 여전히 그래프에 존재하는지, 그리고 활성화 상태인지 확인

그래프에 없는 노드는 제거 리스트에 추가
노드가 그래프에 존재하지만 활성화되지 않은 상태라면, 활성화 상태를 갱신하고 제거 리스트에 추가
최종적으로 제거 리스트에 있는 노드들은 활성 노드 목록에서 삭제

주요 단계와 알고리즘 로직

2. 라벨별로 노드 처리

for (const auto& label : config.labels) {

이 코드는 라벨(label) 별로 노드를 처리합니다.
- config.labels: 그래프에서 관리 중인 라벨 목록
- 각 라벨에 해당하는 노드들을 순회하며, 활성화 여부를 결정

4. 활성화된 노드 확인

for (const auto& node_id : active_nodes_.at(label)) {

각 라벨에 대해 활성화된 노드 목록(activenodes)을 가져와 순회
- active_nodes_.at(label)은 해당 라벨에 속하는 활성 노드들의 집합
- 이 순회에서 각 노드가 여전히 활성화 상태인지 확인

5. 그래프에서 해당 노드가 존재하는지 확인

if (!graph.hasNode(node_id)) {
  removed_nodes.push_back(node_id);
  continue;
}

해당 노드가 그래프 내에 존재하는지 확인합니다.
- 만약 해당 노드가 그래프에 없다면, 더 이상 유효하지 않은 노드이므로 제거 리스트(removed_nodes)에 추가

6. 노드의 속성 가져오기

auto& attrs = graph.getNode(node_id).attributes<ObjectNodeAttributes>();

그래프 내에서 해당 노드의 속성(attributes)을 가져옵니다.
- ObjectNodeAttributes는 노드가 가지고 있는 속성으로,
  - 이 속성에는 노드의 메쉬 연결 정보(mesh_connections)와 활성화 상태(is_active) 등이 포함되어 있음

7. 노드의 활성 상태 여부 확인

bool is_active = false;
for (const auto index : attrs.mesh_connections) {
  if (index >= num_archived_vertices) {
    is_active = true;
    break;
  }
}

노드가 활성 상태인지 확인하는 과정입니다.
- attrs.mesh_connections: 이 노드가 연결된 메쉬 포인트의 인덱스 목록
- 메쉬 연결 인덱스들이 num_archived_vertices보다 크면, 해당 노드는 여전히 활성화된 상태
  - num_archived_vertices는 아카이브된 메쉬 포인트의 수를 나타냅니다.
    - 즉, 이 숫자보다 큰 인덱스를 가진 메쉬 연결이 있으면, 해당 노드는 아카이브되지 않은 활성화된 상태로 간주

8. 노드의 활성 상태 업데이트

attrs.is_active = is_active;
if (!attrs.is_active) {
  removed_nodes.push_back(node_id);
}

is_active 값에 따라 노드의 활성 상태를 업데이트합니다.
- attrs.is_active = is_active;: is_active 값을 노드 속성의 활성 상태에 적용합니다.
- 만약 노드가 더 이상 활성 상태가 아니라면, 제거 리스트(removed_nodes)에 추가

9. 활성화되지 않은 노드 제거

for (const auto& node_id : removed_nodes) {
  active_nodes_[label].erase(node_id);
}

앞서 제거 리스트(removed_nodes)에 추가된 노드들을 active_nodes_에서 제거합니다.
- active_nodes_[label].erase(node_id): 해당 라벨에 속한 활성 노드 목록에서, 제거할 노드들을 삭제합니다.

About_work

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

이전 포스트

[frontend] hydra code - src/frontend/mesh_segmenter.cpp detail

다음 포스트

[frontend] hydra code - src/frontend/mesh_segmenter.cpp

lifelong scene graph

핵심 요약

주요 구성 요소

1. `MeshSegmenter::detect`

결론

주요 단계와 알고리즘 로직 설명

2. 활성화된 메쉬 인덱스 가져오기 (`getActiveIndices`)

4. 라벨별로 인덱스를 필터링 (`getLabelIndices`)

6. 라벨별 클러스터 탐색

클러스터 탐지 로직

2. `MeshSegmenter::updateGraph` 함수

함수의 전체 흐름

주요 단계와 알고리즘 로직 설명

2. 이전 노드 아카이브 처리 (`archiveOldNodes`)

3. 탐지된 클러스터 순회

4. 각 클러스터에 대해 처리

5. 기존 노드와의 일치 여부 검사

노드 일치 검사 과정

6. 새로운 노드 추가

7. 노드 병합 (`mergeActiveNodes`)

archiveOldNodes

전체 흐름

주요 단계와 알고리즘 로직

2. 라벨별로 노드 처리

4. 활성화된 노드 확인

5. 그래프에서 해당 노드가 존재하는지 확인

6. 노드의 속성 가져오기

7. 노드의 활성 상태 여부 확인

8. 노드의 활성 상태 업데이트

9. 활성화되지 않은 노드 제거

[frontend] hydra code - src/frontend/mesh_segmenter.cpp detail

hydra code - src/frontend/place_2d_segmenter.cpp

0개의 댓글

[frontend] hydra code - src/frontend/mesh_segmenter.cpp

lifelong scene graph

핵심 요약

주요 구성 요소

1. MeshSegmenter::detect

결론

주요 단계와 알고리즘 로직 설명

2. 활성화된 메쉬 인덱스 가져오기 (getActiveIndices)

4. 라벨별로 인덱스를 필터링 (getLabelIndices)

6. 라벨별 클러스터 탐색

클러스터 탐지 로직

2. MeshSegmenter::updateGraph 함수

함수의 전체 흐름

주요 단계와 알고리즘 로직 설명

2. 이전 노드 아카이브 처리 (archiveOldNodes)

3. 탐지된 클러스터 순회

4. 각 클러스터에 대해 처리

5. 기존 노드와의 일치 여부 검사

노드 일치 검사 과정

6. 새로운 노드 추가

7. 노드 병합 (mergeActiveNodes)

archiveOldNodes

전체 흐름

주요 단계와 알고리즘 로직

2. 라벨별로 노드 처리

4. 활성화된 노드 확인

5. 그래프에서 해당 노드가 존재하는지 확인

6. 노드의 속성 가져오기

7. 노드의 활성 상태 여부 확인

8. 노드의 활성 상태 업데이트

9. 활성화되지 않은 노드 제거

[frontend] hydra code - src/frontend/mesh_segmenter.cpp detail

hydra code - src/frontend/place_2d_segmenter.cpp

0개의 댓글

1. `MeshSegmenter::detect`

2. 활성화된 메쉬 인덱스 가져오기 (`getActiveIndices`)

4. 라벨별로 인덱스를 필터링 (`getLabelIndices`)

2. `MeshSegmenter::updateGraph` 함수

2. 이전 노드 아카이브 처리 (`archiveOldNodes`)

7. 노드 병합 (`mergeActiveNodes`)