## 3 Method
### Dynamic Object Pose Estimation Assisted by Multi-Sensor Fusion  
The dynamic object pose estimation, and the SLAM process are mutually reinforcing. Furthermore, we construct a dynamic object 6D pose estimation module in the proposed SLAM framework, whose main framework structure is shown in Figure 4.  
This module uses panoptic information to identify individual dynamic objects within the sensor's field of view and associate feature points with them. By exploiting the existing odometer's matches information of points, the module naturally acquires the matching relationship of dynamic objects between consecutive multiple frames of motion. To estimate the 6D positional information of dynamic objects, we input the SLAM system's body pose and the coordinates of continuously tracked feature points into a nonlinear optimization solver. Details of the solver's design are provided below. The dynamic object poses estimation, and SLAM processes mutually reinforce each other, improving the overall system's accuracy. Figure 4: Framework of dynamic object pose estimation. The framework integrates various components, leveraging multi-sensor fusion, panoptic segmentation, and optimizer solvers for accurate and real-time estimation of the 6D pose of dynamic objects within the environment. Figure 3: Cluster analysis outcome. On the left, the clustering results are presented, juxtaposed with the initial feature distribution on the right. The color-coded legend distinguishes static features in blue, features linked to one dynamic object in red, those associated with another dynamic object in green, and noise in yellow. Benefiting from the advantages of multi-sensor fusion, when the dynamic object is in the LiDAR field-of-view range, we can obtain the depth information of the corresponding point by the following equation. \[p_{\text{unit}}(\theta,\phi) =f(P_{LiDAR})\] \[\phi =\alpha+\frac{c\times s_{v}}{C} \tag{8}\] \[\theta =i\times\delta\theta,\]  
where \(c\) the channel of point, \(C\) the total number of channels of LiDAR, \(s_{v}\) the scale of vertical FOV, \(\alpha\) the angle of first LiDAR ring, \(p_{unit}\) the coordinate of unit sphere, \(i\) the sequence of point, \(\delta\theta\) the angle increment. After acquiring a certain number of points with depth information, we can construct the following least squares method to find the pose of the corresponding dynamic target. \[\min\sum_{i=1}^{m}\frac{1}{2}\|f(\hat{p}_{\{i,k\}})-T_{d}T_{c}f(\hat{p}_{\{i,k +1\}})\|_{2}^{2} \tag{9}\]  
However, the dynamic object may only sometimes be within the sensing range of the LiDAR, resulting in a lack of point depth information. To address this, the paper also proposes a method for estimating the pose of dynamic objects without depth information. The pose estimation becomes particularly difficult for monocular cameras that lack depth information. The traditional para-polar geometry method does not apply to dynamic object pose estimation because the reverse-projected rays are disjoint. The second is the problem of scale information. The monocular camera needs scale information, and the relative scale of dynamic objects in the camera range is also a challenge that must be addressed. Generally, SLAM systems use cameras with frame rates of 10 to 20 frames or even higher, so we assume that the 6D positional transformation \(T_{d}\) of the dynamic object is the same in three consecutive frames, as shown in Figure 5. In such a short time interval, even if the acceleration reaches 1G, the change in its velocity is still less than 2 m/s. For a slower car, the change in its velocity is even more minor. Therefore, we can express it by the following equation. \[P_{\{i,k\}}=T_{d}P_{\{i,k+1\}}=T_{d}^{2}P_{\{i,k+2\}} \tag{10}\]  
where \(P_{\{i,k\}}=[x,y,z]^{T}\) is the coordinate of the point \(P_{i}\) on the \(k\) th frame.