I have a vehicle with cameras to detect different colored cones. I want to generate a drivable path based on the location of the detected cones.The number of detected cones varies, as it depends on the detection accuracy: for example I might detect 4 blue cones on the left and only 3 yellow cones on the right. I was thinking of using Bezier's curve to perform this task, however it seems like this method will not result in feasible driving paths. Here is what I used to visualise a simple 2D path using the Bezier code that @reptilicus posted in this question:
import numpy as np
from scipy.special import comb
def bernstein_poly(i, n, t):
return comb(n, i) * ( t**(n-i) ) * (1 - t)**i
def bezier_curve(points,nPoints, nTimes=100):
nPoints = len(points)
xPoints = np.array([p[0] for p in points])
yPoints = np.array([p[1] for p in points])
t = np.linspace(0.0, 1.0, nTimes)
polynomial_array = np.array([ bernstein_poly(i, nPoints-1, t) for i in range(0, nPoints) ])
xvals = np.dot(xPoints, polynomial_array)
yvals = np.dot(yPoints, polynomial_array)
return xvals, yvals
if __name__ == "__main__":
from matplotlib import pyplot as plt
nYellow = 3
nBlue = 3
pointsYellow = [[0,0],[10,10], [5,20]]
pointsBlue = [[10,0],[20,10],[15,20]]
xPointsYellow = [p[0] for p in pointsYellow]
yPointsYellow = [p[1] for p in pointsYellow]
xPointsBlue = [p[0] for p in pointsBlue]
yPointsBlue = [p[1] for p in pointsBlue]
xvalsY, yvalsY = bezier_curve(pointsYellow,nYellow, nTimes=100)
xvalsB, yvalsB = bezier_curve(pointsBlue,nBlue, nTimes=100)
xPath = (xvalsY+xvalsB)/2
yPath = (yvalsY+ yvalsB)/2
plt.plot(xvalsY, yvalsY, 'y')
plt.plot(xPointsYellow, yPointsYellow, "yo")
plt.plot(xvalsB, yvalsB, 'b')
plt.plot(xPointsBlue, yPointsBlue, "bo")
plt.plot(xPath, yPath, 'r')
plt.show()
I used the midpoint between the two boundaries as my drivable path (in red). The result is as follows:
It is evident that bezier curve will not be appropriate as the vehicle will knock the cone if it follows this path.
Is there another method I should look into to constrain my path? I haven't come across many examples that use cones as the boundaries.