Curve _fitting
前几天在工作的时候接到了一个需求，希望将不同坐标系，不同角度的两条不规则曲线，并且组成该曲线的点集数量不一致，需求是希望那个可以通过算法的平移和旋转搞到一个概念里最贴合，拟合态进行比较。
这是初步将两组数据画到图里的情况，和背景需求是一致的。其实从肉眼看过去左图逆时针旋转120度可以得到一个大致差不多的图。
但这里存在了两个问题：
- 就算搞到了同一个坐标系，一个基准点选取在哪里，图像绕着这个点旋转才可以得到最拟合点样子
- 找到基准点，判断最拟合的标准是什么，怎么算距离
首先我们将两图换到一个相同坐标系下
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 算法的基本步骤如下：
- 初始化：创建一个二维矩阵，其中行数和列数分别等于两个输入序列的长度。将第一个元素设为 0，其余元素设为无穷大。
- 递归填充：从左上角开始，计算当前位置的距离（通常是欧氏距离），并将其与左方、上方、左上方三个元素的最小值相加，结果存储在当前位置。
- 寻找路径：从右下角开始，向左上角回溯，寻找最小累计距离的路径。这就是两个序列之间的最佳对齐路径。
- 输出距离：返回最后一个元素的值，即为两个序列之间的 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 degreerotate_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 在另一条线上。
下面是该函数的详细步骤：
- 定义辅助函数：这些函数用于执行向量运算、平移和旋转点等操作。
- 定义主要流程：在
align_lines函数中，首先通过向量差得到平移量T，将c和d两点进行平移，使得平移后的c点与a点重合；接着计算AB和CD两向量的夹角theta，并确定旋转方向direction；最后根据direction*theta对平移后的c和d进行旋转。该函数返回的是theta的度数形式。 - 调用主要流程：在
get_degree函数中，调用align_lines函数并返回得到的角度。
简单来说，这段代码就是把以 c 和 d 为端点的线段通过平移和旋转，让它和以 a 和 b 为端点的线段对齐，然后返回这个旋转的角度。获取的角度就是一个比较贴合的角度，但是不是最精准的。
此时旋转的角度确实如我们所想，度数120度左右，效果大概是这样，看起来还不错。那这样的基础上我们再去编写DTW算法就速度比较快了。Python并没有自带的DTW（Dynamic Time Warping）算法，但是第三方的库，例如fastdtw和dtaidistance提供了这个算法的实现。
使用fastdtw：
from scipy.spatial.distance import euclideanfrom fastdtw import fastdtwx = 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 dtwx = 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.0best_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算法得出的结果，但是我个人觉得起点对齐的时候更拟合一点。～～～
全部代码可以联系本菜鸡！纯原创！！！
本文来自博客园，作者：ivanlee717，转载请注明原文链接：https://www.cnblogs.com/ivanlee717/p/17547754.html