How to select control points for B-spline smoothing from a set of points?

Question

I have an initial path that resulted from RRT-connect algorithm in the form (xi,yi) and i want to smooth it using B-spline algorithm. The question is "Shall i consider all the returned (xi,yi) values as control points or shall i choose some? and if the second what are the criteria to choose some waypoints as control points and not the others?

Answer 1

You are interested in approximating the polygonal line (xi, yi) with a smooth B-spline. Instead of filtering the input points, I suggest you use a least-square B-spline fitting on all the input points. The result is a smooth spline, and you can control the fitting parameters to get a smoother or tighter curve approximation, as I show below.

The following python code produces a noisy polygonal line (xi, yi), which I will use as an example for demonstration.

import numpy as np
import matplotlib.pyplot as plt

num_sample_pts = 20
noise = 0.03
np.random.seed(0)

t = np.linspace(0, np.pi, num_sample_pts)
xi = np.linspace(0, np.pi, num_sample_pts) + noise * np.random.randn(num_sample_pts)
yi = np.sin(t) + noise * np.random.randn(num_sample_pts)

plt.figure()
plt.axis("equal")
plt.plot(xi, yi, 'r-*')

One fitting option is to interpolate your data points. This results in a spline curve that passes through all the input points. This usually results in a more "wavy" spline (depending on your input). Note that for B-spline fitting a parameterization ui is needed for each input point (see also my answers here and here for further explanation). A common parameterization in spline fitting is the chord-length parameterization. This parameterization is defined by the accumulated length of the distances between the ordered points (u0=0, u1=|p1-p0|, u2 = u1+|p2-p1|... etc.).

The code below uses scipy.interpolate.UnivariateSpline to interpolate the input points above and display the results over the above figure. The result is the green spline curve passing through the input points.

xy = np.vstack([xi, yi]).T
ui = np.cumsum(np.r_[[0], np.linalg.norm(np.diff(xy, axis=0), axis=1)])
sx = interpolate.InterpolatedUnivariateSpline(ui, xi)
sy = interpolate.InterpolatedUnivariateSpline(ui, yi)

# sampling the resulting spline and displaying the results
uu = np.linspace(ui[0], ui[-1], 100)
xx = sx(uu)
yy = sy(uu)
plt.plot(xx, yy, "g")

On the data above, this result may not be smooth enough. An alternative to interpolation is least-square approximation. The following code uses scipy.interpolate.LSQUnivariateSpline to fit a smooth curve to the input. Note that the least-square fit needs an additional parameter knots - adjusting this parameter can make a trade-off between smoothness and a tighter fit to the input points.

n = 2
knots = np.linspace(ui[0], ui[-1], n)
sx = interpolate.LSQUnivariateSpline(ui, xi, knots[1:-1])
sy = interpolate.LSQUnivariateSpline(ui, yi, knots[1:-1])

Plotting the result gives the following figure.

As noted above, the knots parameter can be adjusted, the following figure is a result of running the code above with n=10. It can be viewed as somewhere in between the interpolation and the previous least-square fit.

As can be seen from the examples, the smoothing can be played with depending on your data. Another alternative is to use scipy.interpolate.splrep, which has a smoothing parameter as part of its interface. Have a look at the documentation of these functions to find what suits your needs.