点云对齐/轨迹对齐方法及论文讲解

点云对齐/轨迹对齐方法及论文讲解

已知两组对应点云，求解两组点云之间的坐标系变换。或者计算里程计的精度问题，得到里程计估计的时间离散位姿后以及轨迹位姿真值后，如何评判里程计的精度。
本文介绍三种方法：
1.Umeyama算法
参考文献：
《Least-Squares Fitting of Two 3-D Point Sets》
2.Horn四元数闭合求解算法
参考文献：
《Closed-form solution of absolute orientation using unit quaternions》
3.Marta的迭代最小二乘解法
参考文献：
《Trajectory Alignment and Evaluation in SLAM: Horn’s Method vs Alignment on the Manifold》

1 Umeyama算法

原理说明https://blog.csdn.net/weixin_41469272/article/details/105314963
代码实现及工具使用https://blog.csdn.net/weixin_41469272/article/details/119885449

2 Horn四元数闭合求解算法

使用单位四元数实现求解旋转闭式解。
设两组点集分别为${\mathbf{r}}_l$和${\mathbf{r}}_r$，则其误差方程构成如下：
$${\mathbf{e}}_i={\mathbf{r}}_{(}r,i)-sR({\mathbf{r}}_{(}l,i))-{\mathbf{r}}_0$$
其中待求解的变量为旋转矩阵R，平移向量${\mathbf{r}}_0$及尺度因子$s$

（1）求解最少满足的点数
对齐两个轨迹或者对齐两组点云所要求解的自由度包含两组坐标系的3个平移，3个旋转及一个放缩尺度共7个自由度。3个点能建立9个约束(一个三维点对应3个约束)。因此3个点则能够满足求解约束。

（2）点集质心约束及平移求解
设两组点集质心对应计算如下：

误差构成的最小二乘问题：

通过对${\mathbf{r}}_0$求导可得:
$${\mathbf{r}}_0={\overline{\mathbf{r}}}_r-sR{\overline{\mathbf{r}}}_l$$
在两组点集的质心处建立新的坐标系，轴向与原坐标系一致，从而得到点集在新坐标系下的坐标：
$${\mathbf{r}}_{l,i}^{\prime }={\mathbf{r}}_{l,i}-{\overline{\mathbf{r}}}_l,{\mathbf{r}}_{r,i}^{\prime }={\mathbf{r}}_{r,i}-{\overline{\mathbf{r}}}_r$$
将质心代入到误差方程，可消去$r_0$，误差方程变为：
$${\mathbf{e}}_i^{\prime }={\mathbf{r}}_{r,i}^{\prime }-sR{\mathbf{r}}_{l,i}^{\prime }$$

（3）尺度求解
1）由新的误差方程构成最小二乘问题：

得到关于$s$的一元二次方程，解得：

2）未知R阵求解s
将误差方程进行如下调整：

由于$R^{T}R=I$将关于$s$与$R$分离，构成关于$s$的最小二乘问题，从而在未知$R$阵的情况下得到$s$值，即

另外，同时需满足：
$$\underset{R}{\min }\sum _{i=1}^n{\mathbf{r}}_{r,i}^{{}^{\prime }T}R{\mathbf{r}}_{l,i}^{\prime }$$

（4）三点求解旋转
在已知两组坐标系之间的旋转变换后，求解平移及尺度因子是相对比较简单的。因此求解$R$阵是该问题的关键。
设3个不共线点在两组坐标系下的表示为：${\mathbf{r}}_{l,1},{\mathbf{r}}_{l,2},{\mathbf{r}}_{l,3}\in {\mathbf{r}}_l$及${\mathbf{r}}_{r,1},{\mathbf{r}}_{r,2},{\mathbf{r}}_{r,3}\in r_r$

利用3个点分别建立新的坐标系，设$r_{l,1}$为坐标系l的原点，$r_{l,1}\to r_{l,2}$为x轴方向，y轴位于三个点构成的平面上，垂直于x轴，z轴符合右手定则正交与x轴与z轴。如下示意图：

令：${\mathbf{x}}_l={\mathbf{r}}_{l,2}-{\mathbf{r}}_{l,1}，{\hat{\mathbf{x}}}_l={\mathbf{x}}_l/\Vert {\mathbf{x}}_l\Vert$，则${\hat{\mathbf{x}}}_l$为指向x轴的单位向量
令：${\mathbf{y}}_l=({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1})-[({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1})\cdot {\hat{\mathbf{x}}}_l]{\hat{\mathbf{x}}}_l，{\hat{\mathbf{y}}}_l={\mathbf{y}}_l/\Vert {\mathbf{y}}_l\Vert$
$[({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1})\cdot {\hat{\mathbf{x}}}_l]{\hat{\mathbf{x}}}_l$为${\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1}$向量在x轴方向上的投影向量，即为坐标系原点到第三点${\mathbf{r}}_{l,3}$在x轴的投影点的向量，因此$({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1})-[({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1})\cdot {\hat{\mathbf{x}}}_l]{\hat{\mathbf{x}}}_l$为$r_{l,3}$在y轴的投影点的向量。${\hat{\mathbf{y}}}_l$为其单位向量，即指向y轴的单位向量。
令：${\hat{\mathbf{z}}}_l={\hat{\mathbf{x}}}_l\times {\hat{\mathbf{y}}}_l$为对应z轴的单位向量，得到新坐标系单位矩阵构成的基向量${\mathbf{M}}_l=|{\hat{\mathbf{x}}}_l,{\hat{\mathbf{y}}}_l,{\hat{\mathbf{z}}}_l|$

同理使用第二组点集构成新的坐标系单位向量${\mathbf{M}}_r=|{\hat{\mathbf{x}}}_r,{\hat{\mathbf{y}}}_r,{\hat{\mathbf{z}}}_r|$
${\mathbf{M}}_l$与${\mathbf{M}}_r$分别为三个点构成的坐标系的基础向量在两个原始坐标系的表示。如下图示意图所示：

因此：
$${\mathbf{M}}_r=R{\mathbf{M}}_l$$
从而可知：
$$R={\mathbf{M}}_r{\mathbf{M}}_l^T$$
（5）四元数求解旋转矩阵
使用四元数$\mathbf{q}$表示旋转：
$\mathbf{q}=q_0+i{q}_x+j{q}_y+k{q}_z$
$$r^{\prime }=Rr$$
将空间点表示成思维虚四元数
$$\overline{\mathbf{r}}=[0,\mathbf{r}{]}^{\mathrm{T}}=[0,x,y,z{]}^{\mathrm{T}}=0+ix+jy+kz$$
$${\mathbf{r}}^{\prime }={\mathbf{q}\overline{\mathbf{r}}\mathbf{q}}^{-1}$$
其中，${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/{\mathbf{q}}^2$, ${\mathbf{q}}^{\ast }$为$q$的共轭四元数，${\mathbf{q}}^{\ast }=q_0-i{q}_x-j{q}_y-k{q}_z$
文中使用单位四元数，因此$q^2=q\cdot q=1$，因此，${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }$

定义四元数的两种乘法
1）直接

2）点乘

要求解
$$\underset{R}{\max }\sum _{i=1}^n({\mathbf{r}}_{r,i}^{\prime T}R{\mathbf{r}}_{l,i}^{\prime })$$
使用四元数表示：
$$\underset{\mathbf{q}}{\max }\sum _{i=1}^n({\overline{\mathbf{r}}}_{r,i}^{\prime }({\mathbf{q}\overline{\mathbf{r}}}_{l,i}{\mathbf{q}}^{-1}))$$
向量乘法交换律：

由四元数的性质:

上式可化为：

其中：$\overline{\mathbf{r}}=[0,\mathbf{r}{]}^{\mathrm{T}}=[0,x,y,z{]}^{\mathrm{T}}$


以此得：

令:

上式的解：q为N对应最大特征值对应的特征向量，证明过程见参考论文附文A3
（6）总结求解流程：
1）整理残差方程

2）求解尺度因子

3）求解$R$阵对应四元数表示$\mathbf{q}$

求解其最大特征值对应的特征向量
4）求解偏移量
$$r_0={\overline{\mathbf{r}}}_r-sR{\overline{\mathbf{r}}}_l$$

Marta的迭代最小二乘解法

采用最小二乘迭代求解轨迹对齐变换矩阵，除考虑平移，也考虑了姿态角构成误差方程的约束：
设里程计估计得到的位姿组为$P_1,P_2,\cdots P_N$，真实轨迹对应的$Q_1,Q_2,\cdots Q_N$，定义两组位姿$P_i$和$G_i$之间的误差为：
$E_i=Q_i^{-1}T{P}_i$
每个位姿由3轴旋转$R$及3轴平移构成的4*4矩阵，该矩阵符合李群性质。
对误差进行log-v操作，将误差变换到切空间：该操作与将旋转矩阵转换为李代数(轴角)一致。

定义两组轨迹的总误差约束方程为：

使用LM方法迭代求解$T$阵

注：