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.linspace(0, np.pi *
     
    
  
 

文章来源: blog.csdn.net，作者：AI视觉网奇，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/jacke121/article/details/120169879