From 7ff3542375e74b73b93d04b2b0490929296c1238 Mon Sep 17 00:00:00 2001 From: itskalvik Date: Sat, 14 Dec 2024 18:19:27 -0600 Subject: [PATCH] Add support for 3d points in tsp path resampler --- sgptools/utils/tsp.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/sgptools/utils/tsp.py b/sgptools/utils/tsp.py index d075f87..3a00f11 100644 --- a/sgptools/utils/tsp.py +++ b/sgptools/utils/tsp.py @@ -171,8 +171,14 @@ def resample_path(waypoints, num_inducing=10): Returns: points (ndarray): (num_inducing, n_dim); Resampled path """ + ndim = np.shape(waypoints)[-1] + if not (ndim==2 or ndim==3): + raise Exception(f"ndim={ndim} is not supported for path resampling!") line = LineString(waypoints) distances = np.linspace(0, line.length, num_inducing) points = [line.interpolate(distance) for distance in distances] - points = np.array([[p.x, p.y] for p in points]) + if ndim==2: + points = np.array([[p.x, p.y] for p in points]) + elif ndim==3: + points = np.array([[p.x, p.y, p.z] for p in points]) return points \ No newline at end of file