[codex] Add GICP RViz trajectory overlays

gicp_localization/README.md

@@ -279,12 +279,26 @@ correct negative offset.
 |---|---|---|
 | `localized_pose` (`gicp/localization/pose`) | `geometry_msgs/PoseStamped` | Localized pose (scan rate). |
 | `localized_odom` (`gicp/localization/odom`) | `nav_msgs/Odometry` | Localized odom propagated at IMU rate (~100 Hz). |
-| `localized_path` (`gicp/localization/path`) | `nav_msgs/Path` | Trajectory history. |
+| `localized_path` (`gicp/localization/path`) | `nav_msgs/Path` | Final adopted localization trajectory history. |
+| `gt_ins` (`/gt_ins`) | `nav_msgs/Path` | RViz/debug GT/INS reference path, sampled at the same LiDAR scan stamps as `localized_path`. |
+| `gicp/localization/gicp_only_path` | `nav_msgs/Path` | RViz/debug current accepted-GICP-only segment; clears when the node falls back to IMU/GT recovery. |
+| `gicp/localization/gicp_only_segments` | `visualization_msgs/MarkerArray` | RViz/debug accepted-GICP-only history as broken orange line segments, preserving gaps across fallback/recovery. |
 | `gicp/localization/pose_utm` / `odom_utm` / `path_utm` | (same types) | Optional legacy UTM-frame mirrors, only when `utm_transform_path` is set (the primary `map`-frame outputs above are already local ENU with the adapter). |
 | `aligned_cloud` (`gicp/localization/aligned_cloud`) | `sensor_msgs/PointCloud2` | Aligned scan in `map`. |
 | `map` (`gicp/localization/map`) | `sensor_msgs/PointCloud2` | Downsampled visualization map. |
 | TF: `map → base_frame` | | Published when `publish_tf=true`. |
 
+### RViz path colors
+
+The default `launch/localization.rviz` config uses fixed colors for the path
+overlays:
+
+| Color | Display | Topic | Meaning |
+|---|---|---|---|
+| Green | Trajectory Path | `/gicp/localization/path` | Final adopted localization trajectory, including accepted GICP, IMU dead-reckoning fallback, and GT recovery snaps. |
+| Orange | GICP-only Segments | `/gicp/localization/gicp_only_segments` | Accepted-GICP-only trajectory segments. The line breaks while the node is on IMU fallback or GT recovery. |
+| Blue | GT INS Reference | `/gt_ins` | GT/INS reference path sampled at the same LiDAR scan timestamps as the final path. |
+
 ### Debug topics (require `localization/debug/enable_pub: true`)
 
 Per-scan scalar metrics on `gicp/localization/debug/*`:

gicp_localization/include/gicp_localization/localization.h

@@ -246,6 +246,9 @@ class LocalizationNode : public rclcpp::Node {
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr localized_odom_pub;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr gt_ins_path_pub;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr gicp_only_path_pub;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr gicp_only_segments_pub;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr utm_pose_pub;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr utm_odom_pub;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr utm_path_pub;
@@ -310,6 +313,7 @@ class LocalizationNode : public rclcpp::Node {
   Eigen::Matrix4f last_gicp_pose_;
   rclcpp::Time last_gicp_stamp_;
   bool last_gicp_valid_;
+  bool last_scan_gicp_accepted_{false};
   double last_fitness_score_{-1.0};  // -1 = no scan yet
   double last_accepted_scan_stamp_{-1.0};  // s — stamp of last accepted GICP scan (P3 dead-reckon cov)
 
@@ -318,6 +322,13 @@ class LocalizationNode : public rclcpp::Node {
   // subscribers, so we don't pay an O(N) DDS serialize on every scan.
   nav_msgs::msg::Path path_msg;
   std::deque<geometry_msgs::msg::PoseStamped> path_buffer_;
+  nav_msgs::msg::Path gt_ins_path_msg_;
+  std::deque<geometry_msgs::msg::PoseStamped> gt_ins_path_buffer_;
+  nav_msgs::msg::Path gicp_only_path_msg_;
+  std::deque<geometry_msgs::msg::PoseStamped> gicp_only_path_buffer_;
+  visualization_msgs::msg::MarkerArray gicp_only_segments_msg_;
+  bool gicp_only_segment_active_{false};
+  int gicp_only_segment_next_id_{0};
 
   // IMU data structures
   boost::circular_buffer<ImuMeas> imu_buffer;

gicp_localization/launch/localization.rviz

@@ -251,6 +251,47 @@ Visualization Manager:
         Value: /gicp/localization/path
       Value: true
 
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 0; 170; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: GT INS Reference
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 0; 170; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /gt_ins
+      Value: true
+
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: GICP-only Segments
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /gicp/localization/gicp_only_segments
+      Value: true
+
     - Alpha: 1
       Autocompute Value Bounds:
         Max Value: 10

gicp_localization/launch/localization_with_tf.launch.py

@@ -44,6 +44,8 @@ def generate_launch_description():
     odom_topic = LaunchConfiguration('odom_topic', default='/odom')
     gt_odom_topic = LaunchConfiguration('gt_odom_topic', default='/gps_p1/filtered_odom')
     imu_only = LaunchConfiguration('imu_only', default='false')
+    debug_pub = LaunchConfiguration('debug_pub', default='false')
+    verbose_scan_log = LaunchConfiguration('verbose_scan_log', default='false')
     urdf_path = LaunchConfiguration(
         'urdf_path',
         default='')
@@ -75,6 +77,12 @@ def generate_launch_description():
     declare_imu_only_arg = DeclareLaunchArgument(
         'imu_only', default_value=imu_only,
         description='If true, disable GICP and run IMU-only propagation')
+    declare_debug_pub_arg = DeclareLaunchArgument(
+        'debug_pub', default_value=debug_pub,
+        description='If true, publish debug topics including localized_path and RViz comparison paths')
+    declare_verbose_scan_log_arg = DeclareLaunchArgument(
+        'verbose_scan_log', default_value=verbose_scan_log,
+        description='If true, log per-scan GICP accept/reject diagnostics')
     declare_urdf_path_arg = DeclareLaunchArgument(
         'urdf_path', default_value=urdf_path,
         description='Absolute path to the vehicle URDF used by robot_state_publisher '
@@ -140,6 +148,8 @@ def make_localization_node(context):
             localization_yaml_path,
             {'localization/lidar_frame': child_frame_value},
             {'localization/imu_only': LaunchConfiguration('imu_only')},
+            {'localization/debug/enable_pub': LaunchConfiguration('debug_pub')},
+            {'localization/debug/verbose_scan_log': LaunchConfiguration('verbose_scan_log')},
             {'localization/lidar_concat/urdf_path': urdf_file},
         ]
         if map_path_value:
@@ -200,6 +210,8 @@ def make_rviz_node(context):
         declare_odom_topic_arg,
         declare_gt_odom_topic_arg,
         declare_imu_only_arg,
+        declare_debug_pub_arg,
+        declare_verbose_scan_log_arg,
         declare_urdf_path_arg,
         declare_parent_frame_arg,
         declare_child_frame_arg,

gicp_localization/src/localization.cc

@@ -984,6 +984,11 @@ gicp_localization::LocalizationNode::LocalizationNode() : Node("gicp_localizatio
 
   if (this->debug_pub_enabled_) {
     this->path_pub = this->create_publisher<nav_msgs::msg::Path>("localized_path", 10);
+    this->gt_ins_path_pub = this->create_publisher<nav_msgs::msg::Path>("/gt_ins", 10);
+    this->gicp_only_path_pub =
+        this->create_publisher<nav_msgs::msg::Path>("gicp/localization/gicp_only_path", 10);
+    this->gicp_only_segments_pub =
+        this->create_publisher<visualization_msgs::msg::MarkerArray>("gicp/localization/gicp_only_segments", 10);
     this->gt_snap_pub = this->create_publisher<geometry_msgs::msg::PoseStamped>("gicp/localization/gt_snap", 10);
     this->dbg_initial_guess_pose_pub =
         this->create_publisher<geometry_msgs::msg::PoseStamped>("gicp/localization/debug/initial_guess_pose", 10);
@@ -1725,6 +1730,19 @@ void gicp_localization::LocalizationNode::applyInitialPoseFromParams() {
   this->path_msg.poses.clear();
   this->path_msg.header.frame_id = this->map_frame;
   this->path_msg.header.stamp = this->now();
+  this->path_buffer_.clear();
+  this->gt_ins_path_msg_.poses.clear();
+  this->gt_ins_path_msg_.header.frame_id = this->map_frame;
+  this->gt_ins_path_msg_.header.stamp = this->path_msg.header.stamp;
+  this->gt_ins_path_buffer_.clear();
+  this->gicp_only_path_msg_.poses.clear();
+  this->gicp_only_path_msg_.header.frame_id = this->map_frame;
+  this->gicp_only_path_msg_.header.stamp = this->path_msg.header.stamp;
+  this->gicp_only_path_buffer_.clear();
+  this->gicp_only_segments_msg_.markers.clear();
+  this->gicp_only_segment_active_ = false;
+  this->gicp_only_segment_next_id_ = 0;
+  this->last_scan_gicp_accepted_ = false;
 
   RCLCPP_INFO(this->get_logger(),
               "Initial pose loaded from parameters at [%.2f, %.2f, %.2f] m with RPY [%.2f, %.2f, %.2f] rad",
@@ -1780,6 +1798,19 @@ void gicp_localization::LocalizationNode::applyInitialPose(const Eigen::Vector3f
   this->path_msg.poses.clear();
   this->path_msg.header.frame_id = this->map_frame;
   this->path_msg.header.stamp = stamp.nanoseconds() > 0 ? stamp : this->now();
+  this->path_buffer_.clear();
+  this->gt_ins_path_msg_.poses.clear();
+  this->gt_ins_path_msg_.header.frame_id = this->map_frame;
+  this->gt_ins_path_msg_.header.stamp = this->path_msg.header.stamp;
+  this->gt_ins_path_buffer_.clear();
+  this->gicp_only_path_msg_.poses.clear();
+  this->gicp_only_path_msg_.header.frame_id = this->map_frame;
+  this->gicp_only_path_msg_.header.stamp = this->path_msg.header.stamp;
+  this->gicp_only_path_buffer_.clear();
+  this->gicp_only_segments_msg_.markers.clear();
+  this->gicp_only_segment_active_ = false;
+  this->gicp_only_segment_next_id_ = 0;
+  this->last_scan_gicp_accepted_ = false;
 
   RCLCPP_INFO(this->get_logger(), "Received initial pose (%s) at [%.2f, %.2f, %.2f]",
               source.c_str(), p.x(), p.y(), p.z());
@@ -2802,6 +2833,7 @@ void gicp_localization::LocalizationNode::performLocalization() {
   RCLCPP_DEBUG(this->get_logger(), "performLocalization: Acquiring mutex lock...");
   std::lock_guard<std::mutex> lock(this->pose_mutex);
   RCLCPP_DEBUG(this->get_logger(), "performLocalization: Mutex acquired");
+  this->last_scan_gicp_accepted_ = false;
 
   // Set source cloud
   RCLCPP_DEBUG(this->get_logger(), "performLocalization: Setting input source (%lu points)...",
@@ -2820,9 +2852,10 @@ void gicp_localization::LocalizationNode::performLocalization() {
   // in map<-lidar, then convert the optimizer output back to map<-base.
   const Eigen::Matrix4f T_base_lidar = this->extrinsics.baselink2lidar_T;
   const Eigen::Matrix4f T_lidar_base = T_base_lidar.inverse();
-  Eigen::Matrix4f initial_guess = this->deskew_
-      ? Eigen::Matrix4f::Identity()
-      : (this->T_prior * T_base_lidar);
+  Eigen::Matrix4f initial_guess = Eigen::Matrix4f::Identity();
+  if (!this->deskew_) {
+    initial_guess = this->T_prior * T_base_lidar;
+  }
   Eigen::Matrix4f guess_pose_map = this->T_prior;
 
   double guess_from_last_trans = 0.0;
@@ -3152,6 +3185,7 @@ void gicp_localization::LocalizationNode::performLocalization() {
 
   if (gicp_accepted) {
     this->current_pose = candidate_pose;
+    this->last_scan_gicp_accepted_ = true;
 
     // Update lidar pose for next iteration
     Eigen::Vector3f new_p = this->current_pose.block<3, 1>(0, 3);
@@ -3314,6 +3348,100 @@ void gicp_localization::LocalizationNode::publishPose() {
     this->path_pub->publish(this->path_msg);
   }
 
+  // Publish a scan-time-aligned INS/GT reference path. Each point is sampled at
+  // the same scan_stamp used by localized_path, so RViz playback compares both
+  // paths on the same timeline.
+  if (this->gt_ins_path_pub && this->gt_odom_enabled_ && this->gt_odom_received_.load()) {
+    GtSample gt;
+    if (this->getGtPoseAt(this->scan_stamp.seconds(), gt)) {
+      Eigen::Vector3f gt_position;
+      Eigen::Quaternionf gt_orientation;
+      if (!this->composeGtPoseInBase(gt, gt_position, gt_orientation)) {
+        gt_position = gt.p;
+        gt_orientation = gt.q;
+      }
+      gt_orientation.normalize();
+
+      geometry_msgs::msg::PoseStamped gt_pose_msg;
+      gt_pose_msg.header.stamp = this->scan_stamp;
+      gt_pose_msg.header.frame_id = this->map_frame;
+      gt_pose_msg.pose.position.x = gt_position.x();
+      gt_pose_msg.pose.position.y = gt_position.y();
+      gt_pose_msg.pose.position.z = gt_position.z();
+      gt_pose_msg.pose.orientation.w = gt_orientation.w();
+      gt_pose_msg.pose.orientation.x = gt_orientation.x();
+      gt_pose_msg.pose.orientation.y = gt_orientation.y();
+      gt_pose_msg.pose.orientation.z = gt_orientation.z();
+
+      if (this->gt_ins_path_buffer_.size() >= 10000) this->gt_ins_path_buffer_.pop_front();
+      this->gt_ins_path_buffer_.push_back(gt_pose_msg);
+      if (this->gt_ins_path_pub->get_subscription_count() > 0) {
+        this->gt_ins_path_msg_.header.stamp = this->scan_stamp;
+        this->gt_ins_path_msg_.header.frame_id = this->map_frame;
+        this->gt_ins_path_msg_.poses.assign(this->gt_ins_path_buffer_.begin(), this->gt_ins_path_buffer_.end());
+        this->gt_ins_path_pub->publish(this->gt_ins_path_msg_);
+      }
+    }
+  }
+
+  auto publish_gicp_only_path = [&]() {
+    if (this->gicp_only_path_pub && this->gicp_only_path_pub->get_subscription_count() > 0) {
+      this->gicp_only_path_msg_.header.stamp = this->scan_stamp;
+      this->gicp_only_path_msg_.header.frame_id = this->map_frame;
+      this->gicp_only_path_msg_.poses.assign(
+          this->gicp_only_path_buffer_.begin(), this->gicp_only_path_buffer_.end());
+      this->gicp_only_path_pub->publish(this->gicp_only_path_msg_);
+    }
+  };
+
+  auto publish_gicp_only_segments = [&]() {
+    if (this->gicp_only_segments_pub && this->gicp_only_segments_pub->get_subscription_count() > 0) {
+      for (auto& marker : this->gicp_only_segments_msg_.markers) {
+        marker.header.stamp = this->scan_stamp;
+      }
+      this->gicp_only_segments_pub->publish(this->gicp_only_segments_msg_);
+    }
+  };
+
+  if (this->last_scan_gicp_accepted_) {
+    if (!this->gicp_only_segment_active_) {
+      this->gicp_only_path_buffer_.clear();
+
+      visualization_msgs::msg::Marker segment;
+      segment.header.stamp = this->scan_stamp;
+      segment.header.frame_id = this->map_frame;
+      segment.ns = "gicp_only_segments";
+      segment.id = this->gicp_only_segment_next_id_++;
+      segment.type = visualization_msgs::msg::Marker::LINE_STRIP;
+      segment.action = visualization_msgs::msg::Marker::ADD;
+      segment.pose.orientation.w = 1.0;
+      segment.scale.x = 0.18;
+      segment.color.a = 1.0f;
+      segment.color.r = 1.0f;
+      segment.color.g = 0.55f;
+      segment.color.b = 0.0f;
+      this->gicp_only_segments_msg_.markers.push_back(segment);
+      this->gicp_only_segment_active_ = true;
+    }
+
+    if (this->gicp_only_path_buffer_.size() >= 10000) this->gicp_only_path_buffer_.pop_front();
+    this->gicp_only_path_buffer_.push_back(pose_msg);
+    publish_gicp_only_path();
+
+    if (!this->gicp_only_segments_msg_.markers.empty()) {
+      geometry_msgs::msg::Point point;
+      point.x = pose_msg.pose.position.x;
+      point.y = pose_msg.pose.position.y;
+      point.z = pose_msg.pose.position.z;
+      this->gicp_only_segments_msg_.markers.back().points.push_back(point);
+    }
+    publish_gicp_only_segments();
+  } else if (this->gicp_only_segment_active_ || !this->gicp_only_path_buffer_.empty()) {
+    this->gicp_only_segment_active_ = false;
+    this->gicp_only_path_buffer_.clear();
+    publish_gicp_only_path();
+  }
+
   // Publish UTM-frame pose/path
   if (this->utm_enabled_) {
     Eigen::Matrix4f T_utm_base = this->T_utm_map_ * this->current_pose;