【Eigen学习笔记】-- Umeyama

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使目标函数最优:
1 n ∑ i = 1 n ∣ ∣ q i − ( c R p i + t ) ∣ ∣ 2 2 \frac{1}{n} \sum_{i = 1}^n ||q_i - (cRp_i + t) ||_2^2 n1i=1nqi(cRpi+t)22
从该目标函数公式上可以看出,最终的判断标准是距离的平方和,也就是一个最小二乘估计问题,仔细看看,是不是与icp的损失函数是一样的;
具体推导公式就不做累述了,若后期有需求,会添加。
最终计算的结果为:
T = [ c R t 0 1 ] T = \begin{bmatrix} cR & t \\ 0 &1 \end{bmatrix} T=[cR0t1]
其中, c表示一个缩放系数。

三、Umeyama使用方法

为了能更为准确的计算位置关系,输入的数据PQ需要满足以下情况:

  • PQ中样点的数量是相同的,且数据之间的对应关系也是一致的
  • PQ中的样点不能共线

如下列代码片段所示,其中, cloud_tgtcloud_src3 * 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

你可能感兴趣的:(点云,算法,人工智能)