对齐两段不同坐标系的轨迹

对齐两段不同坐标系的轨迹，实际就是已知两两3D点的对应关系，求取它们之间的R,T。这实际就是简化的ICP算法（省去了寻找点的对应关系这一步）。
设一段轨迹的3d点为pt, 一段为ps，那么它们之间的关系为：
$$s\ast p_t=R\ast p_s+T$$
假设两段轨迹的尺度是一样的，则s=1.
将R,T去耦合，我们知道点云的平均值也是满足旋转关系的，即
$$\overline{p_t}=R\ast \overline{p_s}+T$$
故：
$$p_t-\overline{p_t}=R\ast (p_s-\overline{p_s})$$
简写为：
$$q_t=R\ast q_s$$
对于所有的点
$$sum=\sum _i^N||q_t^i-R{q}_s^i|{|}^2$$
即求R使得sum最小。
$$\begin{gathered} sum=\sum _i^N(q_t^i-R{q}_s^i{)}^T(q_t^i-R{q}_s^i) \\ =\sum _i^N({q_t^i}^T\ast q_t^i+{q_s^i}^T\ast q_s^i-{q_t^i}^T\ast R\ast q_s^i-{q_s^i}^T{R}^t\ast q_t^i) \end{gathered}$$
R是单位对称矩阵，有：
$$\begin{gathered} R=R^T \\ {a}^T\ast R\ast b=b^T\ast R\ast a \end{gathered}$$
故上面可以简化为：
$$\sum _i^N({q_t^i}^T\ast q_t^i+{q_s^i}^T\ast q_s^i-2{q_t^i}^T\ast R\ast q_s^i$$
即求R使得F最大
$$\begin{gathered} F=\sum _i^N{q_t^i}^T\ast R\ast q_s^i \\ =Trace(\sum _i^{N}R{q}_s^i{q_t^i}^T) \\ =Trace(RH) \\ H=\sum _i^N(q_s^i{q_t^i}^T) \end{gathered}$$
上面转换是由于$$Trace(AB)=Trace(BA)$$

当det(X) = -1 时，

因此我们最终的结果：
$$\begin{gathered} ifdet(V{U}^T)=1 \\ R=V{U}^T \\ ifdet(V{U}^T)=-1 \\ R=V^{\prime }{U}^T \\ {V}^{\prime }=[v1,v2,-v3] \\ T=\overline{p_t}-R\ast \overline{p_s} \end{gathered}$$
补充一点线性知识：
1.Trace迹，是矩阵主对角线的和。
对于方阵A，$$A=Q^{-1}\Lambda Q$$
$$\begin{gathered} trace(A)={\lambda }_1+{\lambda }_2+... \\ det(A)={\lambda }_1\ast {\lambda }_2\ast ... \end{gathered}$$
如果A和B是相似矩阵$$B=P^{-1}AP$$
那么
trace(A) = trace(B)
det(A) = det(B)
实际是因为A和B的特征值是一样的。

A可以看做是imu坐标系的旋转矩阵，B可以看做是wheel坐标系下的旋转矩阵
B= R_wheel2b.transpose() * A * R_wheel2b
哈哈，就是我们代码里常用的公式了

参考论文：Least-Squares Fitting of Two 3-D Point Sets