python实现两函数通过缩放，平移和旋转进行完美拟合

Curve _fitting

前几天在工作的时候接到了一个需求，希望将不同坐标系，不同角度的两条不规则曲线，并且组成该曲线的点集数量不一致，需求是希望那个可以通过算法的平移和旋转搞到一个概念里最贴合，拟合态进行比较。

这是初步将两组数据画到图里的情况，和背景需求是一致的。其实从肉眼看过去左图逆时针旋转120度可以得到一个大致差不多的图。

但这里存在了两个问题：

1. 就算搞到了同一个坐标系，一个基准点选取在哪里，图像绕着这个点旋转才可以得到最拟合点样子
2. 找到基准点，判断最拟合的标准是什么，怎么算距离

首先我们将两图换到一个相同坐标系下

def Convert_to_the_same_scale(xs1, ys1, xs2, ys2):
    xs1 = np.array(xs1)
    ys1 = np.array(ys1)

    xs2 = np.array(xs2)
    ys2 = np.array(ys2)

    # 减去平均值，使得数据中心化
    xs1 -= np.mean(xs1)  # 算一个平均值，最后回到0，0坐标系下
    ys1 -= np.mean(ys1)

    xs2 -= np.mean(xs2)
    ys2 -= np.mean(ys2)

    # 除以标准差，使得数据标准化
    xs1 /= np.std(xs1)
    ys1 /= np.std(ys1)

    xs2 /= np.std(xs2)
    ys2 /= np.std(ys2)


    return xs1.tolist(), ys1.tolist(), xs2.tolist(), ys2.tolist()

我们运用numpy中的函数进行了数据中心化和标准化的处理。np.std()函数是Numpy库中的一个方法，它被用来计算数组中元素的标准差。标准差是一种衡量数据分散程度的指标，值越大表明数据越分散。将数据点除以该数值可以统一到一个离散的程度。经过处理之后得到了以下图样：

接下来就要引入一个比较热的判断距离的算法DTW，DTW是指动态时间扭曲（Dynamic Time Warping）算法，这是一种用于测量两个可能不等长的序列之间相似度的算法。该算法可以找到序列之间的对齐方式，使得对齐后的总体误差最小。

在信号处理、识别和数据挖掘领域，DTW广泛应用于各种任务，如语音识别和手写数字识别。它的主要优点是能够处理时间序列的“弹性”对齐问题，即即使在时间尺度上存在变形，也能够匹配和识别模式。

DTW 算法的基本步骤如下：

1. 初始化：创建一个二维矩阵，其中行数和列数分别等于两个输入序列的长度。将第一个元素设为 0，其余元素设为无穷大。
2. 递归填充：从左上角开始，计算当前位置的距离（通常是欧氏距离），并将其与左方、上方、左上方三个元素的最小值相加，结果存储在当前位置。
3. 寻找路径：从右下角开始，向左上角回溯，寻找最小累计距离的路径。这就是两个序列之间的最佳对齐路径。
4. 输出距离：返回最后一个元素的值，即为两个序列之间的 DTW 距离。

值得注意的是，尽管 DTW 可以很好地处理变化的速度和非线性变形，但是它对输入序列的噪声和异常值敏感，并且计算成本相当高。

我这两个图像都是由几千个点组成的，所以如果让DTW算法去自己360度无死角找best angle和最拟合的点的话，计算量就太大了。

所以经过一些计算，我可以先拿到两组点的端点，将起点对齐之后，再去将一条线以另一条线为准进行旋转，此时得到的角度可以被视作是一个范围角度。

def get_degree(a,b,c,d):
    def get_vector_from_points(p1, p2):
        return np.array([p2[0] - p1[0], p2[1] - p1[1]])

    def dot_product(v1, v2):
        return np.dot(v1, v2)

    def cross_product(v1, v2):
        return v1[0]*v2[1] - v1[1]*v2[0]

    def length_of_vector(v):
        return np.linalg.norm(v)

    def translate_point(p, t):
        return [p[0]+t[0], p[1]+t[1]]

    def rotate_point(p, angle):
        px, py = p
        cos_theta = np.cos(angle)
        sin_theta = np.sin(angle)
        
        qx = cos_theta * px - sin_theta * py
        qy = sin_theta * px + cos_theta * py
        
        return [qx, qy]

    def align_lines(A, B, C, D):
        # Step 1: Translate
        T = get_vector_from_points(C, A)
        C_translated = translate_point(C, T)
        D_translated = translate_point(D, T)

        # Step 2: Rotate
        vAB = get_vector_from_points(A, B)
        vCD = get_vector_from_points(C_translated, D_translated)
        cos_theta = dot_product(vAB, vCD) / (length_of_vector(vAB)*length_of_vector(vCD))
        theta = np.arccos(cos_theta)
        direction = np.sign(cross_product(vAB, vCD)) 

        C_final = rotate_point(C_translated, direction*theta)
        D_final = rotate_point(D_translated, direction*theta)

        return np.degrees(direction*theta)
    degree = align_lines(a,b,c,d)
    return degree

rotate_bosch = [[BOSCH_xs[0],BOSCH_ys[0]],[BOSCH_xs[-1],BOSCH_ys[-1]]]
rotate_ego = [[EGO_xs[0],EGO_ys[0]],[EGO_xs[-1],EGO_ys[-1]]]
theta = (get_degree(rotate_ego[0],rotate_ego[1],rotate_bosch[0],rotate_bosch[1]))

tran_x = EGO_xs[0] - BOSCH_xs[0]
tran_y = EGO_ys[0] - BOSCH_ys[0]

这段代码定义了一个名为 get_degree 的函数，其目的是计算两条线之间的夹角。这里的参数 a,b,c,d 分别代表两条线上的四个点，其中 a 和 b 在一条线上，c 和 d 在另一条线上。

下面是该函数的详细步骤：

1. 定义辅助函数：这些函数用于执行向量运算、平移和旋转点等操作。
2. 定义主要流程：在 align_lines 函数中，首先通过向量差得到平移量 T，将 c 和 d 两点进行平移，使得平移后的 c 点与 a 点重合；接着计算 AB 和 CD 两向量的夹角 theta，并确定旋转方向 direction；最后根据 direction*theta 对平移后的 c 和 d 进行旋转。该函数返回的是 theta 的度数形式。
3. 调用主要流程：在 get_degree 函数中，调用 align_lines 函数并返回得到的角度。

简单来说，这段代码就是把以 c 和 d 为端点的线段通过平移和旋转，让它和以 a 和 b 为端点的线段对齐，然后返回这个旋转的角度。获取的角度就是一个比较贴合的角度，但是不是最精准的。

此时旋转的角度确实如我们所想，度数120度左右，效果大概是这样，看起来还不错。那这样的基础上我们再去编写DTW算法就速度比较快了。Python并没有自带的DTW（Dynamic Time Warping）算法，但是第三方的库，例如fastdtw和dtaidistance提供了这个算法的实现。

使用fastdtw：

from scipy.spatial.distance import euclidean
from fastdtw import fastdtw

x = np.array([1, 2, 3, 4, 5], dtype=float)
y = np.array([2, 3, 4, 5, 6], dtype=float)

distance, path = fastdtw(x, y, dist=euclidean)

print("DTW distance: ", distance)
print("DTW path: ", path)

在上述代码中，fastdtw函数接收两个序列，x和y，以及一个用于计算两点之间距离的函数。这里采用欧氏距离进行计算。

使用dtaidistance：

from dtaidistance import dtw

x = np.array([1, 2, 3, 4, 5], dtype=float)
y = np.array([2, 3, 4, 5, 6], dtype=float)

distance = dtw.distance(x, y)

print("DTW distance: ", distance)

在这段代码中，dtw.distance函数接收两个序列，并返回它们之间的DTW距离。

# 对第二条曲线进行旋转，并计算与第一条曲线的DTW距离
def compute_dtw_distance(points1, points2, angle, center_point):
    rotated_points = rotate_points(points2, angle, center_point)
    distance, _ = fastdtw(points1, rotated_points, dist=euclidean)
    return distance

# 寻找最佳拟合角度
def find_best_fit(points1, points2, center_point=(0, 0), theta = 0):
    """Finds the rotation angle that gives the best fit between two sets of points."""
    angles=np.arange(theta-10, theta+10, 0.3)
    min_distance = float('inf')
    best_angle = None

    for angle in angles:
        distance = compute_dtw_distance(points1, points2, angle, center_point)

        if distance < min_distance:
            min_distance = distance
            best_angle = angle

    return best_angle, min_distance

代码里面我们为了减轻计算量，角度区间为之前得到的theta以及 正负10度，每隔0.3度进行一次计算。然后我们再根据得到的best_angle旋转1次获得最后的结果。

这样一看，两条线就非常的贴合了，角度也调整成了115.5度。但是老板突然跟我说有没有可能这不是最贴合的情况(我#¥#@#@!#@$$#$@),要我把初始点不对齐也算一下，我想了一下，也做了几组测试，我选取了起点周围情况0.4*0.4面积内的格点，一般情况也不会在这个范围之外了。进行一个循环的计算，保留做小的距离和最佳的角度

tran_x = EGO_xs[0] - BOSCH_xs[0]
tran_y = EGO_ys[0] - BOSCH_ys[0]
trans_x = np.linspace(tran_x-0.2, tran_x + 0.2, 5)
trans_y = np.linspace(tran_y-0.2, tran_y + 0.2, 5)

min_distance = float('inf')
best_angle = 0.0
best_bosch_x = []
best_bosch_y = []
start_time = time.time()
for i in trans_x:
    for j in trans_y:
        tmpx,tmpy = copy.deepcopy(BOSCH_xs), copy.deepcopy(BOSCH_ys)
        tmpx,tmpy = translate_points(tmpx,tmpy, i, j)

        ego = np.column_stack((EGO_xs, EGO_ys))
        bosch = np.column_stack((tmpx,tmpy))
        angle, distance = find_best_fit(ego,bosch,center_point=(tmpx[0],tmpy[0]),theta= theta)
        if distance < min_distance:
            min_distance = distance
            best_angle = angle
            best_bosch_x = tmpx
            best_bosch_y = tmpy
        print("此时Bosch的起点在{},{}, 平移的长度为{},{}".format(tmpx[0],tmpy[0],i,j))
        print(BOSCH_xs[0], BOSCH_ys[0])
        print("起点在{},{} 最佳角度为{}  误差距离为{}".format(tmpx[0],tmpy[0],angle,distance))
BOSCH_xs, BOSCH_ys = rotate_points_after_DTW(best_bosch_x,best_bosch_y, best_angle)

别说，你还真别说，上图所示的才是最拟合状态，角度也有微小的变化。这是根据DTW算法得出的结果，但是我个人觉得起点对齐的时候更拟合一点。～～～

全部代码可以联系本菜鸡！纯原创！！！