用Eigen::umeyama计算两组向量的旋转变换

之前使用pcl库提供的icp算法对点云进行校准，但是校准结果质量很不理想，因为用于计算迭代方程的同名点对选取的效果很不好，于是决定人工选取同名点对，再求解旋转矩阵R和平移矩阵T，得到校准后的点云。
人工选取同名点对通过为osg视图添加两个EventHandler实现，分别是PickHandler和CvlMeasureHandler。PickHandler的作用是根据鼠标在屏幕上的位置获取（pick）视图内相对应的点的坐标，CvlMeasureHandler的作用是高亮显示被选中的点。
人工选取若干组点对后，根据点对的坐标信息计算旋转矩阵。参考pcl的icp方法计算旋转矩阵的流程，使用Eigen内置的umeyama方法计算旋转矩阵。Umeyama是一种求解m维欧氏空间中两点模式匹配的算法，参考：umeyama算法。在Umeyama.h文件中的定义：

umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true)

使用：首先声明一个4X4的矩阵用于存放旋转矩阵：

Eigen::Matrix<double, 4, 4> dTransRT;

随后声明两个输入矩阵target和source，旋转矩阵是从source点云的坐标系向target坐标系转换的矩阵。以100个点对为例：

Eigen::Matrix<double, 3, Eigen::Dynamic> cloud_tgt(3, 100);
Eigen::Matrix<double, 3, Eigen::Dynamic> cloud_src(3, 100);

为这两个输入矩阵内部元素赋值。
计算旋转矩阵：

dTransRT = pcl::umeyama(cloud_src, cloud_tgt, false);

矩阵dTransRT的内部数据组织结构是一个长度为16的一维数组，包含了旋转矩阵R和平移矩阵T，其中平移矩阵位于dTransRT的第12-14位，访问方式为dTransRT(12)。旋转矩阵位于第0 1 2 4 5 6 8 9 10位。排列方式：
R=
[0,4,8;
1,5,9;
2,6,10;]