From 18f373b4497f32159904c827e92b060f7379d312 Mon Sep 17 00:00:00 2001 From: ZhiangQi Date: Mon, 11 Aug 2025 22:30:50 +0800 Subject: [PATCH] fix(posegraph): Resolve segfault in loop closure visualization A race condition between the 20Hz visualization timer and the registration thread caused a segmentation fault when a loop closure attempt failed. The root cause was that the `need_lc_cloud_vis_update_` flag was set to `true` immediately after performing registration, but before checking if the registration was valid. The asynchronous visualization timer would then read this flag, attempt to access data from the failed registration (which contained null pointers for clouds like `FinalAlignedCloud`), and crash. This commit resolves the issue through a two-pronged approach: 1. **Corrected Logic:** The `need_lc_cloud_vis_update_` flag is now only set to `true` inside the `if (reg_output.is_valid_)` block, ensuring that the visualization is only triggered for successful loop closures. 2. **Defensive Programming:** Added null pointer checks within the `visualizeLoopClosureClouds` function itself. This makes the visualizer more robust and prevents crashes even if similar logic errors occur in the future. --- ros/src/slam/pose_graph_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros/src/slam/pose_graph_manager.cpp b/ros/src/slam/pose_graph_manager.cpp index 483099c..c935532 100644 --- a/ros/src/slam/pose_graph_manager.cpp +++ b/ros/src/slam/pose_graph_manager.cpp @@ -382,9 +382,10 @@ void PoseGraphManager::performRegistration() { loop_idx_pair_queue_.pop(); const RegOutput ®_output = loop_closure_->performLoopClosure(keyframes_, query_idx, match_idx); - need_lc_cloud_vis_update_ = true; + //need_lc_cloud_vis_update_ = true; if (reg_output.is_valid_) { + need_lc_cloud_vis_update_ = true; RCLCPP_INFO(this->get_logger(), "LC accepted. Overlapness: %.3f", reg_output.overlapness_); gtsam::Pose3 pose_from = eigenToGtsam(reg_output.pose_ * keyframes_[query_idx].pose_corrected_); gtsam::Pose3 pose_to = eigenToGtsam(keyframes_[match_idx].pose_corrected_);