SLAM从入门到精通（tf的使用）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        在ros的机器人学习过程中，有一件事情是肯定少不了的。那就是坐标系的转换。其实这也很容易理解。假设有一个机器人，它有一个3d camera、有一个机械手臂。这个时候有一个需求，需要通过3d camera告知物体的位置，然后通知另外一个机械手臂取走。

        看上去这个任务很简单，但是这中间就涉及到了坐标系的转换。比如说，摄像头识别到物体，这个时候物体是摄像头坐标系下的一个pose，需要通过robot1坐标系、全局坐标系的转换关系，进一步变成世界坐标系下的位置。这样，camera看到的物体pose，就有一个全局pose信息，虽然还是同一个物品。

        那么这个物体的位姿信息怎么通知到机械手臂呢？那么又需要进行反向的坐标系转换。即将物体的全局pose，转成robot2坐标系下的pose，然后进一步转换成机械手臂下的局部pose。这样机械手臂，就可以根据这个局部的pose信息，实现物体的抓取了。

1、tf的作用

        tf的主要作用，其实就是实现坐标系的转换。它的作用，有可能是局部坐标系到世界坐标系，也有可能是世界坐标系到局部坐标系。

2、tf的主要构成

        坐标系的主要沟通有两部分，一部分是xyz，一部分是围绕xyz坐标轴的旋转。其中xyz就是直接用浮点数表示，旋转一般用xyzw的四元数来表示。如果要转成rpy的角度转换，需要用tf提供的公式转换一下。

3、tf的计算方法

        目前计算的方法很多，不过主要还是利用矩阵计算，如果是计算一个点在另外一个坐标系下的坐标，过程中仅仅涉及到旋转的话，一般是

        中间如果涉及到位移的话，一般还会添加一个offset，

        这样的公式虽然比较简单，但是转成矩阵不太方便，我们可以通过补齐1来处理，

        这样看上去公式完美一些了，可以进一步简化一下，

        公式上面似乎回到了原点，但是每一个变量的含义其实都发生了改变。当然，这里的R仅仅表示P0到P1的转变。很多人也许会问了，如果是P1到P0的转变，这个时候应该怎么处理呢。这个时候矩阵的优势就发挥出来了，

        所以，如果是需要P1坐标系下面的一个点，此时需要秀姐P0坐标系下的坐标，它所需要的就是求解旋转矩阵R的逆即可。有了单一的坐标转换，那么连续的坐标转换就变得容易了，

        反之也是一样，

4、tf中的静态消息和动态消息

        坐标系转换中，有的是静态转换，有的是动态转换。所谓的静态转换，就是那种确定了之后，就一直不变化的。比如传感器和robot之间的固定位置。还有一种就是动态变换，它所指向的就是那种一直在变化的坐标映射关系，比如robot和map之间的位置转换关系。

5、amcl举例说明

        关于amcl算法，大家可以参考这个链接http://wiki.ros.org/amcl。在这中间就包含了大家想学习的tf信息。

        根据输入，它所以依赖的消息有scan雷达、tf坐标系转换、初始位置、map地图四个数据。第1、3、4都比较好理解。而第2个数据就和今天学习的知识相关，包含了lidar到robot、robot到odom的转换等。一开始的时候，我们还在寻找算法里面怎么没有里程计odom的数据，其实答案就在tf里面。

        经过算法求解，这个时候会输出三种数据，分别是amcl位姿、粒子云和tf。第1、2都比较好理解，还有一个tf数据。根据英文解释，它发布的就是map坐标系下odom的里程数据。

6、tf的编程范例

        了解一些tf的编程接口，也对我们理解和认识tf很有帮助。如果是tf的发布，一般会涉及到这样的接口，

tf::TransformBroadcaster
tf::Transform
tf::Quaternion

        相反，如果是一些接收的接口，也会有一些tf的数据结构，

tf::TransformListener
geometry_msgs::PointStamped

        利用TransformBroadcaster和TransformListener可以实现tf内部的消息传递。当然也可以使用subscribe和publish的机制。现在为了说明如何使用这些代码，我们可以通过编写两个程序来说明一下。一个是book_tfpub.cpp，一个是book_tflis.cpp，前者的代码如下所示，

#include 
#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "geometry_msgs/Point.h"
#include "tf/tf.h"
 
int main(int argc, char* argv[])
{
	ros::init(argc, argv, "tf_transformpub");
	ros::NodeHandle nh;
 
	static tf::TransformBroadcaster transfpub;
	tf::Transform base2laser;
	base2laser.setOrigin(tf::Vector3(1,0,0));
 
	tf::Quaternion q;
	q.setRPY(0,0,0);
	base2laser.setRotation(q);
	ros::Rate rate(10);
 
	while(nh.ok())
	{
		transfpub.sendTransform(tf::StampedTransform(base2laser, ros::Time::now(), "base_link", "laser_link"));
		rate.sleep();
	}
	return 0;
}

        后者的代码如下所示，

 
#include <iostream>
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
 
using namespace  std;
 
int main(int argc, char* argv[])
{
	ros::init(argc, argv, "tf_transformlis");
	ros::NodeHandle nh;
 
	tf::TransformListener tflis;
	geometry_msgs::PointStamped plaser;
	plaser.header.frame_id = "laser_link";
	plaser.point.x = 1;
	plaser.point.y = 0;
	plaser.point.z = 0;
 
	geometry_msgs::PointStamped pbase;
	ros::Rate rate(10);
 
	while(nh.ok())
	{
		cout << "start listening" << endl;
		tflis.waitForTransform("base_link", "laser_link", ros::Time(0), ros::Duration(3));
		tflis.transformPoint("base_link", plaser, pbase);
		cout << "pbase is: (" << pbase.point.x << "," << pbase.point.y << "," << pbase.point.z << ")" << endl;
		rate.sleep();
	}
 
	return 0;
}

        为了让两个代码都能顺利编译通过。有两个地方需要修改下。一个是package.xml，

  <build_depend>tf</build_depend>

        另外一个就是CMakeLists.txt，

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS message_generation roscpp rospy std_msgs genmsg tf)
 
add_executable(book_tfpub src/book_tfpub.cpp)
target_link_libraries(book_tfpub ${catkin_LIBRARIES})
add_dependencies(book_tfpub beginner_tutorials_generate_messages_cpp)
 
add_executable(book_tflis src/book_tflis.cpp)
target_link_libraries(book_tflis ${catkin_LIBRARIES})
add_dependencies(book_tflis beginner_tutorials_generate_messages_cpp)

        经过catkin_make编译和rosrun启动，就可以看到这样的打印信息了，

start listening
pbase is: (2,0,0)
start listening
pbase is: (2,0,0)
start listening
pbase is: (2,0,0)
start listening
pbase is: (2,0,0)
start listening
pbase is: (2,0,0)
start listening
pbase is: (2,0,0)

注：

        ros坐标系如下所示，