1、空间描述与变换
有两个坐标系A和B，B坐标系中有一个点P，如何把B坐标系中的P映射到A坐标系呢，这就涉及到空间描述与变换，
先看一下旋转矩阵：
上面中间的行向量
中的元素表示在B坐标系当中的元素用A坐标系来表达。
是B坐标原点在A坐标系中的表达，它也是平移量；AP表示P在A坐标系中的表达，BP表示P在坐标系B中的表达。
要想将BP转换为A坐标系中的表达，就需要乘以旋转矩阵
，它是旋转量。
下面目的是将B坐标系中的P点在A坐标系中进行表达
将它可以写成齐次坐标的形式如下，
中间的表达式可以成为旋转算子，
首先，看一下xyz固定角坐标系：
首先将一个坐标系B与一个已知坐标系A重合，先将B坐标系绕XA坐标系逆时针旋转γ角度，这里X轴坐标不变；然后再绕YA坐标轴逆时针旋转β角度；最后绕ZA轴逆时针旋转α角度。每一次旋转都是按照对应的固定参考系。我们将这种姿态的表示方法叫做X-Y-Z固定角坐标系。有时也叫偏航角，横滚角，俯仰角。
再来看一下欧拉表示法：
首先将一个坐标系B与一个已知坐标系A重合，首先将B坐标系绕ZA轴逆时针旋转α角度；再绕YA坐标轴逆时针旋转β角度；最后绕XA坐标系逆时针旋转γ角度。这个与上面的区别在于，每次旋转都是绕着坐标系B中的坐标轴旋转而不是绕着坐标系A中的坐标旋转。
最后来看一下四元素表示法：
与矩阵相比四元组更加高效，占用的空间更小，且便于插值。四元组表示复数，w+xi+yj+zk,其中i,j,k都是虚数单位。可以把四元组看做是一个标量和一个3维向量的组合；实部w表示标量，虚部(x,y,z)用v表示。其中w与旋转角度相关，v与旋转轴相关。
操作四元组，欧拉角或者X-Y-Z固定角可以用Bullet方法，KDL::Rotation方法等
tf
tf的原理
有三个坐标系，一个未知坐标系query，已知坐标系known，一个世界坐标系world
要求得未知坐标系在世界坐标系当中的表达，可以用旋转算子来表示：
未知坐标系在世界坐标系中的旋转算子=未知坐标系在已知坐标系中的旋转算子*已知坐标系在世界坐标系中的旋转算子，通过例子来看一下，
1、roscore
2、roslaunch turtle_tf turtle_tf_demo.launch
出现两只乌龟一只乌龟随着另外一只乌龟跑，
一只乌龟跟随另一只乌龟的运动而运动，这里面就包含三个坐标系，一个是乌龟1坐标系，一个是乌龟2坐标系，还有一个世界坐标系，
使用命令行工具，
view frames是以pdf的形式来展示运行框架的，
rosrun tf view_frames
打开该pdf，如下
turtle1和turtle2的父坐标系均为world坐标系。
然后再看，
下面看一下turtle1与turtle2作为之间的关系，前者作为父坐标系，后者作为子坐标系
再看一下tf_monitor工具
仍然以turtle1作为父框架，turtle2作为子框架
可以看到上面从turtle1到turtle2的变换的相关信息
TransformBroadcaster有一个方法sendTransform它可以提供的信息是已知坐标系到世界坐标系的旋转算子。
TransformListener提供了一个方法lookupTransform提供了一个未知坐标系到一个已知坐标系的旋转算子。
ExtrapolationException异常是两个frame之间存在请求，但是其中有一个frame是过时的，例如turtle1在listen时请求的是turtle2在50秒之前的请求，则会无效。
编写程序实现turtle tf_demo那个例子
打开eclipse，导入工程，然后新建一个源文件turtle_tf_broadcaster.cpp
#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>//turtlesim广播的是一个位姿消息，包括位置和角度
//定义一个全局变量，用于在命令行中输入turtle的名字
std::string turtle_name;
//回调函数的消息为turtlesim的msg
void posecallback(turtlesim::PoseConstPtr &msg)
{
//创建一个TransformBroadcaster存储变换的相关信息
static tf::TransformBroadcaster br;
//接着创建一个对象Transform存储变换的相关信息,它包含旋转和位移的信息，有一个向量叫做Vector3表示坐标，以及四元组表示方向
tf::Transform transform;
//下面把turtlepose的相关信息copy到transform中，通过transform在tf树中进行广播，
transform.setOrigin(tf::Vector3(msg->x,msg->y,0));
//创建一个四元组
tf::Quaternion q；
//调用它的方法,因为它是绕着z轴旋转故有下面参数形式
q.setRPY(0,0,msg->theta);
//为transform的rotation赋值
transform.setRotation(q);
//然后再广播出去,创建一个transform::stamped对象
br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}
int main(int agc,char **argv)
{
ros::init(argc,argv,"turtle_tf_broadcaster");
if(argc!=2)
{
ROS_ERROR("need turtle name as argument.");
return -1;
}
turtle_name=argv[1];//传递进来的名字
ros::NodeHandle node;
//下面创建一个subscriber订阅对象,订阅的主题是turtle_name+"/pose",列表长度为10，回调函数为posecallback
ros::Subscriber sub=node.subscribe(turtle_name+"/pose",10，&posecallback);
ros::spin();
return 0;
}
下面修改CMakeLisets.txt文件
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})