路径平滑

from copy import deepcopy
import matplotlib.pyplot as plt
import numpy as np
%matplotlib inline

def draw_path(path, newpath):
    plt.plot(np.array(path)[:,0],np.array(path)[:,1],'g', label='original')
    plt.plot(np.array(newpath)[:,0],np.array(newpath)[:,1],'r', label='newpath')
    plt.legend(loc='best')
    plt.show()

path=[[0, 0], 
      [1, 0],
      [2, 0],
      [3, 0],
      [4, 0],
      [5, 0],
      [6, 0],
      [6, 1],
      [6, 2],
      [6, 3],
      [5, 3],
      [4, 3],
      [3, 3],
      [2, 3],
      [1, 3],
      [0, 3],
      [0, 2],
      [0, 1]]

环形路径

def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001):
    N = len(path)
    newpath = deepcopy(path)
    err = 2*tolerance
    while err > tolerance: 
        err = 0.
        for i in range(N):
            for j in range(2):
                delta = weight_data * (path[i][j] - newpath[i][j]) + \
                         weight_smooth * (newpath[(i-1)%N][j] + newpath[(i+1)%N][j] - 2.0 * newpath[i][j])
                newpath[i][j] += delta
                err += abs(delta)         
    return newpath

[0.000, 0.000] -> [0.471, 0.424]
[1.000, 0.000] -> [1.176, 0.165]
[2.000, 0.000] -> [2.059, 0.071]
[3.000, 0.000] -> [3.000, 0.047]
[4.000, 0.000] -> [3.941, 0.071]
[5.000, 0.000] -> [4.824, 0.165]
[6.000, 0.000] -> [5.529, 0.424]
[6.000, 1.000] -> [5.765, 1.106]
[6.000, 2.000] -> [5.765, 1.894]
[6.000, 3.000] -> [5.529, 2.576]
[5.000, 3.000] -> [4.824, 2.835]
[4.000, 3.000] -> [3.941, 2.929]
[3.000, 3.000] -> [3.000, 2.953]
[2.000, 3.000] -> [2.059, 2.929]
[1.000, 3.000] -> [1.176, 2.835]
[0.000, 3.000] -> [0.471, 2.576]
[0.000, 2.000] -> [0.235, 1.894]
[0.000, 1.000] -> [0.235, 1.106]

两端固定路径

from copy import deepcopy
def smooth(path, weight_data = 0.4, weight_smooth = 0.1, tolerance = 0.00001):
    N = len(path)
    newpath = deepcopy(path)    
    err = 2*tolerance
    while err > tolerance: 
        err = 0.
        for i in range(1,N-1):
            for j in range(2):
                delta = weight_data * (path[i][j] - newpath[i][j]) + \
                         weight_smooth * (newpath[(i-1)%N][j] + newpath[(i+1)%N][j] - 2.0 * newpath[i][j])
                newpath[i][j] += delta
                err += abs(delta)         
    return newpath

[0.000, 0.000] -> [0.000, 0.000]
[1.000, 0.000] -> [1.000, 0.000]
[2.000, 0.000] -> [2.000, 0.000]
[3.000, 0.000] -> [2.999, 0.001]
[4.000, 0.000] -> [3.995, 0.005]
[5.000, 0.000] -> [4.970, 0.030]
[6.000, 0.000] -> [5.822, 0.176]
[6.000, 1.000] -> [5.964, 1.025]
[6.000, 2.000] -> [5.964, 1.975]
[6.000, 3.000] -> [5.822, 2.824]
[5.000, 3.000] -> [4.970, 2.970]
[4.000, 3.000] -> [3.995, 2.995]
[3.000, 3.000] -> [3.000, 2.998]
[2.000, 3.000] -> [2.005, 2.995]
[1.000, 3.000] -> [1.030, 2.970]
[0.000, 3.000] -> [0.177, 2.823]
[0.000, 2.000] -> [0.029, 1.971]
[0.000, 1.000] -> [0.000, 1.000]

带约束的路径平滑

固定四个角点：

path = [[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [5, 0], [6, 0], [6, 1], [6, 2], [6, 3], [5, 3], [4, 3], [3, 3], [2, 3], [1, 3], [0, 3], [0, 2], [0, 1]]
fixpts = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0]

def smooth(path, fix, weight_data=0.0, weight_smooth=0.1, tolerance=0.00001):
    N = len(path)
    newpath = deepcopy(path)    
    err = 2*tolerance
    while err > tolerance: 
        err = 0.
        for i in range(N):
            if not fix[i]:
                for j in range(2):
                    delta = weight_smooth * (newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j] - \
                             2.0 * newpath[i][j]) + \
                             (weight_smooth / 2.0) * (2.0 * newpath[(i-1)%len(path)][j] - \
                             newpath[(i-2)%len(path)][j] - newpath[i][j]) + \
                             (weight_smooth / 2.0) * (2.0 * newpath[(i+1)%len(path)][j] - \
                             newpath[(i+2)%len(path)][j] - newpath[i][j])
                    newpath[i][j] += delta
                    err += abs(delta)         
    return newpath

原理：

上式 是为了保证在固定点处的路径尽量平滑。