Lidar SLAM运动方程和点云补偿的推导

运动方程

做scanmatch前，需要预测lidar当前姿态，用线性插值方法估计。
问题：
已知n时刻姿态为$(q_n,p_n)$, 和n-1时刻与n时刻的相对运动关系$(q_n^{n-1},p_n^{n-1})$，预测n+1时刻姿态。

解答：
world <- n <- n+1

$$\begin{gathered} X_w=q_n^w{X}_n+p_n^w \\ {X}_w=q_n^w(q_{n+1}^n{X}_{n+1}+p_{n+1}^n)+p_n^w \\ {X}_w=q_n^w{q}_{n+1}^n{X}_{n+1}+q_n^w{p}_{n+1}^n+p_n^w \\ {X}_w\approx q_n^w{q}_n^{n-1}{X}_{n+1}+q_n^w{p}_n^{n-1}+p_n^w \\ => \\ {q}_{n+1}^w=q_n^w{q}_n^{n-1},p_{n+1}^w=q_n^w{p}_n^{n-1}+p_n^w \end{gathered}$$

点云补偿

问题：
已知n时刻姿态为$(q_n,p_n)$, 和n-1时刻与n时刻的相对运动关系$(q_n^{n-1},p_n^{n-1})$，以及某点在$n^{\ast }$时刻$X_{n^{\ast }}$，其中$(n<=n^{\ast }<n+1)$ ，预测该点在n+1时刻，在姿态$(q_{n+1},p_{n+1})$下的坐标。

解答：
先线性插值得到$n^{\ast }$时刻姿态
$$\begin{gathered} q_{n^{\ast }}^n=q_{n+1}^n.slerp(s) \\ {p}_{n^{\ast }}^n=s{p}_{n+1}^n \\ => \\ {q}_{n^{\ast }}^n=q_n^{n-1}.slerp(s) \\ {p}_{n^{\ast }}^n=s{p}_n^{n-1} \end{gathered}$$
其中s为时间占比。

$n+1\leftarrow n\leftarrow n^{\ast }$
$$\begin{gathered} X_n=q_{n+1}^n{X}_{n+1}+p_{n+1}^n \\ => \\ {X}_{n+1}={q_{n+1}^n}^{-1}(X_n-p_{n+1}^n) \\ {X}_{n+1}={q_{n+1}^n}^{-1}(q_{n^{\ast }}^n{X}_{n^{\ast }}+p_{n^{\ast }}^n-p_{n+1}^n) \\ => \\ {X}_{n+1}\approx {q_n^{n-1}}^{-1}(q_{n^{\ast }}^n{X}_{n^{\ast }}+p_{n^{\ast }}^n-p_n^{n-1}) \end{gathered}$$