Skip to content

Commit af456c7

Browse files
Implement Catmull-Rom Spline with test and documentation (#1085)
1 parent 1f42140 commit af456c7

File tree

6 files changed

+239
-0
lines changed

6 files changed

+239
-0
lines changed
Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
import numpy as np
2+
import matplotlib.pyplot as plt
3+
4+
def blending_function_1(t):
5+
return -t + 2*t**2 - t**3
6+
7+
def blending_function_2(t):
8+
return 2 - 5*t**2 + 3*t**3
9+
10+
def blending_function_3(t):
11+
return t + 4*t**2 - 3*t**3
12+
13+
def blending_function_4(t):
14+
return -t**2 + t**3
15+
16+
def plot_blending_functions():
17+
t = np.linspace(0, 1, 100)
18+
19+
plt.plot(t, blending_function_1(t), label='b1')
20+
plt.plot(t, blending_function_2(t), label='b2')
21+
plt.plot(t, blending_function_3(t), label='b3')
22+
plt.plot(t, blending_function_4(t), label='b4')
23+
24+
plt.title("Catmull-Rom Blending Functions")
25+
plt.xlabel("t")
26+
plt.ylabel("Value")
27+
plt.legend()
28+
plt.grid(True)
29+
plt.axhline(y=0, color='k', linestyle='--')
30+
plt.axvline(x=0, color='k', linestyle='--')
31+
plt.show()
32+
33+
if __name__ == "__main__":
34+
plot_blending_functions()
Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
"""
2+
Path Planner with Catmull-Rom Spline
3+
Author: Surabhi Gupta (@this_is_surabhi)
4+
Source: http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf
5+
"""
6+
7+
import sys
8+
import pathlib
9+
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
10+
11+
import numpy as np
12+
import matplotlib.pyplot as plt
13+
14+
def catmull_rom_point(t, p0, p1, p2, p3):
15+
"""
16+
Parameters
17+
----------
18+
t : float
19+
Parameter value (0 <= t <= 1) (0 <= t <= 1)
20+
p0, p1, p2, p3 : np.ndarray
21+
Control points for the spline segment
22+
23+
Returns
24+
-------
25+
np.ndarray
26+
Calculated point on the spline
27+
"""
28+
return 0.5 * ((2 * p1) +
29+
(-p0 + p2) * t +
30+
(2 * p0 - 5 * p1 + 4 * p2 - p3) * t**2 +
31+
(-p0 + 3 * p1 - 3 * p2 + p3) * t**3)
32+
33+
34+
def catmull_rom_spline(control_points, num_points):
35+
"""
36+
Parameters
37+
----------
38+
control_points : list
39+
List of control points
40+
num_points : int
41+
Number of points to generate on the spline
42+
43+
Returns
44+
-------
45+
tuple
46+
x and y coordinates of the spline points
47+
"""
48+
t_vals = np.linspace(0, 1, num_points)
49+
spline_points = []
50+
51+
control_points = np.array(control_points)
52+
53+
for i in range(len(control_points) - 1):
54+
if i == 0:
55+
p1, p2, p3 = control_points[i:i+3]
56+
p0 = p1
57+
elif i == len(control_points) - 2:
58+
p0, p1, p2 = control_points[i-1:i+2]
59+
p3 = p2
60+
else:
61+
p0, p1, p2, p3 = control_points[i-1:i+3]
62+
63+
for t in t_vals:
64+
point = catmull_rom_point(t, p0, p1, p2, p3)
65+
spline_points.append(point)
66+
67+
return np.array(spline_points).T
68+
69+
70+
def main():
71+
print(__file__ + " start!!")
72+
73+
way_points = [[-1.0, -2.0], [1.0, -1.0], [3.0, -2.0], [4.0, -1.0], [3.0, 1.0], [1.0, 2.0], [0.0, 2.0]]
74+
n_course_point = 100
75+
spline_x, spline_y = catmull_rom_spline(way_points, n_course_point)
76+
77+
plt.plot(spline_x,spline_y, '-r', label="Catmull-Rom Spline Path")
78+
plt.plot(np.array(way_points).T[0], np.array(way_points).T[1], '-og', label="Way points")
79+
plt.title("Catmull-Rom Spline Path")
80+
plt.grid(True)
81+
plt.legend()
82+
plt.axis("equal")
83+
plt.show()
84+
85+
if __name__ == '__main__':
86+
main()
Loading
Loading
Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
Catmull-Rom Spline Planning
2+
-----------------
3+
4+
.. image:: catmull_rom_path_planning.png
5+
6+
This is a Catmull-Rom spline path planning routine.
7+
8+
If you provide waypoints, the Catmull-Rom spline generates a smooth path that always passes through the control points,
9+
exhibits local control, and maintains 𝐶1 continuity.
10+
11+
12+
Catmull-Rom Spline Fundamentals
13+
~~~~~~~~~~~~~~
14+
15+
Catmull-Rom splines are a type of cubic spline that passes through a given set of points, known as control points.
16+
17+
They are defined by the following equation for calculating a point on the spline:
18+
19+
:math:`P(t) = 0.5 \times \left( 2P_1 + (-P_0 + P_2)t + (2P_0 - 5P_1 + 4P_2 - P_3)t^2 + (-P_0 + 3P_1 - 3P_2 + P_3)t^3 \right)`
20+
21+
Where:
22+
23+
* :math:`P(t)` is the point on the spline at parameter :math:`t`.
24+
* :math:`P_0, P_1, P_2, P_3` are the control points surrounding the parameter :math:`t`.
25+
26+
Types of Catmull-Rom Splines
27+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~
28+
29+
There are different types of Catmull-Rom splines based on the choice of the **tau** parameter, which influences how the curve
30+
behaves in relation to the control points:
31+
32+
1. **Uniform Catmull-Rom Spline**:
33+
The standard implementation where the parameterization is uniform. Each segment of the spline is treated equally,
34+
regardless of the distances between control points.
35+
36+
37+
2. **Chordal Catmull-Rom Spline**:
38+
This spline type takes into account the distance between control points. The parameterization is based on the actual distance
39+
along the spline, ensuring smoother transitions. The equation can be modified to include the chord length :math:`L_i` between
40+
points :math:`P_i` and :math:`P_{i+1}`:
41+
42+
.. math::
43+
\tau_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
44+
45+
3. **Centripetal Catmull-Rom Spline**:
46+
This variation improves upon the chordal spline by using the square root of the distance to determine the parameterization,
47+
which avoids oscillations and creates a more natural curve. The parameter :math:`t_i` is adjusted using the following relation:
48+
49+
.. math::
50+
t_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
51+
52+
53+
Blending Functions
54+
~~~~~~~~~~~~~~~~~~
55+
56+
In Catmull-Rom spline interpolation, blending functions are used to calculate the influence of each control point on the spline at a
57+
given parameter :math:`t`. The blending functions ensure that the spline is smooth and passes through the control points while
58+
maintaining continuity. The four blending functions used in Catmull-Rom splines are defined as follows:
59+
60+
1. **Blending Function 1**:
61+
62+
.. math::
63+
b_1(t) = -t + 2t^2 - t^3
64+
65+
2. **Blending Function 2**:
66+
67+
.. math::
68+
b_2(t) = 2 - 5t^2 + 3t^3
69+
70+
3. **Blending Function 3**:
71+
72+
.. math::
73+
b_3(t) = t + 4t^2 - 3t^3
74+
75+
4. **Blending Function 4**:
76+
77+
.. math::
78+
b_4(t) = -t^2 + t^3
79+
80+
The blending functions are combined in the spline equation to create a smooth curve that reflects the influence of each control point.
81+
82+
The following figure illustrates the blending functions over the interval :math:`[0, 1]`:
83+
84+
.. image:: blending_functions.png
85+
86+
Catmull-Rom Spline API
87+
~~~~~~~~~~~~~~~~~~~~~~~
88+
89+
This section provides an overview of the functions used for Catmull-Rom spline path planning.
90+
91+
API
92+
++++
93+
94+
.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_point
95+
96+
.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_spline
97+
98+
99+
References
100+
~~~~~~~~~~
101+
102+
- `Catmull-Rom Spline - Wikipedia <https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline>`__
103+
- `Catmull-Rom Splines <http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf>`__

tests/test_catmull_rom_spline.py

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
import conftest
2+
from PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path import catmull_rom_spline
3+
4+
def test_catmull_rom_spline():
5+
way_points = [[0, 0], [1, 2], [2, 0], [3, 3]]
6+
num_points = 100
7+
8+
spline_x, spline_y = catmull_rom_spline(way_points, num_points)
9+
10+
assert spline_x.size > 0, "Spline X coordinates should not be empty"
11+
assert spline_y.size > 0, "Spline Y coordinates should not be empty"
12+
13+
assert spline_x.shape == spline_y.shape, "Spline X and Y coordinates should have the same shape"
14+
15+
if __name__ == '__main__':
16+
conftest.run_this_test(__file__)

0 commit comments

Comments
 (0)