Frenet坐标系与Cartesian坐标系互转(二):Python代码函数实现

这篇具有很好参考价值的文章主要介绍了Frenet坐标系与Cartesian坐标系互转(二):Python代码函数实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

1D [ x , y ] → [ s , d ] [x, y]\rightarrow[s, d] [x,y]→[s,d]

import numpy as np
from math import *
def cartesian_to_frenet1D(rs, rx, ry, rtheta, x, y):
    s_condition = np.zeros(1)
    d_condition = np.zeros(1)
    
    dx = x - rx
    dy = y - ry
    
    cos_theta_r = cos(rtheta)
    sin_theta_r = sin(rtheta)
    
    cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx
    d_condition[0] = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd)    
    
    s_condition[0] = rs
    
    return s_condition, d_condition

1D [ s , d ] → [ x , y ] [s, d]\rightarrow[x, y] [s,d]→[x,y]

import numpy as np
from math import *
def frenet_to_cartesian1D(rs, rx, ry, rtheta, s_condition, d_condition):
    if fabs(rs - s_condition[0])>= 1.0e-6:
        print("The reference point s and s_condition[0] don't match")
        
    cos_theta_r = cos(rtheta)
    sin_theta_r = sin(rtheta)
    
    x = rx - sin_theta_r * d_condition[0]
    y = ry + cos_theta_r * d_condition[0]    

    return x, y

2D [ x , y , v , θ ] → [ s , s ˙ , d , d ′ ] [x, y, v, \theta]\rightarrow [s, \dot{s}, d,d'] [x,y,v,θ]→[s,s˙,d,d′]

import numpy as np
from math import *
def cartesian_to_frenet2D(rs, rx, ry, rtheta, rkappa, x, y, v, theta):
    s_condition = np.zeros(2)
    d_condition = np.zeros(2)
    
    dx = x - rx
    dy = y - ry
    
    cos_theta_r = cos(rtheta)
    sin_theta_r = sin(rtheta)
    
    cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx
    d_condition[0] = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd)
    
    delta_theta = theta - rtheta
    tan_delta_theta = tan(delta_theta)
    cos_delta_theta = cos(delta_theta)
    
    one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
    d_condition[1] = one_minus_kappa_r_d * tan_delta_theta
    
    
    s_condition[0] = rs
    s_condition[1] = v * cos_delta_theta / one_minus_kappa_r_d

    return s_condition, d_condition

2D [ s , s ˙ , d , d ′ ] → [ x , y , v , θ ] [s, \dot{s}, d,d']\rightarrow [x, y, v, \theta] [s,s˙,d,d′]→[x,y,v,θ]

import numpy as np
from math import *
def frenet_to_cartesian2D(rs, rx, ry, rtheta, rkappa, s_condition, d_condition):
    if fabs(rs - s_condition[0])>= 1.0e-6:
        print("The reference point s and s_condition[0] don't match")
        
    cos_theta_r = cos(rtheta)
    sin_theta_r = sin(rtheta)
    
    x = rx - sin_theta_r * d_condition[0]
    y = ry + cos_theta_r * d_condition[0]

    one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
    tan_delta_theta = d_condition[1] / one_minus_kappa_r_d
    delta_theta = atan2(d_condition[1], one_minus_kappa_r_d)
    cos_delta_theta = cos(delta_theta)
    
    theta = NormalizeAngle(delta_theta + rtheta)    
    
    d_dot = d_condition[1] * s_condition[1]
    
    v = sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d * s_condition[1] * s_condition[1] + d_dot * d_dot)   

    return x, y, v, theta 

3D [ x , y , v , θ , k , a ] → [ s , s ˙ , s ¨ , d , d ′ , d ′ ′ ] [x, y, v, \theta, k,a]\rightarrow [s, \dot{s},\ddot{s}, d,d',d''] [x,y,v,θ,k,a]→[s,s˙,s¨,d,d′,d′′]

import numpy as np
from math import *
def cartesian_to_frenet3D(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a, theta, kappa):
    s_condition = np.zeros(3)
    d_condition = np.zeros(3)
    
    dx = x - rx
    dy = y - ry
    
    cos_theta_r = cos(rtheta)
    sin_theta_r = sin(rtheta)
    
    cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx
    d_condition[0] = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd)
    
    delta_theta = theta - rtheta
    tan_delta_theta = tan(delta_theta)
    cos_delta_theta = cos(delta_theta)
    
    one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
    d_condition[1] = one_minus_kappa_r_d * tan_delta_theta
    
    kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]
    
    d_condition[2] = (-kappa_r_d_prime * tan_delta_theta + 
      one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
          (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa))
    
    s_condition[0] = rs
    s_condition[1] = v * cos_delta_theta / one_minus_kappa_r_d
    
    delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa
    s_condition[2] = ((a * cos_delta_theta -
                       s_condition[1] * s_condition[1] *
                       (d_condition[1] * delta_theta_prime - kappa_r_d_prime)) /
                          one_minus_kappa_r_d)
    return s_condition, d_condition

3D [ s , s ˙ , s ¨ , d , d ′ , d ′ ′ ] → [ x , y , v , θ , k , a ] [s, \dot{s},\ddot{s}, d,d',d'']\rightarrow [x, y, v, \theta, k,a] [s,s˙,s¨,d,d′,d′′]→[x,y,v,θ,k,a]

def frenet_to_cartesian3D(rs, rx, ry, rtheta, rkappa, rdkappa, s_condition, d_condition):
    if fabs(rs - s_condition[0])>= 1.0e-6:
        print("The reference point s and s_condition[0] don't match")
        
    cos_theta_r = cos(rtheta)
    sin_theta_r = sin(rtheta)
    
    x = rx - sin_theta_r * d_condition[0]
    y = ry + cos_theta_r * d_condition[0]

    one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
    tan_delta_theta = d_condition[1] / one_minus_kappa_r_d
    delta_theta = atan2(d_condition[1], one_minus_kappa_r_d)
    cos_delta_theta = cos(delta_theta)
    
    theta = NormalizeAngle(delta_theta + rtheta)
    kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]
        
    kappa = ((((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *
                 cos_delta_theta * cos_delta_theta) /
                    (one_minus_kappa_r_d) +
                rkappa) *
               cos_delta_theta / (one_minus_kappa_r_d))
    
    
    d_dot = d_condition[1] * s_condition[1]
    
    v = sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d * s_condition[1] * s_condition[1] + d_dot * d_dot)
    
    delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * (kappa) - rkappa     
    a = (s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +
           s_condition[1] * s_condition[1] / cos_delta_theta *
               (d_condition[1] * delta_theta_prime - kappa_r_d_prime))
    return x, y, v, a, theta, kappa 

文章来源地址https://www.toymoban.com/news/detail-603602.html

到了这里,关于Frenet坐标系与Cartesian坐标系互转(二):Python代码函数实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 用ArcGIS模型构建器生成、导出Python转换空间坐标系的代码

      本文介绍在 ArcMap 软件中,通过创建 模型构建器 ( ModelBuilder ),导出 地理坐标系 与 投影坐标系 之间相互 转换 的 Python 代码的方法。   在 GIS 领域中,矢量、栅格图层的投影转换是一个经常遇见的问题;而由于地理坐标系与投影坐标系各自都分别具有很多不同的种

    2024年01月18日
    浏览(44)
  • 标准DH坐标系,改进DH坐标系转换矩阵matlab函数代码2.0

    1.标准DH坐标系(Standard DH) 2.改进DH坐标(modified DH)

    2024年02月13日
    浏览(34)
  • 坐标系变换推导(欧拉角、方向余弦矩阵、四元数)+代码解析

    描述两个坐标系之间的变换关系主要有几个方法 1、欧拉角法(存在奇异性和万向锁而且三个轴旋转的顺序不好定) 2、方向余弦矩阵法(翻译为Directional cosine matrix,简称DCM,也称为旋转矩阵,看了很多博客写的是C11-C33的那个矩阵,没明白为什么也称之为一个方法,有知道的指导

    2024年02月08日
    浏览(89)
  • 怎样通过Python和齐次坐标变换方法实现坐标系之间的转换?

    齐次坐标变换是一种用于实现坐标系之间变换的数学技术。它通常用于计算机图形学、计算机视觉和机器人技术。在齐次坐标系中,3D点/顶点由4D向量(x,y,z,w)表示,其中w是比例因子。齐次表示允许有效的矩阵运算并简化变换过程。坐标系之间的变换可以通过使用齐次变

    2024年02月05日
    浏览(45)
  • 经纬度转换 | 基于Python的经纬度与xy坐标(屏幕坐标)相互转换(可批量),并在平面坐标系上以特定点为坐标原点重新建立坐标系,输出各点新坐标

    用的更多的场景是把 经纬度转化为xy平面坐标 ,因为经纬度是方便我们确定地理位置的,我们可以很容易的从地图数据(可利用高德开放平台)上获取某一个地址它的经纬度,但是我们看到的地图是平面的,所以要利用各种投影把经纬度转换为平面坐标便于我们自己分析~

    2024年02月07日
    浏览(90)
  • python解决坐标系两点之间有多少种走法问题

      问题:在坐标系中第一象限内的点P(x,y)     x6,y6.  到终点(5,5)有多少种走法?并显示出现所经过的点坐标 限制条件:从起点坐标只能向上或者向右走。其中(4,3)是被断开的点。 MAXCOUNT =5 # 设置坐标系中出口的坐标位置M(5,5),计算A(x,y)到M点有多少种走法,要求只能

    2024年02月08日
    浏览(35)
  • 基于Python的经纬度与xy坐标系相互转换

    代码是已经运行过的,可以直接输入参数使用 注意事项: 经纬度参数不可以写颠倒,否则会由于math.log参数不能为负数和0的原因报错 由于地球半径的不确定性,所以(x,y)与(lon, lat)相互转换会有误差 奉上代码及运行结果: 经纬度转换成xy坐标 实例经维度参数 (114.5456282282352

    2024年02月12日
    浏览(56)
  • 世界坐标系、相机坐标系、图像坐标系、像素坐标系

    四个坐标系都是什么? 1.世界坐标系-相机坐标系-图像坐标系-像素坐标系 2.像素坐标系-图像坐标系-相机坐标系-世界坐标系 图像处理、立体视觉等等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系                     构建世界坐标系只是

    2024年01月21日
    浏览(68)
  • 【Python&GIS】面矢量数据投影转换(WGS84转地方坐标系)

            之前分享过点矢量怎么进行投影转换,今天跟大家分享下面矢量如何投影转换。代码与之前的类似,只要注意一下GDAL对矢量文件划分的关系层次即可。         ogr库是一个处理地理空间矢量数据的开源库。它可以读取多种数据格式,进行地理处理、属性表操作、

    2024年02月14日
    浏览(41)
  • 坐标转换(相机坐标系、世界坐标系、图像物理坐标系、图像像素坐标系)

    一般情况下我们所涉及到的坐标包括四个,即相机坐标系、世界坐标系、图像物理坐标系、图像像素坐标系。我们本文的讲解思路是在讲解每个坐标转换之前先讲清楚每个坐标系所表示的含义。本文主要参考由高翔主编的视觉SLAM十四讲第五章相机模型。 相机将三维世界的坐

    2024年02月09日
    浏览(71)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包