핵심요약! 이거만 보면 돼

• updateFromLcdQueue 메서드는 루프 클로저 감지 모듈에서 생성된 새로운 루프 클로저 결과를 백엔드의 큐(backend_lcd_queue)에서 가져와 처리합니다.
• 큐에서 각 루프 클로저를 꺼내어 addLoopClosure 함수를 통해 변형 그래프에 추가합니다.
• 이 과정에서 루프 클로저의 변환 정보를 계산하고, 변형 그래프에 적절한 방식으로 반영합니다.
• 또한 새로운 루프 클로저가 추가되었음을 나타내는 상태 플래그와 카운터를 업데이트하여 시스템이 이를 인식하도록 합니다.

updateFromLcdQueue

목차

1. 루프 클로저란 무엇인가?
2. 함수 개요
3. updateFromLcdQueue 함수 상세 설명
   • 3.1 함수의 목적
   • 3.2 루프 처리 로직
   • 3.3 루프 클로저 데이터 처리
4. addLoopClosure 함수 상세 설명
   • 4.1 함수의 목적
   • 4.2 희소화 여부에 따른 처리
   • 4.3 키 변환 및 변형 계산
5. 전체 흐름과 연관 관계
6. 결론

2. 함수 개요

• updateFromLcdQueue: 루프 클로저 감지 모듈(LCD, Loop Closure Detection)로부터 새로운 루프 클로저 결과를 받아와서 처리합니다.
  • addLoopClosure: 새로운 루프 클로저를 변형 그래프(Deformation Graph)에 추가하고, 필요에 따라 희소화(sparsification)를 처리

5. 전체 흐름과 연관 관계

• 루프 클로저 감지 및 큐에 추가:
  • 루프 클로저 감지 모듈은 새로운 루프 클로저 결과를 생성하고, 이를 backend_lcd_queue에 추가합니다.
• updateFromLcdQueue 함수에서 처리:
  • 큐에서 루프 클로저 결과를 하나씩 꺼내어 처리합니다.
  • 각 루프 클로저에 대해 addLoopClosure 함수를 호출하여 변형 그래프에 반영합니다.
• addLoopClosure 함수에서의 처리:
  • 희소화 여부에 따라 루프 클로저를 적절히 변형 그래프에 추가합니다.
  • 희소화된 그래프에서는 원본 노드 대신 희소 프레임 노드를 사용하여 그래프의 복잡성을 줄입니다.
• 상태 업데이트 및 플래그 설정:
  • 새로운 루프 클로저가 추가되었음을 시스템 상태에 반영하여 이후의 최적화 과정에서 사용됩니다.

---

---

3. updateFromLcdQueue 함수 상세 설명

3.1 함수의 목적

• 루프 클로저 큐(backend_lcd_queue)로부터 새로운 루프 클로저 결과를 받아와서 시스템에 반영
• 새로운 루프 클로저가 추가되었는지 여부를 반환하여 이후 처리 단계에서 사용됩니다.

3.2 루프 처리 로직

bool BackendModule::updateFromLcdQueue() {
  bool added_new_loop_closure = false;
  while (!state_->backend_lcd_queue.empty()) {
    const auto result = state_->backend_lcd_queue.pop();
    // ... 루프 클로저 처리 ...
  }
  return added_new_loop_closure;
}

• 루프: 루프 클로저 큐가 빌 때까지 반복하여 새로운 루프 클로저 결과를 처리합니다.
• added_new_loop_closure 플래그: 새로운 루프 클로저가 추가되었는지 추적하여 반환합니다.

3.3 루프 클로저 데이터 처리

// 포즈 그래프의 관례에 따라 변환 계산
const gtsam::Pose3 to_T_from(gtsam::Rot3(result.to_R_from), result.to_p_from);
LoopClosureLog lc{result.to_node, result.from_node, to_T_from, true, result.level};

// 루프 클로저를 시스템에 추가
addLoopClosure(lc.src,
               lc.dest,
               lc.src_T_dest,
               (result.level ? KimeraPgmoInterface::config_.lc_variance
                             : config.pgmo.sg_loop_closure_variance));

// 루프 클로저 로그에 추가
loop_closures_.push_back(lc);

// 상태 업데이트
added_new_loop_closure = true;
have_loopclosures_ = true;
have_new_loopclosures_ = true;
num_loop_closures_++;
status_.new_loop_closures++;

• 포즈 변환 계산:
  • to_T_from: 루프 클로저 결과에서 얻은 변환 정보를 사용하여 from 노드에서 to 노드로의 변환을 생성합니다.
  • 포즈 그래프의 관례: pose = src.between(dest)를 따릅니다.
• 루프 클로저 로그 생성:
  • LoopClosureLog 구조체에 루프 클로저 정보를 저장합니다.
• 루프 클로저 추가:
  • addLoopClosure 함수를 호출하여 루프 클로저를 변형 그래프에 반영
  • 분산(variance): 루프 클로저의 신뢰도에 따라 분산 값을 선택합니다.
    • result.level이 참이면 KimeraPgmoInterface의 설정 값을 사용하고, 그렇지 않으면 config.pgmo의 설정 값을 사용합니다.
• 상태 플래그 업데이트:
  • 새로운 루프 클로저가 추가되었음을 나타내는 여러 플래그와 카운터를 업데이트합니다.

---

4. addLoopClosure 함수 상세 설명

4.1 함수의 목적

• 루프 클로저를 변형 그래프에 추가합니다.
• 희소화(sparsification) 여부에 따라 루프 클로저를 처리하는 방식을 결정합니다.

4.2 희소화 여부에 따른 처리

void BackendModule::addLoopClosure(const gtsam::Key& src,
                                   const gtsam::Key& dest,
                                   const gtsam::Pose3& src_T_dest,
                                   double variance) {
  if (full_sparse_frame_map_.size() == 0 ||
      !KimeraPgmoInterface::config_.b_enable_sparsify) {
    // 희소화가 비활성화된 경우 또는 희소 프레임 맵이 비어 있는 경우
    deformation_graph_->addNewBetween(src, dest, src_T_dest, gtsam::Pose3(), variance);
  } else {
    // 희소화가 활성화된 경우
    // ... 추가 처리 ...
  }
}

• 희소화 비활성화 또는 초기 상태:
  • 희소화가 비활성화되어 있거나 희소 프레임 맵이 비어 있는 경우, 원본 노드(src, dest)를 사용하여 루프 클로저를 추가합니다.
• 희소화 활성화된 경우:
  • 희소 프레임을 고려하여 루프 클로저를 처리합니다.

4.3 키 변환 및 변형 계산

if (!full_sparse_frame_map_.count(src) || !full_sparse_frame_map_.count(dest)) {
  // 해당 노드가 처리되지 않은 경우 에러 출력 후 종료
  LOG(ERROR)
      << "Attempted to add loop closure with node not yet processed by PGMO.\n";
  return;
}

gtsam::Key sparse_src = full_sparse_frame_map_.at(src);
gtsam::Key sparse_dest = full_sparse_frame_map_.at(dest);

// 희소 프레임에서의 변환 계산
gtsam::Pose3 sparse_src_T_sparse_dest =
    sparse_frames_.at(sparse_src).keyed_transforms.at(src) * src_T_dest *
    sparse_frames_.at(sparse_dest).keyed_transforms.at(dest).inverse();

// 변형 그래프에 루프 클로저 추가
deformation_graph_->addNewBetween(
    sparse_src, sparse_dest, sparse_src_T_sparse_dest, gtsam::Pose3(), variance);

• 노드 존재 여부 확인:
  • 원본 노드(src, dest)가 희소 프레임 맵(full_sparse_frame_map_)에 존재하는지 확인합니다.
  • 존재하지 않으면 에러를 출력하고 함수를 종료합니다.
• 희소 프레임 키 획득:
  • 원본 노드에 대응하는 희소 프레임 노드(sparse_src, sparse_dest)를 가져옵니다.
• 희소 프레임에서의 변환 계산:
  • sparse_src_T_sparse_dest를 계산하여 희소 프레임에서의 변환을 구합니다.
    • 이는 원본 노드의 변환을 희소 프레임의 변환으로 변환한 것입니다.
• 변형 그래프에 루프 클로저 추가:
  • 계산된 변환을 사용하여 희소 프레임 노드 간에 루프 클로저를 추가합니다.

---