【Eigen学习笔记】-- Umeyama

最新推荐文章于 2024-05-26 19:45:00 发布

琼筵醉月

最新推荐文章于 2024-05-26 19:45:00 发布

阅读量9.8k

点赞数 20

分类专栏：点云文章标签：算法人工智能

本文链接：https://blog.csdn.net/weixin_42823098/article/details/111308627

版权

点云专栏收录该内容

7 篇文章 1 订阅

订阅专栏

Umeyama算法

前言
一、Umeyama目标
二、Umeyama原理介绍
三、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 - (cRp_i + t) ||_2^2$
从该目标函数公式上可以看出，最终的判断标准是距离的平方和，也就是一个最小二乘估计问题，仔细看看，是不是与icp的损失函数是一样的；
具体推导公式就不做累述了，若后期有需求，会添加。
最终计算的结果为：
$\begin{bmatrix} cR & t \\ 0 &1 \end{bmatrix}$
其中, c表示一个缩放系数。

三、Umeyama使用方法

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

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

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

   std::vector<Eigen::Vector4f> s, t;
   Eigen::Matrix<double, 3, Eigen::Dynamic> cloud_tgt(3, 4);
   Eigen::Matrix<double, 3, Eigen::Dynamic> 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。