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})