python 轨迹识别

判断曲率

https://zhuanlan.zhihu.com/p/72083902

import numpy as np
import numpy.linalg as LA
from matplotlib import pyplot as plt

def PJcurvature(x ,y):
    """
    input  : the coordinate of the three point
    output : the curvature and norm direction
    refer to https://github.com/Pjer-zhang/PJCurvature for detail
    """
    t_a = LA.norm([x[1 ] -x[0] ,y[1 ] -y[0]])
    t_b = LA.norm([x[2 ] -x[1] ,y[2 ] -y[1]])

    M = np.array([
        [1, -t_a, t_a**2],
        [1, 0,    0     ],
        [1,  t_b, t_b**2]
    ])

    a = np.matmul(LA.inv(M) ,x)
    b = np.matmul(LA.inv(M) ,y)

    kappa = 2* (a[2] * b[1] - b[2] * a[1]) / (a[1] ** 2. + b[1] ** 2.) ** (1.5)
    return kappa, [b[1], -a[1]] / np.sqrt(a[1] ** 2. + b[1] ** 2.)

if __name__ == '__main__':
    theta_arr = np.lins