slope&curvature

曲率

import math

from shapely.geometry import LineString
import numpy as np


def get_curvature2(points):
    x1, x2, x3 = points[:, 0]
    y1, y2, y3 = points[:, 1]
    # 计算三个点之间的距离
    AB = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
    BC = math.sqrt((x3 - x2) ** 2 + (y3 - y2) ** 2)
    AC = math.sqrt((x3 - x1) ** 2 + (y3 - y1) ** 2)

    # 计算三角形ABC的面积
    area = 0.5 * abs((x1 - x2) * (y2 - y3) - (x2 - x3) * (y1 - y2))

    # 计算曲率
    kappa = 4 * area / (AB * BC * AC)

    return kappa


def get_curvature1(points):
    """

    # 示例数据：道路上的坐标点
    points = np.array([[0, 0], [1, 1], [2, 2], [3, 0]])

    curvature = get_curvature1(points)

    print("道路曲线的曲率：")
    for i, curv in enumerate(curvature):
        print(f"点{i+1}的曲率为：{curv}")
    :param points:
    :return:
    """
    # 计算曲率
    x = points[:, 0]
    y = points[:, 1]

    dx = np.gradient(x)
    dy = np.gradient(y)
    ddx = np.gradient(dx)
    ddy = np.gradient(dy)

    curvatures = np.abs(dx * ddy - dy * ddx) / np.power(dx ** 2 + dy ** 2, 1.5)

    return curvatures


if __name__ == '__main__':
    points = np.array([[0, 0], [1, 0], [2, 2]])
    print(get_curvature1(points))
    print(get_curvature2(points))

曲率

def calculate_route_slope(point1, point2):
    x1, y1, z1 = point1
    x2, y2, z2 = point2
    elevation_diff = abs(z2 - z1)
    distance_diff = distance.distance((y1, x1), (y2, x2)).m
    if distance_diff > 0:
        return elevation_diff / distance_diff 
    return 0