坐标系的概念和坐标系之间的变换

Posted 2023-04-17

参考技术A

坐标系是建立图形与数之间对应联系的参考系。它可以直观方便的描述图形的几何信息、大小、位置。

在计算机图形学中，从对象的建模，到在不同显示设备上显示、处理图形时会使用一系列的坐标系。例如：在屏幕上，会使用像素构成的二维坐标系来表示图形的像素值。

对于一个给定的场景，我们通常并不是按照像素坐标来考虑。我们要考虑屏幕适配、组件复用、软件跨平台等等一系列工程化问题。因此，我们希望将程序中用于描述对象几何信息（形状，拓扑结构）的数值，和那些用于描述对象中大小和位置的数值区分开来。

前者通常被看作一个建模（moduling）的任务，后者是一个观察（viewing）的任务。

例如，教室里一张桌子，我们需要从不同距离和角度去描绘，并显示到屏幕。针对这个问题，我们当然可以把每次“建模-观察”的结果一一绘制到屏幕上。但是，我们需要注意到在这个过程中，桌子的几何信息客观上是固定不变的。因此，我们可以把“建模”的任务和“观察”的任务分离开来，每次绘制只需要改变观察任务的的数值即可。

当我们把“建模”与“观察”分开后，产生一个新的问题，即多坐标系问题。不同的坐标系具有它存在的必要性。

为了方便“建模”，我们给每一个建模任务构建一个自己的坐标系，叫做建模坐标系。同时，为了方便“观察”，我们也给每一个观察任务构建一个观察坐标系。为了完成某一次图形的显示，我们需要建立相应“建模”任务和相应“观察”任务的适当的映射关系，因此产生了世界坐标系的概念。为了使得计算机图形能够在特定的屏幕上正确显示，我们需要知道特定屏幕的屏幕坐标系，即设备坐标系。为了使得计算机图形能够在不同屏幕上正确显示，我们又定义了规范化坐标系。另外，对于一个大场景，我们可能只关注某一局部。因此，产生了裁剪坐标系。

在系统（场景）中用于描述其他坐标系位置和对象模型位置的参考坐标系，被称为 世界坐标系 。

在我的理解中，世界坐标系是一个特定场景中全局的、绝对的公共参照系。

注意这句话，“全局的”、“绝对的”，都是在我们所关心的特定的场景中，这个特定场景很多情况下并非真实世界或者宇宙，它可能是一间房，一个虚拟的游戏世界。

用于描述对象的几何信息的独立于世界坐标系的参照系，叫做 建模坐标系 。

建模坐标系又被称为局部坐标系。一旦定义了“局部”（相对于上面提到的世界坐标系的全局）的对象，就可以很容易地将“局部”对象放入世界坐标系中，使它由局部升为全局。

观察坐标系主要用于从观察者的角度对整个世界坐标系中的对象进行重新定位和描述。

依据观察窗口的方向和形状在世界坐标系中定义的坐标系被称为 观察坐标系 。

观察坐标系用于指定图形的输出范围。

对于世界坐标系来说，观察坐标系其实是一种特殊的建模坐标系。

适合特定输出设备输出对象的坐标系叫做 设备坐标系 。比如屏幕坐标系。

在多数情况下，每一个具体的显示设备，都有一个单独的坐标系统。

规范化坐标系独立于设备，能够很容易地转变为设备坐标系，是一个中间坐标系。
为使图形软件能在不同设备之间移植，采用规范化坐标系，其坐标轴取值范围是[-1, 1]或[0, 1]。

前面提到，对于给定的问题，我们大致分为“建模”和“观察”两个任务。对于一个给定的对象模型，我们已知它在建模坐标系下的坐标，我们如何在观察坐标系下得到该对象的定位和描述。

像这种已知对象在一个坐标系的坐标，要获得该对象在另一坐标系下的坐标，我们把这一过程叫做 坐标转换 。而这个过程离不开世界坐标系，因为世界坐标系决定了建模坐标系与观察坐标系的映射关系。

需要注意，坐标转换过程中，对象的实际位置和状态并未发生改变，我们仅仅是使用了不同的坐标系描述对象的位置。

例下图，在一个世界坐标系 中，一个点 在建模坐标系 中的坐标为 。现在得到观察坐标系 中 点的坐标。

从图中，我们可以很容易的得出，点 在观察坐标系 中的坐标为 。然而，在实际问题中，我们不可能总能通过观察得出新的坐标，这就需要通过变换计算得到。

就这个问题，我们先要得到建模坐标系 到观察坐标系 的变换矩阵R：

可以看到，坐标系A先平移向量 ，再平移向量 ，最后基于Y轴反射 得到坐标系B（实际应用中，连续的相同变换操作可以合并，如这里的连续平移可以合并为 ，这里为了更清晰表示变换过程—— ，将其拆分出来）。代入式（1）得到：

所以，由式(2)可知 基变换矩阵：

由式(3)可得坐标变换矩阵 ：

则，点P在坐标系B中的坐标 为：

同样的，我们可以得出 的基变换矩阵 为：

他恰好就是 的坐标变换矩阵。因此，我们要求 的坐标变换矩阵就可以改为求 的基变换矩阵，可以少一次矩阵的逆运算。

通过这个示例，我们可以得出一个结论， 计算机中，图形的显示过程就是几何（对象）模型在不同坐标系之间的映射变换。

在图中我们可以观察到， 可以先经过两次平移，再经过一次反射得到，记作 ，也可以是先经过一次反射再经过两次平移得到,记作 ，或者更多的变换组合……，但结果肯定是惟一的。

我们尝试将式(2)中的 与 交换顺序以实现变换 ：

得到的结果是：

这与式(3)的结果不一致，因为矩阵点乘一般不满足交换律。可是，这与我们观察到的 和 都可行的结果不一致。

其实 和 两种变换顺序都没有问题，只不过我们观察变换 时采用的的参考系出了问题。

的所有变换操作都隐藏了一个前提：基于目标坐标系（变换操作的参照系）原点的变换。如 实际应该是：

而 中第一步的反射并非基于世界坐标系 轴的变换，而是基于 的反射，所以应该先平移 ,再做反射操作。正确的表达式是：

修正后的变换过程我们记作 。

可以看到，结果与式(3)相同。

虽然 和 都可以得到正确的结果，但是为了防止出现如 一般的错误或为了减少计算量，在 连续不同坐标系变换 过程中，我们应该始终以 本次变换的目标坐标系 作为参考系进行先平移后作线性变换。

[ROS基础] --- TF坐标转换

1 TF坐标转换概念

tf：TransForm Frame,坐标变换
坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
TF左边转换作用：在 ROS 中用于实现不同坐标系之间的点或向量的转换。

说明
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 封装了坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

2 坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped。前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

2.1 1.geometry_msgs/TransformStamped

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

2.2 2.geometry_msgs/PointStamped

std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

3 静态坐标变换

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

需求描述:
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

实现分析:
坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

程序实现：
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方 tf_pub.cpp

/* 
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息 

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建静态坐标转换广播器
        4.创建坐标系信息
        5.广播器发布坐标系信息
        6.spin()
*/


// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])

    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    //----设置子级坐标系
    ts.child_frame_id = "laser";
    //----设置子级相对于父级的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----设置四元数:将 欧拉角数据转换成四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;

3.订阅方tf_sub.cpp

/*  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系)
        6.spin()
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])

    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        
        catch(const std::exception& e)
        
            // std::cerr << e.what() << '\\n';
            ROS_INFO("程序异常.....");
        


        r.sleep();  
        ros::spinOnce();
    


    return 0;

4 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

实现分析:
乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成 坐标系相对信息并发布

程序实现：
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose)
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();


    //  5-3.广播器发布数据
    broadcaster.sendTransform(tfs);


int main(int argc, char *argv[])

    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;

3.订阅方

//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])

    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        
        catch(const std::exception& e)
        
            // std::cerr << e.what() << '\\n';
            ROS_INFO("程序异常:%s",e.what());
        


        r.sleep();  
        ros::spinOnce();
    


    return 0;

5 TF2与TF

5.1 TF2与TF比较_简介

TF2已经替换了TF，TF2是TF的超集，建议学习 TF2 而非 TF

TF2 功能包的增强了内聚性，TF 与 TF2 所依赖的功能包是不同的，TF 对应的是tf包，TF2 对应的是tf2和tf2_ros包，在 TF2 中不同类型的 API 实现做了分包处理。

TF2 实现效率更高，比如在:TF2 的静态坐标实现、TF2 坐标变换监听器中的 Buffer 实现等

5.2 TF2与TF比较_静态坐标变换演示

接下来，我们通过静态坐标变换来演示TF2的实现效率。

5.2.1 启动 TF2 与 TF 两个版本的静态坐标变换

TF2 版静态坐标变换:rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /base_link /laser

TF 版静态坐标变换:rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_link /laser 100

会发现，TF 版本的启动中最后多一个参数，该参数是指定发布频率

5.2.2 运行结果比对

使用rostopic查看话题，包含/tf与/tf_static, 前者是 TF 发布的话题，后者是 TF2 发布的话题，分别调用命令打印二者的话题消息

rostopic echo /tf: 当前会循环输出坐标系信息

rostopic echo /tf_static: 坐标系信息只有一次

5.3 结论

如果是静态坐标转换，那么不同坐标系之间的相对状态是固定的，既然是固定的，那么没有必要重复发布坐标系的转换消息，很显然的，tf2 实现较之于 tf 更为高效