【Eigen学习笔记】-- Umeyama

前言

一、Umeyama目标

在研究pcl中的icp算法时，看到在求解对应点关系矩阵时，用的是Umeyama算法，此算法在Eigen中已经实现了封装，因此，可直接进行调用求解，该算法的原始论文是：Least-squares estimation of transformation parameters between two point patterns", Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573。
Umeyama算法是为了计算两组数据之间的位置关系，比如说，现在有两组位于不同坐标系下的点位数据（可以是2D，也可是是3D），若事先已经知道点位之间的对应关系，则利用Umeyama·算法可以计算出两组数据间的旋转与平移矩阵。

二、Umeyama原理介绍

该算法的目标是计算一组R,t使目标函数最优：
$$\frac{1}{n}\sum _{i=1}^n||q_i-(cR{p}_i+t)|{|}_2^2$$
从该目标函数公式上可以看出，最终的判断标准是距离的平方和，也就是一个最小二乘估计问题，仔细看看，是不是与icp的损失函数是一样的；
具体推导公式就不做累述了，若后期有需求，会添加。
最终计算的结果为：
$$T=\left[\begin{array}{cc}cR& t\\ 0& 1\end{array}\right]$$
其中, c表示一个缩放系数。

三、Umeyama使用方法

为了能更为准确的计算位置关系，输入的数据P与Q需要满足以下情况：

• P和Q中样点的数量是相同的，且数据之间的对应关系也是一致的
• P或Q中的样点不能共线

如下列代码片段所示，其中, cloud_tgt与cloud_src为3 * n维的矩阵，

   std::vector s, t;
   Eigen::Matrix cloud_tgt(3, 4);
   Eigen::Matrix cloud_src(3, 4);
   for (int i = 0; i < s.size(); i++) {
   	cloud_src(0, i) = s[i](0);
   	cloud_src(1, i) = s[i](1);
   	cloud_src(2, i) = s[i](2);
   	cloud_tgt(0, i) = t[i](0);
   	cloud_tgt(1, i) = t[i](1);
   	cloud_tgt(2, i) = t[i](2);
   }
   Eigen::Matrix4d rt = Eigen::umeyama(cloud_src, cloud_tgt, false);

当umeyama函数的最后一个值设置为false时，第二节中的c = 1。