点云对齐/轨迹对齐方法及论文讲解
点云对齐/轨迹对齐方法及论文讲解
点云对齐和轨迹对齐是SLAM（同时定位与地图构建）领域的重要技术，广泛应用于机器人导航、自动驾驶和三维重建等领域。本文将介绍三种常用的点云对齐方法：Umeyama算法、Horn四元数闭合求解算法和Marta Salas的迭代最小二乘解法，并分析它们的优缺点及改进思路。
点云对齐/轨迹对齐方法及论文讲解
已知两组对应点云，求解两组点云之间的坐标系变换。或者计算里程计的精度问题，得到里程计估计的时间离散位姿后以及轨迹位姿真值后，如何评判里程计的精度。
本文介绍三种方法：
- Umeyama算法
- 参考文献：《Least-Squares Fitting of Two 3-D Point Sets》
- Horn四元数闭合求解算法
- 参考文献：《Closed-form solution of absolute orientation using unit quaternions》
- 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
存在的问题
Umeyama算法不能处理两个处于非同手性坐标系的情况，意思是如果其中一个坐标系是左手坐标系，而另外一个是右手坐标系时，会出现解算错误的情况。
我们可以生成一组三维轨迹，如三维S曲线，使用Umeyama对齐后的结果如下：
结果会出现对齐问题。
改进方法
左手坐标系可以通过反向一个轴，或者交换两轴位置，则变为右手坐标系。无论是我们反向三轴中的哪个轴，或者是交换哪两个轴，则都可以通过旋转将该两个坐标轴对齐。
引入一个S矩阵，S为单位阵或S = diag[1,1,-1]。从而可以解决不同手性两个坐标系的对齐问题。Umayama中没有考虑角度，设立也只说明对于三维位置或者三维点云对齐的优化方法，设p_i, q_i, i=1,...,N为对应的点。则加入S的解决问题变为：
$$
\min\sum_{i=1}||Sq_i=Rp_i+t||_2
$$
也可以化为：
$$
\min\sum_{i=1}||q_i=SRp_i+St||2\rightarrow\min\sum{i=1}||q_i=SRp_i+t'||_2
$$
同样的，将点云中心化后（p_i-center(p_i), q_i-center(q_i)）得到的\overline p_i,\overline q_i, i=1,...,N则问题变为：
$$
\min\sum_{i=1}||\overline q_i=SR\overline p_i||_2
$$
注意，与Umeyama算法不同的是，改进方法的约束保证R阵的正定向，但是不舍弃S。而Umayama是以假设两个坐标系是只需要通过平移旋转即可对齐的，即同手性假设。
改进的思路正视了S与不同手性情况对应的意义，当S = diag[1,1,-1]，则表明使用不同手性情况去对齐，的最小值更优，即两个坐标系是不同手性的。由于测量误差的存在，在实际应用中，只能是情况估计，表明大概率两个坐标系是不同手性的。而如果是完全的数据模型，则可以确定两个坐标系是不同手性。
即当不同手性出现时，S = diag[1,1,-1]
Umeyama的解算结果为：
$$
\overline q_i=SSR\overline p_i=R\overline p_i
$$
而改进的思路为：
$$
\overline q_i=SR\overline p_i
$$
2 Horn四元数闭合求解算法
使用单位四元数实现求解旋转闭式解。
设两组点集分别为r_l和r_r，则其误差方程构成如下：
$$
e_i=\bm r_(r,i)-sR(\bm r_(l,i) )-\bm r_0
$$
其中待求解的变量为旋转矩阵R，平移向量r_0及尺度因子s
（1）求解最少满足的点数
对齐两个轨迹或者对齐两组点云所要求解的自由度包含两组坐标系的3个平移，3个旋转及一个放缩尺度共7个自由度。3个点能建立9个约束(一个三维点对应3个约束)。因此3个点则能够满足求解约束。
（2）点集质心约束及平移求解
设两组点集质心对应计算如下：
误差构成的最小二乘问题：
通过对r_0求导可得:
$$
r_0=\bm {\overline{r}}_r-sR\bm {\overline{r}}_l
$$
在两组点集的质心处建立新的坐标系，轴向与原坐标系一致，从而得到点集在新坐标系下的坐标：
$$
r'{l,i}=\bm r{l,i}-\bm {\overline{r}}l,\bm r'{r,i}=\bm r_{r,i}-\bm {\overline{r}}_r
$$
将质心代入到误差方程，可消去r_0，误差方程变为：
$$
e'i=\bm r'{r,i}-sR\bm r'_{l,i}
$$
（3）尺度求解
1）由新的误差方程构成最小二乘问题：
得到关于s的一元二次方程，解得：
2）未知R阵求解s
将误差方程进行如下调整：
由于R^T R=I将关于s与R分离，构成关于s的最小二乘问题，从而在未知R阵的情况下得到s值，即
另外，同时需满足：
$$
\mathop{\min}\limits_{R} \sum_{i=1}^{n} \bm r_{r,i}^{'T}R\bm r'_{l,i}
$$
（4）三点求解旋转
在已知两组坐标系之间的旋转变换后，求解平移及尺度因子是相对比较简单的。因此求解R阵是该问题的关键。
设3个不共线点在两组坐标系下的表示为：r_{l,1}, r_{l,2}, r_{l,3}∈r_l及r_{r,1}, r_{r,2}, r_{r,3}∈r_r
利用3个点分别建立新的坐标系，设r_{l,1}为坐标系l的原点，r_{l,1}→r_{l,2}为x轴方向，y轴位于三个点构成的平面上，垂直于x轴，z轴符合右手定则正交与x轴与z轴。如下示意图：
令：x_l=r_{l,2}-r_{l,1}，\hat {x}_l=x_l/‖x_l‖，则\hat {x}_l为指向x轴的单位向量
令：y_l=(r_{l,3}-r_{l,1})-[(r_{l,3}-r_{l,1}) ⋅ \hat {x}_l ] \hat {x}_l，\hat {y}_l=y_l/‖y_l ‖
[(r_{l,3}-r_{l,1})⋅\hat {x}l ] \hat {x}l为r{l,3}-r{l,1}向量在x轴方向上的投影向量，即为坐标系原点到第三点r_{l,3}在x轴的投影点的向量，因此(r_{l,3}-r_{l,1})-[(r_{l,3}-r_{l,1}) ⋅ \hat {x}_l ] \hat {x}l为r{l,3}在y轴的投影点的向量。y^ l 为其单位向量，即指向y轴的单位向量。
令：\hat {z}_l=\hat {x}_l×\hat {y}_l为对应z轴的单位向量，得到新坐标系单位矩阵构成的基向量M_l=|\hat {x}_l,\hat {y}_l,\hat {z}_l |
同理使用第二组点集构成新的坐标系单位向量M_r=|\hat {x}_r,\hat {y}_r,\hat {z}_r|
M_l与M_r分别为三个点构成的坐标系的基础向量在两个原始坐标系的表示。如下图示意图所示：
因此：
$$
M_r=R M_l
$$
从而可知：
$$
R=M_r M_l^T
$$
（5）四元数求解旋转矩阵
使用四元数q表示旋转：
$$
q=q_0+iq_x+jq_y+kq_z
$$
r'=Rr
将空间点表示成思维虚四元数
$$
\overline r=[0,\bm r]^{\rm T}=[0,x,y,z]^{\rm T}=0+ix+jy+kz
$$
r'=q\overline rq^{-1}
其中，q^{-1}=\bm q^/\bm q^2,q^为q的共轭四元数，q^=q_0-iq_x-jq_y-kq_z
文中使用单位四元数，因此q^2=q\cdot q=1，因此，q^{-1}=\bm q^*
定义四元数的两种乘法
1）直接
2）点乘
要求解
$$
\mathop{\max}\limits_R \sum_{i=1}^n(\bm r_{r,i}'^T R\bm r'_{l,i})
$$
使用四元数表示：
$$
\mathop{\max}\limits_{\bm q} \sum_{i=1}^n(\bm{\overline r}'{r,i} (\bm{q\overline r}{l,i}\bm q^{-1} ))
$$
向量乘法交换律：
由四元数的性质:
上式可化为：
其中：\overline r=[0,\bm r]^{\rm T}=[0,x,y,z]^{\rm T}
以此得：
令:
上式的解：q为N对应最大特征值对应的特征向量，证明过程见参考论文附文A3
（6）总结求解流程：
1）整理残差方程
2）求解尺度因子
3）求解R阵对应四元数表示q
求解其最大特征值对应的特征向量
4）求解偏移量
$$
r_0=\bm{\overline r}_r-sR\bm{\overline r}_l
$$
3 Marta Salas 的迭代最小二乘解法
采用最小二乘迭代求解轨迹对齐变换矩阵，除考虑平移，也考虑了姿态角构成误差方程的约束：
设里程计估计得到的位姿组为P_1, P_2, ⋯P_N，真实轨迹对应的Q_1, Q_2, ⋯Q_N，定义两组位姿P_i和G_i之间的误差为：E_i =Q_i^{-1} TP_i
每个位姿由3轴旋转R及3轴平移构成的4*4矩阵，该矩阵符合李群性质。
对误差进行log-v操作，将误差变换到切空间：该操作与将旋转矩阵转换为李代数(轴角)一致。
$$
d_i:=log\left((\mathbf{Q}_i)^{-1}\mathbf{T}\mathbf{P}_i\right)^\vee
$$
定义两组轨迹的总误差约束方程为：
$$
\arg\min_{\mathbf{T}}\sum_{i=1}^N\mathbf{d}_i^T(\mathbf{T})\boldsymbol{\Lambda}_i\mathbf{d}_i(\mathbf{T})
$$
使用LM方法迭代求解T阵。
列出存在尺度求解时的雅可比矩阵：
$$
\mathbf{J} = \frac{\partial}{\partial\boldsymbol{\delta}}log\left(\mathbf{Q}{i}^{-1}exp(\boldsymbol{\delta})\mathbf{T}\mathbf{P}{i}\right){\mathbf{sim}(3)}^{\vee} |{\delta=0}\approx Adj_{\mathbf{Q}_{i}^{-1}}
$$
令
$$
S=Q^{-1}=\left[\begin{array}{cc}s\mathbf{R}&\mathbf{t}\\mathbf{0}_{1\times3}&1\end{array}\right]
$$
$$
Adj_{\mathbf{S}}=\left[\begin{array}{ccc}s\mathbf{R}&[\mathbf{t}]\times\mathbf{R}&-\mathbf{t}\\mathbf{0}{3\times3}&\mathbf{R}&\mathbf{0}{3\times1}\\mathbf{0}{1\times3}&\mathbf{0}_{1\times3}&1\end{array}\right]\in\mathbb{R}^{7\times7}
$$
$$
(\mathbf{J}^T\mathbf{\Lambda}\mathbf{J}+\mu\mathbf{I})\mathbf{\delta}=-\mathbf{J}^T\mathbf{\Lambda}\mathbf{d}(\mathbf{T})
$$
$$
\mathbf{T}^{k+1}=exp(\hat{\boldsymbol{\delta}})\mathbf{T}^{k}
$$
注：
存在问题
Salas方法虽然考虑角度，其问题假设如下：
设两组对应6DOF轨迹P_i, Q_i, i=1,...,N。其对应关系为：
$$
Q_i=TP_i=\left[\begin{array}{cc}s\mathbf{R}&\mathbf{t}\\mathbf{0}_{1\times3}&1\end{array}\right]P_i
$$
其中：P_i, Q_i, T均∈ Sim(3)。
而实际的轨迹中，如TUM，使用四元数存储旋转，是没有尺度的，即P_i, Q_i ∈ SE(3)，而T ∈ Sim(3)。如此会导致尺度的解析问题。
同样，使用KITTI数据集加尺度验证该问题：
解算出现尺度解析错误。
改进思路
对于无尺度轨迹文件，如TUM格式，使用如下约束来进行求解：
$$
T(\xi_{\mathrm{T}})=arg\mathrm{min}\sum_{i=1}^{N}|r_{i}(\xi_{\mathrm{T}})|{2}=arg\mathrm{min}\sum{i=1}^{N}\left|log\left(\left(T_{gi}^{-1}\cdot\begin{bmatrix}sI_{3\times3}&0\0&1\end{bmatrix}\right)^{-1}TT_{ei}\right)^{\vee}\right|_{2}
$$
其中T_{gi}对应Q_i，T_{ei}对应P_i。
则对应的雅可比矩阵变为：
$$
J_{ri}=adj\left(T_{gi}\cdot\left[\begin{matrix}{sI_{3\times3}}&{0}\{0}&{1}\\end{matrix}\right]\right)^{-1}
$$
从而对齐：