1. addNewMeshEdgesAndNodes

  deformation_graph_->addNewMeshEdgesAndNodes(new_mesh_edges,
                                              new_mesh_nodes,
                                              new_mesh_node_stamps,
                                              &new_indices,
                                              &new_index_stamps,
                                              config_.mesh_edge_variance);

핵심 개념과 역할

1. 새로운 메쉬 노드와 엣지를 backend의 변형 그래프에 추가
2. 메쉬 노드 간의 관계(엣지)를 정의
3. 누락된 노드나 데이터가 있으면 처리

코드의 흐름

1. 새로운 메쉬 노드 추가

• 코드에서는 입력으로 주어진 메쉬 노드와 그 좌표(위치)를 backend의 변형 그래프에 추가
• 각 노드는 고유한 키(예: gtsam::Symbol)를 가지며, 이 키를 통해 노드의 위치와 시간 정보를 관리

처리 과정

• 새로운 노드가 기존 데이터에 없는 경우에만 추가
• 누락된 노드가 발견되면, 그 위치를 일단 무한대(0,0,0)로 설정해 무시하고 넘어가도록 함
• 각 노드의 좌표와 시간 정보가 저장되며, 이후 분석 및 최적화에 사용

예외 처리

• 만약 새로운 노드의 키(prefix)가 처음 발견된다면, 해당 노드 그룹을 새로 생성한 뒤 추가

---

2. 노이즈 모델 설정

• 엣지(Edge)는 메쉬 노드 간의 연결을 나타냄
• 노드 간의 관계에는 항상 오차(Noise)가 존재하므로, 이 오차를 관리하기 위해 노이즈 모델을 설정
  • 이 코드에서는 등방성 노이즈(Isotropic Noise)를 사용
    • 이는 모든 방향에서 동일한 정도의 오차를 가지는 모델

---

3. 새로운 메쉬 엣지 추가

• 두 노드 간의 연결(엣지)을 추가
  • 예를 들어, 노드 A와 노드 B가 있다면, 두 노드를 연결하는 엣지가 추가
• 각 엣지는 두 노드 사이의 변형 정보를 담아 환경의 변화를 반영

조건부 추가

• 만약 엣지에 포함된 노드 중 하나라도 그래프에 존재하지 않으면 그 엣지는 건너뜀
• 이를 통해 일관되지 않은 데이터가 그래프에 추가되는 것을 방지

---

4. 일관성 관리와 최종 반영

• 새로운 노드와 엣지를 일관성 유지 그래프(consistency graph)에 추가
• 그래프가 완성된 후에는 새로운 요소들을 최종 그래프에 반영

---

2. addNodeValence

      deformation_graph_->addNodeValence(
          gtsam::Symbol(GetRobotPrefix(robot_id), node_val.first),
          node_val.second,
          GetVertexPrefix(robot_id),
          config_.pose_mesh_variance);

코드의 개념적/이론적 설명

• Deformation Graph(변형 그래프)에서 로봇의 포즈와 메쉬 포인트 간의 연결(일관성 요소)을 추가하는 작업을 수행
  • 로봇의 포즈와 메쉬 포인트 간의 관계를 모델링하고 그래프에 반영하는 작업

3. 전체 흐름 요약

1. 주어진 포즈와 연결된 메쉬 점들 중, 유효한 점들을 선별해.
2. 포즈와 메쉬 점 사이의 변형 관계를 양방향으로 정의해.
3. 이 변형 관계를 일관성 요소(factor)로 그래프에 추가.
4. 이렇게 생성된 요소들은 최적화 과정에서 로봇의 이동 경로와 메쉬 간의 일관성을 보장하는 역할을 해.

---

2. 주요 작업 흐름

(1) 각 메쉬 포인트와 포즈 연결 확인

• 주어진 포즈(key)에 대해 연결될 메쉬 포인트 목록(valences)을 순회하면서 포즈와 메쉬 간의 연결이 유효한지 확인해.
  • 만약 해당 메쉬 점이 아직 그래프에 반영되지 않았다면(values_ 또는 new_values_에 없음), 연결을 건너뛰어.

(2) 포즈와 메쉬 포인트 간 관계 정의

• 만약 연결이 유효하다면:
  1. 포즈의 위치: node_pose – 로봇의 초기 위치 정보를 담고 있어.
  2. 메쉬 포인트의 위치: vertex_pose – 메쉬 점의 좌표 정보를 담고 있어.

(3) 일관성 요소(factor) 생성

• 포즈와 메쉬 점 사이의 변형 관계를 두 개의 일관성 요소(factor)로 정의해:

  1. Edge 1: 포즈 → 메쉬 포인트로의 변형 관계
  2. Edge 2: 메쉬 포인트 → 포즈로의 변형 관계

• 이 두 방향의 관계를 정의함으로써, 양방향에서의 일관성을 유지해.

(4) 그래프에 요소 추가

• 생성된 일관성 요소를 consistency_factors_와 new_factors_에 각각 추가해.
  • 이를 통해, 로봇의 포즈와 메쉬 점 간 관계가 변형 그래프에 반영되고 최적화될 수 있어.

---