numpy - 线段插值

import numpy as np
import matplotlib.pyplot as plt

line1 = np.array([[  8.52961447, -78.20367063,   1.45      ],
       [  8.56813782, -75.51640874,   1.45      ],
       [  8.6251203 , -72.7974809 ,   1.45      ],
       [  8.68567765, -70.09587369,   1.45      ],
       [  8.75721399, -67.47386859,   1.45      ],
       [  8.84516013, -64.98925429,   1.45      ],
       [  8.94979453, -62.64604742,   1.45      ],
       [  9.07109536, -60.40626443,   1.45      ],
       [  9.2049534 , -58.26199925,   1.45      ],
       [  9.34897763, -56.20273412,   1.45      ],
       [  9.50353983, -54.22013404,   1.45      ],
       [  9.71003019, -51.86334724,   1.45      ],
       [  9.92782242, -49.64561057,   1.45      ],
       [ 10.15775341, -47.5379025 ,   1.45      ],
       [ 10.40263339, -45.51569429,   1.45      ],
       [ 10.73538666, -43.00083777,   1.45      ],
       [ 11.06971288, -40.64564707,   1.45      ],
       [ 11.39709784, -38.26909549,   1.45      ]])

line2 = np.array([[  8.52961447, -78.20367063,   1.45      ],
       [  9.2049534 , -58.26199925,   1.45      ],
       [  9.34897763, -56.20273412,   1.45      ],
       [ 10.40263339, -45.51569429,   1.45      ],
       [ 10.73538666, -43.00083777,   1.45      ],
       [ 11.06971288, -40.64564707,   1.45      ],
       [ 11.39709784, -38.26909549,   1.45      ]])


# vision-1
def line_interpolate(line, min_interval=0.5):
    """

    Args:
        line (np.ndarray): with shape nx3, each elem represents 3d points(x, y, z).
        min_interval (float): the minimum physical interval(meter) between points.

    Returns:
        interpolated_line (np.ndarray): array, with shape mx3.

    """
    n_points = line.shape[0]
    if n_points == 1:
        return line

    minus = line[:-1] - line[1:]

    line_len = np.sqrt(np.stack([np.square(elem) for elem in minus]).sum(axis=-1)).sum()
    interval =  line_len / (n_points - 1)

    new_n_points = np.int32(np.ceil(n_points * interval / min_interval))

    if new_n_points <= n_points:
        return line

    point_range = np.linspace(0, n_points, n_points)
    new_point_range = np.linspace(0, n_points, new_n_points)

    new_points = []
    for i in range(line.shape[1]):
        new_points.append(np.interp(new_point_range, point_range, line[:, i]))

    return np.stack(new_points).transpose(1, 0)


plt.plot(line1[:,0], line1[:,1], 'o')
target = line_interpolate(line1)
plt.plot(target[:,0], target[:,1], '-x')
plt.show()


plt.plot(line2[:,0], line2[:,1], 'o')
target = line_interpolate(line2)
plt.plot(target[:,0], target[:,1], '-x')
plt.show()

可视化：