点的坐标系变换原理与代码示例

点的坐标系变换原理与代码示例

引用

CSDN

1.

https://blog.csdn.net/qq_35971623/article/details/138267205

坐标系变换是计算机视觉和机器人领域中的一个基础但重要的概念。本文将详细介绍点的坐标系变换原理，并通过具体的代码示例帮助读者理解这一过程。

变换矩阵

变换矩阵可以理解为从一个坐标系（A系）到另一个坐标系（B系）的转换关系。具体来说：

1. 它表示A系到B系的变换矩阵，用于将点从A系转换到B系。
2. 它反映了A系在B系中的位姿（位置和姿态）。
3. 它描述了B系需要如何通过旋转和平移才能与A系重合。

其中，表示需要旋转的点在A系下的坐标，为车辆（A系）转换到map系（B系）的旋转矩阵，车辆（A系）在map系（B系）的位姿=[cosθ,-sinθ,(sinθ,cosθ)]，其中Theat为A系相对与B系的逆时针旋转角。为a系原点在b系下的位置。

旋转矩阵

旋转矩阵的列向量是旋转后基向量在原坐标系中（X,Y,Z）方向的投影。具体来说：

• 第一列相当于当前坐标轴X基向量（沿当前X轴上长度为1的向量）在原坐标轴（X,Y,Z）的投影坐标。
• 列向量是旋转前后两个轴夹角余弦值，假设旋转前为单位阵I，旋转后为（(X,Y,Z)轴的方向）。
• 其有6个限制条件，分别为3个长度条件（列向量为单位向量），三个正交条件（3个列向量两两垂直）。

例子

变换矩阵：将机械臂2系的坐标点转换到世界系W下，相当于2系在W系下的坐标，相当于W系要进过。该变换矩阵由左上角3*3的旋转矩阵，和右上角的位移矩阵组成。

对于旋转矩阵：相当于2系在W系中的姿态，旋转矩阵的列向量相当于旋转后的坐标系的单位基向量在旋转前坐标系X,Y,Z中的投影。

对于平移矩阵：相当于2系坐标原点在W系中的坐标（X,Y,0）。

从局部坐标系到全局坐标系

公式推导

对应代码

对应示例

示例：将局部坐标系（车辆坐标系）上的点（局部规划路径终点*10）转换到全局坐标系上（蓝色点）。

从全局坐标系到局部坐标系