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 for p in points]) yPoints = np.array([p 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 for p in pointsYellow] yPointsYellow = [p for p in pointsYellow] xPointsBlue = [p for p in pointsBlue] yPointsBlue = [p 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.