TF坐标变换-将雷达坐标系坐标变换到地图坐标系

1.坐标变换r&t理解

实际程序中有很多坐标变换，看似很简单，可还是需要理解记忆。
　　例如，已知物体在雷达坐标系中坐标pL(xL ,yL)，假设雷达坐标系和机器人坐标系重合，又已知机器人在地图中的坐标，求物体在地图中的坐标pM(xm, ym)？
　　问题很简单，已知pL, 再知道pL和pM之间的R&t，就能计算出pM。
　　直接给出公式：
　　　　　　 R* pL + ｔ＝ｐＭ　或　　 Ｔ＊ｐＬ＝ｐＭ （左乘）
　　但R是什么？R是变换矩阵，使两个坐标系的姿态保持一致。
　　在我们实际问题中，我们知道机器人的里程计或IMU计算的机器人航向角theta。
　　套用如下变换矩阵公式是最快的，但是theta如何取正负？如何理解theta是哪个坐标系相对于哪个坐标系的变换？
　
　　t是平移矩阵。在实际问题中，我们知道机器人的里程计Odom (xｏ, yｏ,theta) ! 如果机器人初始位置在地图原点， 那么直接赋值ｔ＝［xｏ, yｏ］
　　这么赋值的话，就知道了。
　　ｔ是雷达坐标系原点在地图坐标系中的位置，是雷达坐标系相对于地图坐标系的变换。（即地图坐标系相对于自身怎么平移到与雷达坐标系一致）
　　那么上面的Ｒ就知道了，R是雷达坐标系在地图坐标系中的姿态，是雷达坐标系相对于地图坐标系的旋转。（即地图坐标系相对于自身怎么旋转与雷达坐标系姿态一致)
　　＊＊＊右手法则，绕着地图坐标系z轴逆时针旋转为正，那么里程计theta即可直接带入变换矩阵的上式，因为里程计theta就是这么定义的），那么：
　　 　　　　

2.雷达到地图坐标变换

　　已知物体在雷达坐标系坐标(1,1), 机器人里程计数据odom (3, 3, 1,57)，求机器人地图坐标。

　　这样计算很方便，R使用公式，theta为里程计的角度90，是雷达坐标系相对于map坐标系的旋转变换，是右手定则，map坐标系绕自身z轴逆时针旋转90度。t是雷达坐标系相对于map坐标系的平移变换，即里程计的x和y,是map坐标系相对于自身平移的坐标距离。