坐标系变换与坐标变换的关系

问题： 假如坐标系A变换到坐标系B的变换关系为T，那么坐标系B中的点Pb变换到坐标系A中将如何计算呢？

回顾一个实际例子： 在激光SLAM中，假如雷达的位姿已知为pose，雷达坐标系下点云坐标为Point，则点云在地图坐标系中的坐标计算公式为：P(map)=Pose*Point;

在该例子中，Pose相当于地图坐标系变换到雷达坐标系的变换关系，也就是说P（map）=T(map-lidar)*P(lidar)。

总结：
从上述例子中可以看出坐标系变换与坐标变换的关系为：P（B）=T(B-A)*P(A)
式中：
T（B-A）：B坐标系变换到A坐标系中的变换关系；
P(B)： 变换到B坐标系的坐标；
P（A）： A坐标系中的坐标。

应用：
1、在组合导航或者SLAM领域，已知惯导输出的导航坐标系位姿为X，Y，Z，yaw，pitch，roll，激光雷达点云坐标系的局部坐标为P（lidar），假设雷达坐标系与惯导坐标系完全重合而不需要变换，那么如何计算点云坐标在导航坐标系下的坐标呢？
求解：根据组合导航位姿计算变换矩阵T，则P(导航)=T*P(lidar)。

//姿态角计算旋转矩阵
template<typename T>
		void INS2Matrix(const T &r, const T &p, const T &h, Eigen::Matrix<T, 4, 4> &m)
		{
			T rr = r * PI / 180.0;
			T pr = p * PI / 180.0;
			T hr = -h * PI / 180.0;//负号的添加很关键
			T sh = sin(hr);
			T ch = cos(hr);
			T sp = sin(pr);
			T cp = cos(pr);
			T sr = sin(rr);
			T cr = cos(rr);
			m = Eigen::Matrix<T, 4, 4>::Identity();
			m(0, 0) = ch * cr - sh * sp * sr;
			m(0, 1) = -sh * cp;
			m(0, 2) = ch * sr + sh * sp * cr;

			m(1, 0) = sh * cr + ch * sp * sr;
			m(1, 1) = ch * cp;
			m(1, 2) = sh * sr - ch * sp * cr;

			m(2, 0) = -cp * sr;
			m(2, 1) = sp;
			m(2, 2) = cp * cr;
		}