| 171 | } |
| 172 | |
| 173 | size_t LoopClosureModule::processFrontendOutput() { |
| 174 | const auto& msg = state_->lcd_queue->front(); |
| 175 | VLOG(5) << "[Hydra LCD] Received archived places: " |
| 176 | << displayNodeSymbolContainer(msg->archived_places); |
| 177 | |
| 178 | potential_lcd_root_nodes_.insert(potential_lcd_root_nodes_.end(), |
| 179 | msg->archived_places.begin(), |
| 180 | msg->archived_places.end()); |
| 181 | |
| 182 | VLOG(5) << "[Hydra LCD] Adding nodes: " |
| 183 | << displayNodeSymbolContainer(msg->new_agent_nodes); |
| 184 | for (const auto& node : msg->new_agent_nodes) { |
| 185 | agent_queue_.push(node); |
| 186 | } |
| 187 | |
| 188 | size_t timestamp_ns = msg->timestamp_ns; |
| 189 | state_->lcd_queue->pop(); |
| 190 | return timestamp_ns; |
| 191 | } |
| 192 | |
| 193 | NodeIdSet LoopClosureModule::getPlacesToCache(const Eigen::Vector3d& agent_pos) { |
| 194 | NodeIdSet to_cache; |