Add Savitzky–Golay Path Smoother

[SteveMacenski]

I did some quick prototypes and pretty happy with the results of applying SG filters to the problem of path smoothing. Unlike the simple smoother, this won't deviate from the path much, so no need to collision check. Its also analytical so no numerous iterations to converge.

I found the 7-point quadratic formulation to work quite well. 5 point did OK if we did so iteratively, but 7 point was actually good alone.

Task: take R&D code below to clean up and add into a new path smoother algorithm.

Questions / features:

• Include refinement option (e.g. run recursively a few times)
• Add in orientation vectors
• Support for reverse segmentation
• Does this maintain turning boundary conditions on minimum curvature? -- Seems so
• Default include in NavFn to remove odd artifacts?

  // std::array<double, 5> filter{-3.0/35.0, 12.0/35.0, 17.0/35.0, 12.0/35.0, -3.0/35.0};
  std::array<double, 7> filter{-2.0/21.0, 3.0/21.0, 6.0/21.0, 7.0/21.0, 6.0/21.0, 3.0/21.0, -2.0/21.0};
  const unsigned int & num_sequences = plan.poses.size();

  auto applyFilter = [&](const std::vector<double> & data) -> float {
      float val = 0.0;
      for (unsigned int i = 0; i != filter.size(); i++) {
        val += filter[i] * data[i];
      }

      return val;
    };

  auto applyFilterOverX =
    [&](nav_msgs::msg::Path & path) -> void
    {
      unsigned int idx = 0;
      // TODO update for 7 pt
      // path.poses[0].pose.position.x = applyFilter({
      //   path.poses[0].pose.position.x,
      //   path.poses[0].pose.position.x,
      //   path.poses[0].pose.position.x,
      //   path.poses[1].pose.position.x,
      //   path.poses[2].pose.position.x});

      // idx++;
      // path.poses[1].pose.position.x = applyFilter({
      //   path.poses[0].pose.position.x,
      //   path.poses[1].pose.position.x,
      //   path.poses[1].pose.position.x,
      //   path.poses[2].pose.position.x,
      //   path.poses[3].pose.position.x});

      for (unsigned int idx = 3; idx != num_sequences - 3; idx++) {
        path.poses[idx].pose.position.x = applyFilter({
          path.poses[idx - 3].pose.position.x,
          path.poses[idx - 2].pose.position.x,
          path.poses[idx - 1].pose.position.x,
          path.poses[idx].pose.position.x,
          path.poses[idx + 1].pose.position.x,
          path.poses[idx + 2].pose.position.x,
          path.poses[idx + 3].pose.position.x});
      }

      // idx++;
      // path.poses[idx].pose.position.x = applyFilter({
      //     path.poses[idx - 2].pose.position.x,
      //     path.poses[idx - 1].pose.position.x,
      //     path.poses[idx].pose.position.x,
      //     path.poses[idx + 1].pose.position.x,
      //     path.poses[idx + 1].pose.position.x});

      // idx++;
      // path.poses[idx].pose.position.x = applyFilter({
      //    path.poses[idx - 2].pose.position.x,
      //     path.poses[idx - 1].pose.position.x,
      //     path.poses[idx].pose.position.x,
      //     path.poses[idx].pose.position.x,
      //     path.poses[idx].pose.position.x});
    };

  auto applyFilterOverY =
    [&](nav_msgs::msg::Path & path) -> void
    {
      // TODO update for 7 pt
      // unsigned int idx = 0;
      // path.poses[0].pose.position.y = applyFilter({
      //   path.poses[0].pose.position.y,
      //   path.poses[0].pose.position.y,
      //   path.poses[0].pose.position.y,
      //   path.poses[1].pose.position.y,
      //   path.poses[2].pose.position.y});

      // idx++;
      // path.poses[1].pose.position.y = applyFilter({
      //   path.poses[0].pose.position.y,
      //   path.poses[1].pose.position.y,
      //   path.poses[1].pose.position.y,
      //   path.poses[2].pose.position.y,
      //   path.poses[3].pose.position.y});

      for (unsigned int idx = 3; idx != num_sequences - 3; idx++) {
        path.poses[idx].pose.position.y = applyFilter({
          path.poses[idx - 3].pose.position.y,
          path.poses[idx - 2].pose.position.y,
          path.poses[idx - 1].pose.position.y,
          path.poses[idx].pose.position.y,
          path.poses[idx + 1].pose.position.y,
          path.poses[idx + 2].pose.position.y,
          path.poses[idx + 3].pose.position.y});
      }

      // idx++;
      // path.poses[idx].pose.position.y = applyFilter({
      //     path.poses[idx - 2].pose.position.y,
      //     path.poses[idx - 1].pose.position.y,
      //     path.poses[idx].pose.position.y,
      //     path.poses[idx + 1].pose.position.y,
      //     path.poses[idx + 1].pose.position.y});

      // idx++;
      // path.poses[idx].pose.position.y = applyFilter({
      //    path.poses[idx - 2].pose.position.y,
      //     path.poses[idx - 1].pose.position.y,
      //     path.poses[idx].pose.position.y,
      //     path.poses[idx].pose.position.y,
      //     path.poses[idx].pose.position.y});
    };

  applyFilterOverX(plan2);
  applyFilterOverY(plan2);

[SteveMacenski]

This branch as the work: https://github.com/ros-planning/navigation2/tree/sgf_smoother

Still needs unit tests and general testing that it works as expected, but mostly ready to go

Remaining:

• [x] Tests
• [x] Testing with actual planning algorithms (Navfn, Smac's 2D-A*/hybridA*)
• [ ] Configuration Documentation / migration guide / announcement of new algorithm

Unfortunately with ROSCon/IROS travel shortly, I won't be able to complete this in October :cry: So if someone else is motivated and interested, I'd appreciate the help!

I don't expect the smoother to differ greatly from the original path. Its great to smooth out grid-search based jaggedness like from raw Smac 2D-A* as well as NavFn to remove small odd artifacts from grad descent (NavFn) that exist at the end of the path. Since it doesn't impact the path very much, its a nice low-impact smoother that doesn't require collision checking and low parameter (really no tuning required). Unlike the simple smoother which will deviate quite a bit, this stays on the main "path function" and removes "errors" by fitting the polynomial. Fits a nice niche for especially grid techniques or small jagged patterns -- lessor so to remove oscillation -- but then also doesn't meaningfully impact feasibility.

I'm not sure how much utility this algorithm will really have, but it fills a niche that we don't have yet in the smoother algorithms:

• Simple Smoother -- Restricted movement of the path based on keeping near original points but maximizing smoothness as a naive first-order approach that works well and relatively fast. Short cuts corners / turns though.
• Constrained Smoother -- freely move the path based on an optimization based objective function minimization of the key elements of path quality (smoothness, curvature, cost, etc). Very slow.
• Savitsky-Golay Smoother -- Does not move the path at all and tries to model the path "function" to remove jitter / errors, does not deviate from the planners original intention. Very fast and analytical with a long history. Though doesn't remove anything other than path qualities that can be modeled as "error / noise". Good for planners with jitter due to discrete data (NavFn) or jagged search patterns (Moore / Van Nuemann neighborhood search) but doesn't deviate -- only tries to find the "true" function from the data.

[AlexeyMerzlyakov]

Duplicating the Slack, just for the case: The SGF smoother was estimated with SmacHybrid-A* and default NavFn path planner. On both, it produces pretty aligned smoothed path, except of its goal. At the end of path, SGF smoother every time makes incorrect movements, as shown at the attached pictures below (for SmacHybrid-A* planner):

Similar situation for NavFn planner, but unlike SHA* planner, the goal to be reached in this case: From the testing sight, it seems that final direction is being to be correct, while the smoothed poses coordinates near the goal are walking around.

[SteveMacenski]

Pushed branch resolves!

[SteveMacenski]

I hope tomorrow to get on unit tests & doc updates. I'd like this pushed before week's end so I can focus on the EOY tasks (Nav Algo paper + MPPI)

[AlexeyMerzlyakov]

I've verified the SG path smoother with NavFn, ThetaStar and Smac-HybridA* (DUBIN). Now all cases are working pretty well, smoothing the local factured paths produced by path planners. Here are the examples for NavFn (high resolution collage, best to click) (red - raw path, blue - smoothed): ... and for ThetaStar:

For SmacHybridA*, the following metrics (collected by smoother_benchmarking, and compared with SS and CS) were obtained:

--------------------  --------------------  ------------------  ------------------  --------  ----------------------  -----------------------
Planner               Time (s)              Path length (m)     Average cost        Max cost  Path smoothness (x100)  Average turning rad (m)
SmacHybrid            0.11719178941200001   11.504739520948565  12.258683968318367  129.462   1.586654909354627       0.7057524803168388
simple_smoother       0.001119348878        11.002066883616866  18.05353044518565   127.16    0.4624616549599383      3.102313187507408
constrained_smoother  0.022246623292000004  11.493715590115078  6.881435517035139   103.975   1.314291295118698       2.5888961854885775
sg_smoother           5.5814876e-05         11.467456502405593  12.181844171244819  127.762   1.2115300728553242      2.2909156972353713
--------------------  --------------------  ------------------  ------------------  --------  ----------------------  -----------------------

[SteveMacenski]

Sweet - you already knew what I was thinking too. Needed metrics for it to add to the paper :wink:

Thanks for doing the real-world tests. I plan to write the unit tests tomorrow which should button up this topic then beyond documentation!

[SteveMacenski]

Merging imminent