【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使目标函数最优:
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<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

  • 20
    点赞
  • 38
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值