# _*_ coding: utf-8 _*_ #
# @Time: 2019/11/28 17:33
# @Author: XiongChao
import math
import numpy as np
from sklearn.neighbors import KDTree
from . import const
DEF_ALPHA = 0.000001
def two_point_distance(point1, point2):
return math.sqrt((point2[0] - point1[0]) ** 2 + (point2[1] - point1[1]) ** 2)
def interpolation_between_two_point(start_point, end_point, ratio):
# 在起点与终点之间进行插值
distance = two_point_distance(start_point, end_point)
n = int(distance * ratio) # 插值的个数
dataset = []
result = np.linspace(0, 1, n + 2) # 保留首尾
for k in result:
x = k * (end_point[0] - start_point[0]) + start_point[0]
y = k * (end_point[1] - start_point[1]) + start_point[1]
if len(start_point) == 3:
z = k * (end_point[2] - start_point[2]) + start_point[2]
point = (x, y, z)
else:
point = (x, y)
dataset.append(point)
return dataset
def cos_(vector1, vector2):
# 计算两个向量的余弦值
cos_theta = (vector1[0] * vector2[0] + vector1[1] * vector2[1]) / \
(math.sqrt(vector1[0] ** 2 + vector1[1] ** 2) * math.sqrt(vector2[0] ** 2 + vector2[1] ** 2))
# 由于浮点运算的截断误差，可能导致求得的值大于1或者小于-1，因此需要进行处理
if cos_theta >= 1:
cos_theta = 1
elif cos_theta <= -1:
cos_theta = -1
return cos_theta
def angle_of_two_vector(vector1, vector2):
# 返回两个向量的角度，返回的值为角度，为浮点数，可以运算
cos_theta = cos_(vector1, vector2)
theta = math.acos(cos_theta) / math.pi * 180
return theta
def angle_of_three_point(point1, point2, point3):
# 123的角度
vector1 = [point1[0] - point2[0], point1[1] - point2[1]]
vector2 = [point3[0] - point2[0], point3[1] - point2[1]]
angle = angle_of_two_vector(vector1, vector2)
return angle
def vector_dot_product(vector1, vector2):
result = vector1[0] * vector2[0] + vector1[1] * vector2[1]
return result
def three_point_dot_product(point1, point2, point3):
# 向量21与23的内积
result = (point1[0] - point2[0]) * (point3[0] - point2[0]) + \
(point1[1] - point2[1]) * (point3[1] - point2[1])
return result
# def direction_of_data(point, dataset):
# # 返回某一段数据的方向向量，在指定的point处
# # 如果dataset近似直线则返回直线的方向，如果为曲线则返回曲线的切线方向
# x_dataset = np.array([data[0] for data in dataset])
# result = x_dataset - x_dataset[0]
# if not result.any():
# # 此时表示所有点的x值都相同，不能进行拟合，会出现矩阵不可逆的情形，当然可以直接得到方向（垂直于x轴）
# return [0, 1]
# derivative = derivative_of_polynomial_function(point, dataset)
# direction = [1, derivative]
# # 非直线的部分点很密集，取合适的点的个数，防止首尾弯曲超过180°
# approximate_direction_of_dataset = [dataset[-1][0] - dataset[0][0], dataset[-1][1] - dataset[0][1]]
# result = vector_dot_product(direction, approximate_direction_of_dataset)
# if result >= 0:
# return direction
# else:
# return [-1, -derivative] # 取反方向
def dis_from_point_to_line(k, b, point):
# k, b代表直线斜率，默认不与X轴垂直
dis = np.abs(k * point[0] + b - point[1]) / np.sqrt(1 + k * k)
return dis
# def dis_from_point_to_dataset(point, dataset):
# # 某点到某个数据集的距离，此数据集为直线数据集
# x_dataset = np.array([data[0] for data in dataset])
# result = x_dataset - x_dataset[0]
# if not result.any():
# # 此时表示所有点的x值都相同，不能进行拟合，会出现矩阵不可逆的情形，当然可以直接得到方向（垂直于x轴）
# return np.abs(point[0] - x_dataset[0])
# new_dataset, x_mean, y_mean = data_normalization(dataset)
# b, k = np.squeeze(get_parameter(new_dataset, 1))
# b = y_mean + b - k * x_mean
# distance = dis_from_point_to_line(k, b, point)
# return distance
# def nearest_point_index_of_dataset(point, dataset):
# # 在dataset中寻找距离point最近的点
# # 返回的是dataset的下标
# distance_list = [two_point_distance(point, data) for data in dataset]
# return distance_list.index(min(distance_list))
def vertical_xaxis(dataset):
# 判断数据集是否垂直于x轴
x_data = np.array([data[0] for data in dataset])
x_data = x_data - x_data[0]
if x_data.any():
return False
return True
def two_point_slope_bias(point1, point2):
# 根据两点确定斜率和截距
k = (point2[1] - point1[1]) / (point2[0] - point1[0])
b = (point1[1] * point2[0] - point2[1] * point1[0]) / (point2[0] - point1[0])
return k, b
def vertical_point(point1, point2, point3):
# point2, point3确定一条直线，point1到此直线的垂点
if point2[0] != point3[0]:
k, b = two_point_slope_bias(point2, point3)
x = (k * point1[1] + point1[0] - k * b) / (1 + k * k)
y = k * x + b
else:
x = point2[0]
y = point1[1]
return x, y
def one_point_to_vertical_point_dis(point1, point2, point3):
# point2, point3确定一条直线，point1到此直线的垂点
ver_point = vertical_point(point1, point2, point3)
distance = two_point_distance(point1, ver_point)
return distance
def dis_from_dataset_to_dataset(dataset1, dataset2):
# 两个数据集之间的距离，采取一个数据集的点到另一个数据集的距离的平均
# 数据集经过插值后，计算点到垂线的距离来度量宽度的误差会很小
dataset1 = [[data[0], data[1]] for data in dataset1] # 取二维
dataset2 = [[data[0], data[1]] for data in dataset2] # 取二维
# 首先保证同向
distance1 = two_point_distance(dataset1[0], dataset2[0])
distance2 = two_point_distance(dataset1[0], dataset2[-1])
if distance1 > distance2:
dataset2 = dataset2[::-1]
# 取对齐的两段
index1 = nearest_point_index_of_dataset(dataset1[0], dataset2)
index2 = nearest_point_index_of_dataset(dataset2[0], dataset1)
distance1 = two_point_distance(dataset1[0], dataset2[index1])
distance2 = two_point_distance(dataset2[0], dataset1[index2])
if distance1 > distance2:
dataset1 = dataset1[index2:]
else:
dataset2 = dataset2[index1:]
index3 = nearest_point_index_of_dataset(dataset1[-1], dataset2)
index4 = nearest_point_index_of_dataset(dataset2[-1], dataset1)
distance3 = two_point_distance(dataset1[-1], dataset2[index3])
distance4 = two_point_distance(dataset2[-1], dataset1[index4])
if distance3 > distance4:
dataset1 = dataset1[:index4 + 1]
else:
dataset2 = dataset2[:index3 + 1]
if len(dataset2) < 2:
# 仅有一个点，在此特殊情形下，仅作简单计算
point = dataset2[0]
index5 = nearest_point_index_of_dataset(point, dataset1)
target_distance = two_point_distance(point, dataset1[index5])
else:
distance_list = []
for i in range(0, len(dataset1), const.STEP_SIZE):
point1 = dataset1[i]
point2, point3 = nearest_two_point_of_dataset(point1, dataset2)
distance = one_point_to_vertical_point_dis(point1, point2, point3)
distance_list.append(distance)
target_distance = sum(distance_list) / len(distance_list)
return round(target_distance * 1000)
def nearest_point_index_of_dataset(point, dataset):
# 在dataset中寻找距离point最近的点
# 仅使用两坐标
tree = KDTree(np.array(dataset), leaf_size=const.LEAF_SIZE)
dist, ind = tree.query([np.array(point[:2])], k=1)
return ind[0][0]
def nearest_two_point_of_dataset(point, dataset):
# 在dataset中寻找距离point最近的点
# 仅使用两坐标
tree = KDTree(np.array(dataset), leaf_size=const.LEAF_SIZE)
dist, ind = tree.query([np.array(point[:2])], k=2)
return [dataset[ind[0][0]], dataset[ind[0][1]]]
def nearest_point_of_dataset(point, dataset):
# 在dataset中寻找距离point最近的点
# 返回的是dataset的下标
# 仅使用两坐标
distance_list = [two_point_distance(point[:2], data[:2]) for data in dataset]
return dataset[distance_list.index(min(distance_list))]
def get_sorted_data(dataset, point):
# 按照离point的距离，对dataset进行排序，从近到远
dict_ = {two_point_distance(point, data): data for data in dataset}
sort_distance = sorted(dict_.keys())
sorted_data = []
for key in sort_distance:
sorted_data.append(dict_[key])
return sorted_data
def clockwise_counterclockwise(point1, point2, point3):
# 判断123是顺时针还是逆时针
# 计算向量point1point2与point2point3的向量积
result = (point2[0] - point1[0]) * (point3[1] - point2[1]) - (point2[1] - point1[1]) * (point3[0] - point2[0])
if result > 0:
# point1point2point3逆时针
return 1
else:
return 0
def length_of_dataset(dataset):
# 一个数据集的长度，以没两个点直接的距离之和计算
dis = 0
for i in range(len(dataset) - 1):
dis += two_point_distance(dataset[i][:2], dataset[i + 1][:2])
return dis
def is_same_point_2d(point1, point2, alpha=DEF_ALPHA):
# 判断两个点是不是一个点
flag = np.array([np.abs(point1[i] - point2[i]) < alpha for i in range(len(point1))])[:2]
if flag.all():
return True
return False