第十篇：TF、eigen、经纬度等坐标变换

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对**TF坐标变换（导航定位必须用到！）**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、ROS_TF功能包作用

1.TF介绍

坐标转换和坐标订阅发布

TF库的目的是实现系统任一个点在所有坐标系之间的坐标变换，也就是说，只要给定一个坐标系下的一个点的坐标，就可以获得这个点在其他坐标系下的坐标，而且这些坐标变换是带有时间戳stamp的

tf功能包就是给定起始位姿坐标，终止位姿坐标，TF功能包把每个位姿坐标通过tf_tree动态/静态链接依赖，并实时找到其变换矩阵【防盗标记–盒子君hzj】

使用的是广播订阅的通讯，也使用了launch文件管理启动，tf是ROS中的坐标变换系统，在机器人的建模仿真中经常用到
.

2.ROS-TF坐标转换树作用

（1）位置标定

实现各个坐标系的坐标变换，提供了ROS中各个组件（如：底盘、激光雷达，相机，IMU等），位置标定是机器人定位的基础

（2）访问关于机器人及其组件的时间和坐标的信息

我们可以从ROS-TF坐标转换树话题中访问关于机器人及其组件的时间和坐标的信息【防盗标记–盒子君hzj】

（3）记录和推算坐标系之间的关系

（1）五秒之前，机器人的参考坐标系相对于全局参考坐标系的关系是怎样的
（2）机器人上的激光雷达相对于机器人参考坐标系是怎样的
（3）机器人上的相机相对于机器人参考坐标系是怎样的

3.常用参考系类型

（1）全局坐标系（map）
（2）机器人身体坐标系（base_link）
（3）激光雷达参考坐标系
（4）摄像头参考坐标系【防盗标记–盒子君hzj】
（5）IMU参考坐标系
.
.

二、TF的常用功能

1.【transform】坐标系的数据格式、转换及运算

【位姿：欧拉角(rpy)、四元数（quternion）、旋转向量（vector）】

头文件#include<tf/transform_datatypes.h>

（1）tf::Quaternion类【四元素相关–直观】

（0）将一个坐标系下的点转化到另一个坐标系下【transformPoint()】

geometry_msgs::PointStamped point1;
point1.header.stamp=ros::Time();
point1.header.frame_id="child_frame";【防盗标记–盒子君hzj】
point1.point.x=1;
point1.point.y=2;
point1.point.z=3;
geometry_msgs::PointStamped point2;
try
{
     listener.transformPoint("frame",point1,point2);
}
catch (tf::TransformException &ex) 
{
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
 }

（1）用TF初始化创建四元数类型数据

（2）两种四元数数据类型转换【tf::Quaternion&&geometry_msgs::Quaternion】

（3）三种姿态形式的转换【四元数（quternion）、欧拉角(rpy)、旋转向量（vector）】

源码实现

tfScalar getAngle() const 【防盗标记–盒子君hzj】
{
	tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]);
}
Vector3 getAxis() const
{
	tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
	if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero
	        return Vector3(1.0, 0.0, 0.0);  // Arbitrary
	tfScalar s = tfSqrt(s_squared);
	return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
}

（4）四元数运算【相乘，点乘，求逆，加减乘除】

（1）四元数求模：sqrt(q.xq.x + q.yq.y + q.zq.z + q.wq.w)

tfScalar length() const
{
	return tfSqrt(length2());
}

（2）四元数模的平方：q.xq.x + q.yq.y + q.zq.z + q.wq.w

tfScalar length2() const
{
	return dot(*this);【防盗标记–盒子君hzj】
}

（3）四元数归一化

Quaternion& normalize() 
{
	return *this /= length();
}

Quaternion normalized() const 
{
	return *this / length();
}

（4）四元数点乘

tfScalar dot(const Quaternion& q) const
{
	return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
}

（5）四元数相乘

TFSIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
return Quaternion(
	q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
	q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
	q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),【防盗标记–盒子君hzj】
	q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
}

Quaternion& operator*=(const Quaternion& q)
{
	setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
	        m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
	        m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
	        m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
	return *this;
}

（5）四元数插值

Quaternion slerp(const Quaternion& q, const tfScalar& t) const

注意

移植TF库的时候，这些实现原理都要懂

.
.
.

（2）tf::Matrix3x3类【变换矩阵相关–不直观】

（简单认为就是tf::Quaternion类是一维的，tf::Matrix3x3类是三维的）

在Matrix3x3类中，主要涉及到有各种不同方式的旋转矩阵初始化，欧拉角转旋转矩阵，旋转矩阵转欧拉角，四元数转旋转矩阵，旋转矩阵转四元数，以及一些常用的矩阵操作（矩阵相乘，求逆，转置，求行列式，…）等操作 ：【防盗标记–盒子君hzj】

.
.
.

2.【Broadcaster/Listenter】订阅/发布每个关节的坐标（实现步骤）

（1）创建一个TF坐标变换的功能包

【当然这个功能可以在其他功能包内嵌套，但是最好还是单独成立一个tf坐标变换的功能包，好排除bug】

在功能包下的src文件夹新建一个文件夹tf，在tf文件夹里新建tf_boardcaster.cpp\tf_listener.cpp
.
.

（2）（发布者）广播TF变换

【Broadcaster发布坐标】
向系统中广播坐标系与坐标系变换的关系，系统中存在多个不同TF变换广播，每个广播都会直接在坐标系变换树中插入，有助于更快的查询、增加和删除TF节点

实现TF广播器程序步骤

（1）头文件
#include<tf/transform_broadcaster.h>

（2）定义发布器
tf::TransformBroadcaster broadcaster;【防盗标记–盒子君hzj】

（3）发布一个tf（源文件随时发布）
broadcaster.sendTransform( stamped_transform ) ;
【stamped_transform是一个tf::StampedTransform类型的数据】

.
.

（3）（订阅者）监听TF变换

【Listenter订阅坐标】
接收并缓存系统中发布的所有坐标系变换、并从中查询所需要的参考系变换

实现TF监听器程序步骤

（1）头文件
#include<tf/transform_listenter.h>

（2）定义监听器
tf::TransformListener  listener

（3）订阅TF的坐标信息【lookuoTransform()】

使用方法
	tf::StampedTransform stamped_transform;   //定义存放变换关系的变量
	try
	{
	//得到child_frame坐标系原点，在frame坐标系下的坐标与旋转角度
	      listener.lookupTransform("frame", "child_frame",ros::Time(0), stamped_transform);                   
	}
	    catch (tf::TransformException &ex)
	{
	      ROS_ERROR("%s",ex.what());
	      ros::Duration(1.0).sleep();
	      continue;
	}

	listener.lookupTransform("frame", "child_frame",ros::Time(0), stamped_transform);
	//获取到stamped_transform后可以使用这些信息
	transform.getOrigin().x()
	transform.getOrigin().y()
	 
	transform.getRotation().getW();
	transform.getRotation().getX();【防盗标记–盒子君hzj】
	transform.getRotation().getY();
	transform.getRotation().getZ();

（4）编译代码

1、在cmakelist.txt中设置编译选项，设置链接库
2、在元功能包下编译

.
.

3.TF变换的运算本质

TF变换就是两个刚性坐标系的平移和旋转的矩阵运算，不过它是实时运行的，包括位置和姿态

TF变换是基于右手坐标系的
拿起你的右手，先给自己竖个大拇指，然后打开手掌，将大拇指的方向朝向是z轴，让剩下的四根手指朝向的x轴，此时朝向手心外的是y轴的方向

三、使用eigen实现tf功能包的坐标转换功能替换（公司的导航系统没有用ROS_TF，eigen是矩阵运算的首选）

eigen的坐标转换功能有很多，要用什么才移植什么，太多了~

1、eigen的原理学习

https://blog.csdn.net/sunqin_csdn/article/details/108134138
.
.

2、位姿pose转矩阵

Eigen::Matrix4d Converter::poseToTransformMatrix( const Eigen::Vector3d& _ned, Eigen::Vector3d& _euler_angle, const Eigen::Vector3d& _offset_angle, const Eigen::Vector3d& _offset_position) {

  Eigen::Quaterniond _offset_qua;
  eulerToQuaternion(_offset_angle, _offset_qua);
  Eigen::Quaterniond _qua;
  eulerToQuaternion(_euler_angle, _qua);
  Eigen::Translation3d _offset_translation(_offset_position(0), _offset_position(1), _offset_position(2));

  Eigen::Translation3d translation3(_ned[0], _ned[1], _ned[2]);
  Eigen::Matrix4d transform = Eigen::Isometry3d(translation3 * _qua * _offset_translation * _offset_qua) .matrix();  // angle_yaw*angle_pitch*angle_roll
  return transform;
}

3、欧拉角euler转四元数Quaternion

void Converter::eulerToQuaternion(const Eigen::Vector3d& _euler, Eigen::Quaterniond& _qua) {
  Eigen::AngleAxis<double> angle_roll(_euler[0] * 0.0174532922f,  Eigen::Vector3d(1, 0, 0));
  Eigen::AngleAxis<double> angle_pitch(_euler[1] * 0.0174532922f, Eigen::Vector3d(0, 1, 0));
  Eigen::AngleAxis<double> angle_yaw(_euler[2] * 0.0174532922f, Eigen::Vector3d(0, 0, 1));
  _qua = Eigen::Quaterniond(angle_yaw * angle_pitch * angle_roll);
}

四、注意

（1）四元数（0，0，0，1）代表方向都为0的全能角【防盗标记–盒子君hzj】

（2）四元数是用来表示姿态pose的，而不是表示位置point的。能表示姿态还可以是旋转向量vector、欧拉角rpy、四元数（x,y,z,w）

（3）tf坐标连接关系可以通过rosrun rqt_tf_tree rqt_tf_tree查看，若是节点断开了就代表tf连接有问题，去源码看看有没有问题，实在不行就在launch文件中用静态链接指令手动连接在一起（只是一个隐藏的bug）.静态链接指令[x,y,z,r,p,y/x,y,z,四元数 ： 父节点坐标、子节点坐标]【防盗标记–盒子君hzj】

五、经纬度转换

1、北东地坐标表转经纬度坐标

void Converter::nedToLls(const Eigen::Vector3i& _lls_takeoff, const Eigen::Vector3d& _ned, Eigen::Vector3i& _lls) {
  _lls[0] = static_cast<int32_t>(static_cast<double>(_ned[0]) / LANLON_COEFFICIENT) + _lls_takeoff[0];
  _lls[1] = static_cast<int32_t>((static_cast<double>(_ned[1]) / cos(static_cast<double>(_lls[0]) / pow(10, 7) * 0.0174532925f)) / LANLON_COEFFICIENT) + _lls_takeoff[1];
  _lls[2] = _lls_takeoff[2] - _ned[2] * 100.0f;
}

2、经纬度坐标转北东地坐标表

void Converter::llsToNed(const Eigen::Vector3i& _lls_takeoff, const Eigen::Vector3i& _lls, Eigen::Vector3d& _ned) {
  double temp = static_cast<double>(_lls[1] - _lls_takeoff[1]) * LANLON_COEFFICIENT;
  temp *= cos(static_cast<double>(_lls[0]) / pow(10, 7) * 0.0174532925f);
  _ned[1] = temp;
  temp = static_cast<double>(_lls[0] - _lls_takeoff[0]) * LANLON_COEFFICIENT;
  _ned[0] = temp;
  _ned[2] = -static_cast<double>(_lls[2] - _lls_takeoff[2]) / 100.0f;
}

---