It is required to have an equidistant curve for a given polynomial for a great variety of applications. For example, an equidistant is needed for a lane detection visual system of a Self-Driving car in case only one line is well determined (the other one is the equidistant because lane lines are parallel). There is an exact solution for the problem and it is known that an equidistant for a polynomial function is a higher order polynomial function.
However, it is usually needed to have an equidistant polynomial of the same order to the given one. For instance, it is essential for the mentioned above application to make the visual system robust and stable. Therefore, an approximate method was developed.
Task: Given the reference polynomial coefficients pol and desired distance d defined perpendicular to the given line, find approximate equidistant line as polynomial of the same degree and return its coefficients pol_eq.
Ideas
Create a set of N points with the same orthogonal distance (d)from a given polynomial.
Fit a new polynomial of desired degree to the point set.
The following simple expressions are useful for the calculations:
Equation of a straight line:
y = k * x + b;
It is needed to flip the reference slope and change its sign to find a perpendicular slope:
k_{m} = -1 / k;
Implementation
The implementation will use Python with Numpy and Matplotlib library for results visualisation.
First of all, points correspond to the given polynomial should be calculated. X coordinates of reference points initialized as equally spaced points between 0 and max_l.
In order to make things simpler with bounds points and better define the slope coefficients we will consider points in between to neighbour pairs of generated ones. They can be obtained by linear interpolation.
Here we calculate coordinates of such points and direction of perpendiculars as slopes of perpendicular lines (k_m calculated as k_{m} = -1 / k)which start from the middle points:
Shifts dx and dy of the equidistant points (x_eq and y_eq) from the middle reference points (x_m and y_m) should be calculated. In fact, dx = x_eq - x_m and dy = y_eq - y_m.
Taking into account the Pythagorean theorem:
dx^{2} + dy^{2} = d^{2};
And k_{m} definition:
k_{m} = dy / dx;
One can found out dx and dy:
dx = d(1/(1+k_{m}^{2}))^{0.5};
dy = k_{m} * dx;
Here the calculations implemented with respect to desired position (sign of d) of the equidistant and the reference points positions.
The final step is to fit a new polynomial of desired degree to the point set
Results and discussion
As a result, the algorithm is able to produce reasonably well defined equidistant in a simple linear case:
As well as in case of higher order polynomial:
However, in case of real application, one should keep in mind two notes:
Equidistant is not always a smooth function because of geometrical restrains:
The algorithm use the same x coordinates of points for reference points set and for building approximation. That is why it is forced to “predict” the reference line behavior in an extended interval. It can lead to unexpected results, especially in case of high order polynomial:
The last issue can be addressed by wise extending x range of the reference points set.
The code
Final code of the project with visualisation.
The code was used in the Lane Lines Detection project.