两个坐标系转换的变换矩阵

####问题：假定下图中的A坐标系是世界坐标系，我们想求出目标坐标系B到世界坐标系的旋转矩阵。

从B到A的变换既有旋转也有平移。从B到A的变换矩阵是一个4x4的矩阵算子。


B到A的旋转矩阵的定义式：

其中


VS C++实现上述旋转矩阵求解：

```cpp
//求旋转矩阵(两个坐标系)适用于已知转换前的坐标系的三个轴分量，和目标坐标系的一个轴分量。若已知6个坐标轴分量，则注释y,x那两行
Eigen::Matrix3d CoordinateRotation(Eigen::Vector3d u, Eigen::Vector3d v, Eigen::Vector3d w,
	Eigen::Vector3d & x, Eigen::Vector3d & y, Eigen::Vector3d & z)
{

	Eigen::Matrix3d transition;

		y = z.cross(w);//若你完全已知六个轴分量请注释这两行
		x = y.cross(z);//

	x.normalize();
	y.normalize();
	z.normalize();
	transition << u.dot(x), v.dot(x), w.dot(x),
		u.dot(y), v.dot(y), w.dot(y),
		u.dot(z), v.dot(z), w.dot(z);
	transition = transition.transpose();
	return transition;

}

其中u,v,w分别是转换之前的坐标系的坐标轴的分量，x,y,z是转换后的矩阵的坐标轴分量。上述求法是把基坐标系的z轴转换到与目标坐标系的z坐标轴处。所需的头文件自己加，大概就是#include <Eigen/Dense>那些。