Add Savitzky–Golay Path Smoother #3246
Description
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);