• spinOnceImpl 메서드를 중점적으로 보면 됨
• src/loop_closure/detector.cpp 도 참고해야함

input / output

input

• frontend가 채워준 lcd_queue
  • places 정보 (archived_places)
  • 새로 업데이트 된 new_agent_nodes

요약

LoopClosureModule::spinOnceImpl 메서드는 Hydra 시스템에서 루프 클로저 검출을 수행하는 핵심 구성 요소입니다. 그 주요 로직은 다음과 같이 요약할 수 있습니다:

1. 프론트엔드 출력 처리: 메서드는 processFrontendOutput()을 호출하여 프론트엔드로부터 최신 데이터를 처리합니다. 이 함수는 새로운 에이전트 노드와 잠재적인 루프 클로저 루트 노드(potential_lcd_root_nodes_)를 내부 큐에 업데이트합니다.

2. 그래프 동기화: 스레드 안전성을 보장하기 위해 공유된 동적 장면 그래프(DSG)에 대한 뮤텍스 잠금을 획득합니다. DSG의 마지막 업데이트 시간과 현재 타임스탬프가 일치하거나 강제 업데이트가 요청된 경우, 공유된 DSG를 로컬 lcd_graph_에 lcd_graph_->mergeGraph(*dsg.graph)를 사용하여 병합합니다.

3. 루프 클로저 검출 루프: getQueryAgentId(timestamp_ns)를 사용하여 루프 클로저 검출을 위한 준비가 된 에이전트 노드를 가져와 루프를 시작합니다. 이 함수는 에이전트 노드가 추가된 이후 충분한 시간이 지났는지 확인하여 의미 있는 검출을 보장합니다.

4. 디스크립터 캐시 업데이트: 각 에이전트 노드에 대해, 해당 위치를 계산하고 getPlacesToCache(query_pos)를 사용하여 디스크립터 생성 범위(config_.descriptor_creation_horizon_m) 밖에 있는 먼 장소 노드들을 식별합니다. 이러한 노드가 존재하면 displayNodeSymbolContainer(to_cache)를 사용하여 그들의 정보를 로깅하고 lcd_detector_->updateDescriptorCache(*lcd_graph_, to_cache, timestamp_ns)를 호출하여 디스크립터 캐시를 업데이트합니다.

5. 루프 클로저 검출 수행: 에이전트 노드의 타임스탬프를 가져와 lcd_detector_->detect(*lcd_graph_, *query_agent, time.count())를 호출합니다. detect 메서드는 객체와 장소에 대한 캐시된 디스크립터를 사용하여 계층적 디스크립터 매칭을 수행하고, 에이전트 노드에 대한 디스크립터를 구성하여 기존의 것들과 비교함으로써 잠재적인 루프 클로저를 찾습니다.

6. 검출 결과 처리: 검출된 각 유효한 루프 클로저에 대해, 백엔드 루프 클로저 큐에 결과를 state_->backend_lcd_queue.push(result)를 통해 추가하고, LOG(WARNING)을 사용하여 이벤트를 로깅하며 NodeSymbol로 식별된 노드들 간의 성공적인 검출을 표시합니다.

7. 반복적 처리: 메서드는 준비된 에이전트 노드가 더 이상 없거나 종료가 요청될 때까지 에이전트 노드의 처리를 계속합니다. 이를 통해 모든 잠재적인 루프 클로저가 적시에 식별되고 처리되도록 합니다.

전체적으로 spinOnceImpl은 프론트엔드로부터 새로운 데이터를 통합하고, 디스크립터 캐시를 업데이트하며, lcd_detector_의 기능을 활용하여 효율적인 루프 클로저 검출을 수행합니다. displayNodeSymbolContainer 함수는 로깅과 프로세스에 참여하는 노드 추적을 돕고, detect 메서드는 DSG 프레임워크 내에서 루프 클로저 검출에 필요한 핵심 매칭과 검증을 수행합니다.