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