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

About_work·2024년 10월 16일

lifelong scene graph

목록 보기

20/56

1. updateNodeInGraph

updateNodeInGraph 함수와 관련된 여러 함수를 통해
- 새로 검지된 mesh를 target node의 mesh 연결 정보에 병합하고, 물체의 기하학적 속성(중심점, 경계 상자 등)을 업데이트하는 것

전체적인 흐름

updateNodeInGraph:
- 노드의 업데이트 시간과 활성 상태를 갱신하고, 메쉬 연결 정보를 병합한 후, 노드의 기하학적 속성(경계 상자 및 중심점)을 업데이트
mergeList:
- 메쉬 연결 정보를 병합할 때 중복된 인덱스를 제외하고 병합
updateObjectGeometry:
- 노드의 메쉬 연결 정보를 사용해 경계 상자를 계산하고, 중심점을 업데이트
updateNodeCentroid:
- 주어진 메쉬 인덱스를 기반으로 유효한 좌표들의 평균을 계산해 노드의 중심점을 설정

결론

이 코드는 그래프 내 노드의 메쉬 연결 정보와 기하학적 속성(중심점, 경계 상자)을 업데이트하는 일련의 과정을 수행합니다. 이를 통해 노드가 메쉬 데이터에 맞게 정확히 업데이트되며, 이러한 과정은 로봇이나 3D 공간에서의 객체 추적 및 관리에 매우 유용합니다.

1. `updateNodeInGraph` 함수

void MeshSegmenter::updateNodeInGraph(DynamicSceneGraph& graph,
                                      const Cluster& cluster,
                                      const SceneGraphNode& node,
                                      uint64_t timestamp) {
  auto& attrs = node.attributes<ObjectNodeAttributes>();
  attrs.last_update_time_ns = timestamp;
  attrs.is_active = true;

  mergeList(attrs.mesh_connections, cluster.indices);
  updateObjectGeometry(*graph.mesh(), attrs);
}

목적: 이 함수는 주어진 클러스터와 기존 노드의 메쉬 연결 정보를 병합하고, 해당 노드의 기하학적 속성(예: 중심점, 경계 상자)을 업데이트합니다.
주요 단계:
1. 속성 업데이트:
  - node.attributes<ObjectNodeAttributes>(): 해당 노드의 속성(ObjectNodeAttributes)을 가져옵니다.
  - attrs.last_update_time_ns = timestamp;: 노드의 최종 업데이트 시간을 현재 타임스탬프로 설정합니다.
  - attrs.is_active = true;: 노드를 활성 상태로 설정합니다.
2. 메쉬 연결 병합:
  - mergeList(attrs.mesh_connections, cluster.indices);: 기존 노드의 메쉬 연결 정보와 클러스터의 메쉬 인덱스 목록을 병합합니다.
3. 기하학적 속성 업데이트:
  - updateObjectGeometry(*graph.mesh(), attrs);: 노드의 기하학적 속성(중심점과 경계 상자)을 업데이트합니다.

2. `mergeList` 함수

void mergeList(LList& lhs, const RList& rhs) {
  std::unordered_set<size_t> seen(lhs.begin(), lhs.end());
  for (const auto idx : rhs) {
    if (seen.count(idx)) {
      continue;
    }

    lhs.push_back(idx);
    seen.insert(idx);
  }
}

목적: 이 함수는 두 개의 리스트를 병합하되, 중복되는 요소는 제외하고 병합합니다.
입력:
- lhs: 기존의 리스트 (여기에 새로운 인덱스들이 추가됩니다).
- rhs: 추가할 인덱스들을 포함한 리스트.
주요 단계:
1. 중복 방지: 먼저 기존 리스트(lhs)의 요소들을 unordered_set(seen)으로 변환하여 중복 체크에 사용합니다.
2. 병합: rhs의 각 인덱스를 확인하면서, 중복되지 않는 경우에만 lhs에 추가합니다.
  - 중복된 인덱스는 추가하지 않습니다(continue).

3. `updateObjectGeometry` 함수

bool updateObjectGeometry(const spark_dsg::Mesh& mesh,
                          ObjectNodeAttributes& attrs,
                          const std::vector<size_t>* indices,
                          std::optional<BoundingBox::Type> type) {
  std::vector<size_t> mesh_connections;
  if (!indices) {
    mesh_connections.assign(attrs.mesh_connections.begin(),
                            attrs.mesh_connections.end());
  }

  const BoundingBox::MeshAdaptor adaptor(mesh, indices ? indices : &mesh_connections);
  attrs.bounding_box = BoundingBox(adaptor, type.value_or(attrs.bounding_box.type));
  if (indices) {
    return updateNodeCentroid(mesh, *indices, attrs);
  } else {
    return updateNodeCentroid(mesh, mesh_connections, attrs);
  }
}

목적: 노드의 메쉬 연결 정보를 기반으로 경계 상자(bounding box)와 중심점(centroid)을 업데이트하는 역할을 합니다.
주요 단계:
1. 메쉬 연결 정보 설정:
  - indices가 주어졌다면 해당 인덱스를 사용하고, 그렇지 않다면 attrs.mesh_connections에 저장된 메쉬 연결 정보를 사용합니다.
2. 경계 상자 업데이트:
  - BoundingBox::MeshAdaptor를 사용하여, 메쉬 정보를 기반으로 경계 상자를 계산합니다.
  - attrs.bounding_box = BoundingBox(adaptor, type.value_or(attrs.bounding_box.type));: 경계 상자를 업데이트합니다. 여기서 type이 주어지면 이를 사용하고, 그렇지 않으면 기존 경계 상자 타입을 사용합니다.
3. 중심점 업데이트:
  - updateNodeCentroid 함수를 사용해 메쉬 연결 정보를 기반으로 노드의 중심점을 업데이트합니다.

4. `updateNodeCentroid` 함수

bool updateNodeCentroid(const spark_dsg::Mesh& mesh,
                        const std::vector<size_t>& indices,
                        NodeAttributes& attrs) {
  size_t num_valid = 0;
  Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
  for (const auto idx : indices) {
    const auto pos = mesh.pos(idx).cast<double>();
    if (!pos.array().isFinite().all()) {
      continue;
    }

    centroid += pos;
    ++num_valid;
  }

  if (!num_valid) {
    return false;
  }

  attrs.position = centroid / num_valid;
  return true;
}

목적: 이 함수는 메쉬 연결 정보를 기반으로 노드의 중심점(centroid)을 계산하여 업데이트하는 역할을 합니다.
주요 단계:
1. 유효한 메쉬 인덱스 검사:
  - 각 메쉬 인덱스(idx)에 대해 해당 메쉬의 좌표(pos)를 가져옵니다.
  - pos.array().isFinite().all(): 좌표가 유효한 값인지 확인합니다. 유효하지 않다면 그 좌표는 건너뜁니다(continue).
2. 중심점 계산:
  - 유효한 좌표의 경우, centroid에 좌표를 더해줍니다.
  - 유효한 좌표의 개수를 num_valid에 기록합니다.
3. 평균 중심점 계산:
  - 유효한 좌표가 하나도 없으면 false를 반환합니다.
  - 유효한 좌표가 존재하면, 평균 중심점을 계산해 노드의 위치(attrs.position)에 업데이트합니다.

2.

위 코드는 MeshSegmenter 클래스에서 새로운 노드를 그래프에 추가하고, 메쉬와 관련된 기하학적 정보(경계 상자 및 중심점)를 업데이트하는 과정을 설명합니다. 이 과정은 주어진 클러스터에 대해 새로운 객체 노드를 생성하고, 해당 노드의 속성을 설정하며, 객체의 기하학적 속성을 계산하는 절차로 구성됩니다.

1. `addNodeToGraph` 함수

이 함수는 주어진 클러스터의 정보에 따라 새로운 객체 노드를 그래프에 추가하는 역할을 합니다. 클러스터는 3D 공간에서 여러 포인트의 집합으로, 이를 기반으로 노드를 생성하고, 기하학적 속성을 설정한 뒤, 그래프에 삽입합니다.

주요 단계:

빈 클러스터 처리:

만약 클러스터에 속한 인덱스들이 없다면(cluster.indices.empty()), 즉 클러스터가 비어 있다면, 에러 메시지를 출력하고 함수를 종료합니다.

if (cluster.indices.empty()) {
    LOG(ERROR) << "Encountered empty cluster with label" << static_cast<int>(label)
               << " @ " << timestamp << "[ns]";
    return;
}

객체 노드 속성 초기화:
- ObjectNodeAttributes 구조체를 사용해 새로운 속성 객체(attrs)를 생성하고, 여러 속성들을 설정합니다.
- timestamp, is_active 등의 기본 정보를 설정합니다.
- semantic_label: 객체에 레이블(semantic label)을 부여합니다. 이 레이블은 객체가 어떤 카테고리에 속하는지를 나타냅니다.
- name: 노드의 이름을 설정합니다. GlobalInfo를 통해 레이블에 해당하는 이름을 찾고, 해당 이름이 없다면 디폴트로 NodeSymbol을 사용해 노드 ID에 기반한 이름을 설정합니다.
```
auto attrs = std::make_unique<ObjectNodeAttributes>();
attrs->last_update_time_ns = timestamp;
attrs->is_active = true;
attrs->semantic_label = label;
attrs->name = NodeSymbol(next_node_id_).getLabel();

const auto& label_to_name = GlobalInfo::instance().getLabelToNameMap();
auto iter = label_to_name.find(label);
if (iter != label_to_name.end()) {
    attrs->name = iter->second;
} else {
    VLOG(2) << "Missing semantic label from map: " << std::to_string(label);
}
```
메쉬 연결 정보 설정:
- attrs->mesh_connections: 클러스터에서 얻은 메쉬 연결 정보를 노드의 속성에 추가합니다. 여기서 mesh_connections는 노드가 어떤 메쉬 인덱스들과 연결되는지에 대한 정보를 저장하는 리스트입니다.
```
attrs->mesh_connections.insert(
    attrs->mesh_connections.begin(), cluster.indices.begin(), cluster.indices.end());
```

객체 색상 설정:

label_map을 사용해 객체의 레이블에 맞는 색상을 설정합니다. GlobalInfo에서 레이블-색상 맵을 가져와 사용하며, 색상이 유효하지 않다면 랜덤한 색상 맵을 생성합니다.

auto label_map = GlobalInfo::instance().getSemanticColorMap();
if (!label_map || !label_map->isValid()) {
    label_map = GlobalInfo::instance().setRandomColormap();
    CHECK(label_map != nullptr);
}
attrs->color = label_map->getColorFromLabel(label);

객체의 기하학적 속성 업데이트:
- updateObjectGeometry 함수를 호출해 객체의 경계 상자와 중심점을 업데이트합니다. 이때 graph.mesh()를 사용해 메쉬 정보를 가져와 계산에 사용합니다.
```
updateObjectGeometry(*graph.mesh(), *attrs, nullptr, config.bounding_box_type);
```
그래프에 노드 추가:
- graph.emplaceNode를 사용해 새로 생성한 노드를 그래프에 삽입합니다. 그리고 해당 노드를 활성 노드 목록에 추가합니다. 마지막으로 노드 ID를 증가시켜, 다음에 생성할 노드에 대비합니다.
```
graph.emplaceNode(config.layer_id, next_node_id_, std::move(attrs));
active_nodes_.at(label).insert(next_node_id_);
++next_node_id_;
```

2. `updateObjectGeometry` 함수

이 함수는 주어진 메쉬 정보를 사용해 객체의 기하학적 속성(경계 상자와 중심점)을 업데이트합니다.

주요 단계:

메쉬 연결 정보 설정:

indices가 주어지면 그 값을 사용하고, 그렇지 않다면 기존의 메쉬 연결 정보를 사용합니다.

std::vector<size_t> mesh_connections;
if (!indices) {
    mesh_connections.assign(attrs.mesh_connections.begin(),
                            attrs.mesh_connections.end());
}

경계 상자 업데이트:
- BoundingBox::MeshAdaptor를 사용해, 메쉬 정보를 경계 상자로 변환합니다. 여기서 type 인자는 경계 상자의 타입(AABB, OBB 등)을 정의하는 선택적 매개변수입니다.
```
const BoundingBox::MeshAdaptor adaptor(mesh, indices ? indices : &mesh_connections);
attrs.bounding_box = BoundingBox(adaptor, type.value_or(attrs.bounding_box.type));
```
중심점 업데이트:
- updateNodeCentroid 함수를 사용해 주어진 메쉬 연결 정보를 기반으로 중심점을 계산하고 업데이트합니다.
- indices가 주어졌다면 이를 사용해 중심점을 계산하고, 그렇지 않다면 기존 메쉬 연결 정보를 사용합니다.
```
if (indices) {
    return updateNodeCentroid(mesh, *indices, attrs);
} else {
    return updateNodeCentroid(mesh, mesh_connections, attrs);
}
```

전체적인 알고리즘 흐름

addNodeToGraph 함수는 새로운 클러스터에 대해 새로운 객체 노드를 생성하고, 해당 노드의 속성(레이블, 이름, 메쉬 연결 정보 등)을 설정합니다.
객체 색상을 설정하고, updateObjectGeometry 함수를 사용해 기하학적 속성(경계 상자 및 중심점)을 계산합니다.
노드를 그래프에 삽입하고, 해당 노드를 활성 노드 목록에 추가한 뒤, 노드 ID를 증가시켜 준비합니다.
updateObjectGeometry 함수는 객체의 메쉬 정보를 기반으로 경계 상자를 계산하고, 중심점을 업데이트하는 역할을 합니다.

이 과정을 통해 메쉬 데이터를 기반으로 객체를 그래프에 삽입하고, 해당 객체의 기하학적 속성을 갱신하여 3D 공간에서의 정확한 위치와 크기를 파악할 수 있게 됩니다.

3. mergeActiveNodes

활성화된 노드 중 겹치는 객체들을 병합하는 역할
- 즉, 유사하거나 동일한 객체를 나타내는 여러 노드를 하나로 병합하여 그래프를 정리하는 과정
이 과정을 통해 중복된 객체를 제거하고, 메쉬 정보를 기반으로 객체의 경계 상자 및 중심점을 업데이트

전체 알고리즘 요약

활성화된 노드를 순회하며, 유사하거나 겹치는 노드들을 찾아냅니다.
겹치는 노드들을 하나로 병합하며, 병합된 노드들은 그래프에서 삭제하고, 메쉬 연결 정보를 통합
병합 후 객체의 기하학적 정보(경계 상자 및 중심점)를 업데이트하여 정확한 객체 속성을 유지
병합된 노드들을 활성화된 노드 목록에서 제거하여, 더 이상 처리되지 않도록

알고리즘 로직 및 역할 상세 설명

1. 병합된 노드 트래킹

merged_nodes라는 셋(Set)을 사용하여 이미 병합된 노드를 기록합니다. 이 기록을 통해 이미 병합된 노드를 중복 처리하는 것을 방지합니다.
```
std::set<NodeId> merged_nodes;
```

2. 현재 활성 노드 가져오기

현재 활성화된 노드들을 가져옵니다. active_nodes_는 각 레이블에 대한 활성 노드를 기록하는 맵입니다. 주어진 레이블(label)에 속하는 활성화된 노드 리스트를 가져오며, 병합 과정을 이 리스트에 대해 수행합니다.
```
auto& curr_active = active_nodes_.at(label);
```

3. 활성 노드 순회

활성화된 노드들에 대해 순차적으로 순회하면서 병합이 필요한 노드들을 찾아냅니다. 이때 이미 병합된 노드는 merged_nodes에 포함되어 있으므로, 이를 확인하여 중복 처리를 방지합니다.
```
for (const auto& node_id : curr_active) {
  if (merged_nodes.count(node_id)) {
    continue;
  }
  const auto& node = graph.getNode(node_id);
```

4. 병합할 노드 탐색

현재 순회 중인 노드에 대해 병합이 가능한 다른 노드들을 탐색합니다. 다른 노드들과 비교하여, 두 노드가 유사하거나 겹치는지를 확인하는데, 이를 위해 nodesMatch 함수를 사용합니다.
nodesMatch 함수는 객체의 경계 상자 등을 사용하여 두 노드가 같은 객체를 나타내는지 판단하는 역할을 합니다.

만약 노드들이 일치하거나 겹친다면, 해당 노드는 병합 대상으로 추가됩니다.

std::list<NodeId> to_merge;
for (const auto& other_id : curr_active) {
  if (node_id == other_id) {
    continue;
  }

  if (merged_nodes.count(other_id)) {
    continue;
  }

  const auto& other = graph.getNode(other_id);
  if (nodesMatch(node, other) || nodesMatch(other, node)) {
    to_merge.push_back(other_id);
  }
}

5. 노드 병합

병합 대상 노드들(to_merge)에 대해, 메쉬 연결 정보(mesh_connections)를 병합합니다. 이를 통해 하나의 객체가 속한 모든 메쉬 포인트 정보가 병합된 노드에 포함됩니다.
mergeList 함수를 사용해 메쉬 연결 정보 리스트를 병합합니다.

이후, 병합된 다른 노드(other_id)는 그래프에서 삭제되며, merged_nodes에 해당 노드를 기록하여 중복 처리 방지에 사용됩니다.

auto& attrs = node.attributes<ObjectNodeAttributes>();
for (const auto& other_id : to_merge) {
  const auto& other = graph.getNode(other_id);
  auto& other_attrs = other.attributes<ObjectNodeAttributes>();
  mergeList(attrs.mesh_connections, other_attrs.mesh_connections);
  graph.removeNode(other_id);
  merged_nodes.insert(other_id);
}

6. 기하학적 속성 업데이트

병합된 노드에 대해, 메쉬 연결 정보가 갱신되었으므로 경계 상자 및 중심점 등의 기하학적 정보를 다시 계산해야 합니다.
updateObjectGeometry 함수를 호출하여 객체의 기하학적 속성을 업데이트합니다. 이 함수는 메쉬 정보를 사용해 객체의 경계 상자와 중심점을 계산합니다.
```
if (!to_merge.empty()) {
  updateObjectGeometry(*graph.mesh(), attrs);
}
```

7. 병합된 노드 목록에서 제거

마지막으로, 병합된 노드들은 활성화된 노드 목록에서 제거됩니다. 이 작업을 통해 병합된 노드는 더 이상 활성화된 노드로 간주되지 않습니다.
```
for (const auto& node_id : merged_nodes) {
  curr_active.erase(node_id);
}
```

About_work

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

이전 포스트

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

다음 포스트

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

lifelong scene graph

1. updateNodeInGraph

전체적인 흐름

결론

1. updateNodeInGraph 함수

2. mergeList 함수

3. updateObjectGeometry 함수

4. updateNodeCentroid 함수

2.

1. addNodeToGraph 함수

주요 단계:

2. updateObjectGeometry 함수

주요 단계:

전체적인 알고리즘 흐름

3. mergeActiveNodes

전체 알고리즘 요약

알고리즘 로직 및 역할 상세 설명

1. 병합된 노드 트래킹

2. 현재 활성 노드 가져오기

3. 활성 노드 순회

4. 병합할 노드 탐색

5. 노드 병합

6. 기하학적 속성 업데이트

7. 병합된 노드 목록에서 제거

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