## 3 The Motion State Estimation Method
### Ego-Motion Compensation  
In applications such as unmanned vehicles and intelligent mobile robots, Lidar is mounted on a mobile platform. The motion of the platform induces relative motion between scene objects, which adversely affects the motion segmentation results of the perception system. The moving object motion vectors may not consistent with their actual trajectories. Ego-motion elimination is a prerequisite to ensure accurate motion segmentation and vector estimation. The ego-motion information can be estimated by other sensors (e.g., IMU, GPS, etc.) [22], but in this paper, we do not use other sensors and only study the motion vector estimation via Lidar data only; so, the ego-motion estimation and compensation are mainly performed by means of point cloud alignment.  
When performing ego-motion estimation, the ground points at different locations usually have similar features and lack obvious texture and structure, which makes the matching of ground points before and after the motion prone to errors; so, the ground points are eliminated before the matching is performed. In this paper, we adopt the method proposed in [23] to exclude the ground points. Subsequently, the non-ground points in different frames are matched using the ICP (Iterative Closest Point) principle [24]. Due to the computational efficiency of the naive ICP method, the VGICP method [25] is utilized to realize the matching between point clouds.  
Concretely, for point cloud \(P_{t}=\{p_{n},n=1,\ldots,N\}\) and \(P_{t+i}=\{q_{m},m=1,\ldots,M\}\) scanned at time \(t\) and \(t+i\), the goal of VGICP is to estimate the transformation matrix \(\mathbf{M}=[\mathbf{R},\mathbf{T}]\) from \(P_{t+i}\) to \(P_{t}\). Given a point \(p_{n}\) and its neighboring points in \(P_{t+i}\) as \(N_{n}=\{q^{\prime}_{m}|\|q^{\prime}_{m}-p_{n}\|<\theta_{d},q^{\prime}_{m}\in P _{t+i}\}\), the distance \(D_{n}\) is obtained as in Equation (1).  
\[D_{n}=\sum_{N_{n}}(q^{\prime}_{m}-\mathbf{M}*p_{n}) \tag{1}\]  
If \(P_{t}\) and \(P_{t+i}\) are Gaussian sampled with distributions \(p_{n}\sim N(\hat{p}_{n},C^{A}_{n})\) and \(q_{m}\sim N(\hat{q}_{m},C^{B}_{m})\), then \(D_{n}\) has a distribution as in Equation (2).  
\[\begin{split} d_{n}\sim& N(\mu^{d},C^{d})\\ \mu^{d}=&\sum_{N_{n}}(q^{\prime}_{m}-\mathbf{M}*p_{ n})=0\\ C^{d}=&\sum_{N_{n}}(C^{B}_{m}+\mathbf{M}*C^{A}_{n}* \mathbf{M}^{T})\end{split} \tag{2}\]  
The transformation matrix \(\mathbf{M}\) can then be obtained by maximizing the log-likelihood function as in Equation (3).  
\[\mathbf{M}=\underset{\mathbf{M}}{\arg\min}\sum_{n}\Bigg{(}\Bigg{(}\sum_{N_{n} }(q^{\prime}_{m}-\mathbf{M}*p_{n})\Bigg{)}^{T}\Bigg{(}\sum_{N_{n}}(C^{B}_{m}+ \mathbf{M}*C^{A}_{n}*\mathbf{M}^{T})\Bigg{)}^{-1}\Bigg{(}\sum_{N_{n}}(q^{ \prime}_{m}-\mathbf{M}*p_{n})\Bigg{)}\Bigg{)} \tag{3}\]  
Figure 1: Overview of the motion state estimation method.  
After the transformation, we have Equation (4).  
\[\mathbf{M}=\underset{\mathbf{M}}{\arg\min}\sum_{n}\left(|N_{n}|\left(\frac{\sum_{N_ {n}}q_{m}^{\prime}}{|N_{n}|}-\mathbf{M}*p_{n}\right)^{T}\left(\frac{\sum_{N_{n} }C_{m}^{B}}{|N_{n}|}+\mathbf{M}*C_{n}^{A}*\mathbf{M}^{T}\right)^{-1}\left(\frac {\sum\limits_{N_{n}}q_{m}^{\prime}}{|N_{n}|}-\mathbf{M}*p_{n}\right)\right) \tag{4}\]  
where \(|N_{n}|\) denotes the number of \(N_{n}\). By voxelizing point cloud \(P_{t+i}\), \(\frac{\sum_{N_{n}}q_{m}}{|N_{n}|}\) and \(\frac{\sum_{N_{n}}C_{n}^{B}}{|N_{n}|}\) can be easily and quickly obtained, with details found in [25]. After obtaining the transformation matrix \(\mathbf{M}\), \(P_{t+i}\) is transformed to \(P_{t+i}^{\prime}\) under the coordinate system of the origin where \(P_{t}\) is located.