maxGapSplit

tonynajjar · tonynajjar · commit c198a95109c9 · 2026-03-16T19:35:54.000Z
Signed-off-by: Tony Najjar &lt;tony.najjar@dexory.com&gt;
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
@@ -108,13 +108,15 @@ class TrajectoryVisualizer
     * @param trajectory_idx Row index into the trajectories arrays
     * @param trajectories Trajectory data
     * @param normalized_cost Cost value in [0, 1] range
+    * @param is_outlier Whether this trajectory is above the percentile threshold
     * @param ns Marker namespace
     * @param stamp Timestamp
     */
   void addCostColoredTrajectory(
     size_t trajectory_idx,
     const models::Trajectories & trajectories,
     float normalized_cost,
+    bool is_outlier,
     const std::string & ns,
     const builtin_interfaces::msg::Time & stamp);
 
@@ -125,6 +127,19 @@ class TrajectoryVisualizer
     */
   static std_msgs::msg::ColorRGBA costToColor(float normalized);
 
+  /**
+    * @brief Maximum gap clustering (k=2). Splits sorted 1D data into two groups
+    *   by finding the largest consecutive gap. Returns the upper bound of the
+    *   lower group and whether a significant split was found.
+    * @param values Input array (will be copied and sorted internally)
+    * @param gap_ratio_threshold Minimum gap-to-range ratio to consider significant
+    * @param[out] upper_bound Upper bound of the lower (non-outlier) group
+    * @return True if a significant gap was found
+    */
+  static bool maxGapSplit(
+    const Eigen::ArrayXf & values, float gap_ratio_threshold,
+    float & upper_bound);
+
   std::string frame_id_;
   nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     trajectories_publisher_;
diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp
@@ -123,20 +123,21 @@ void TrajectoryVisualizer::add(
     return;
   }
 
-  // Normalize total costs using Eigen vectorized ops
+  // Normalize costs for visualization. Outliers (e.g. collision trajectories,
+  // detected via max-gap clustering) render in dark purple instead of the gradient.
   auto addCostLayer = [&](const Eigen::ArrayXf & layer_costs, const std::string & ns) {
       float min_val = layer_costs.minCoeff();
       float max_val = layer_costs.maxCoeff();
-      float range = max_val - min_val;
-      Eigen::ArrayXf normalized;
-      if (range > 0.0f) {
-        normalized = (layer_costs - min_val) / range;
-      } else {
-        normalized = Eigen::ArrayXf::Zero(layer_costs.size());
-      }
+
+      float upper_bound = max_val;
+      bool has_outliers = maxGapSplit(layer_costs, 0.5f, upper_bound);
+      float range = (has_outliers ? upper_bound : max_val) - min_val;
 
       for (size_t i = 0; i < n_rows; i += trajectory_step_) {
-        addCostColoredTrajectory(i, trajectories, normalized(i), ns, stamp);
+        float norm = (range > 0.0f) ?
+          (layer_costs(i) - min_val) / range : 0.0f;
+        bool is_outlier = has_outliers && layer_costs(i) > upper_bound;
+        addCostColoredTrajectory(i, trajectories, norm, is_outlier, ns, stamp);
       }
     };
 
@@ -153,6 +154,7 @@ void TrajectoryVisualizer::addCostColoredTrajectory(
   size_t trajectory_idx,
   const models::Trajectories & trajectories,
   float normalized_cost,
+  bool is_outlier,
   const std::string & ns,
   const builtin_interfaces::msg::Time & stamp)
 {
@@ -168,7 +170,9 @@ void TrajectoryVisualizer::addCostColoredTrajectory(
   marker.action = Marker::ADD;
   marker.pose.orientation.w = 1.0;
   marker.scale.x = 0.01;  // line width
-  marker.color = costToColor(normalized_cost);
+  marker.color = is_outlier ?
+    utils::createColor(0.5f, 0.0f, 0.5f, 0.6f) :  // dark purple for outliers
+    costToColor(normalized_cost);
 
   marker.points.reserve(n_cols / time_step_ + 1);
   for (size_t j = 0; j < n_cols; j += time_step_) {
@@ -197,6 +201,37 @@ std_msgs::msg::ColorRGBA TrajectoryVisualizer::costToColor(float normalized)
   return utils::createColor(r, g, 0.0f, 0.8f);
 }
 
+bool TrajectoryVisualizer::maxGapSplit(
+  const Eigen::ArrayXf & values, float gap_ratio_threshold,
+  float & upper_bound)
+{
+  if (values.size() < 2) {
+    return false;
+  }
+  Eigen::ArrayXf sorted = values;
+  std::sort(sorted.data(), sorted.data() + sorted.size());
+  float total_range = sorted(sorted.size() - 1) - sorted(0);
+  if (total_range <= 0.0f) {
+    return false;
+  }
+
+  float largest_gap = 0.0f;
+  Eigen::Index gap_idx = 0;
+  for (Eigen::Index k = 1; k < sorted.size(); ++k) {
+    float gap = sorted(k) - sorted(k - 1);
+    if (gap > largest_gap) {
+      largest_gap = gap;
+      gap_idx = k;
+    }
+  }
+
+  if (largest_gap > gap_ratio_threshold * total_range) {
+    upper_bound = sorted(gap_idx - 1);
+    return true;
+  }
+  return false;
+}
+
 void TrajectoryVisualizer::reset()
 {
   marker_id_ = 0;
