3.2.1. Ground feature extraction

First, we adjust ground parameters in the point cloud using the iterative PCA algorithm proposed in ref. [Reference Zermas, Izzat and Papanikolopoulos44] and mark the corresponding points near the ground in the point cloud ${\boldsymbol{\Phi}}_g^i$ (1: ground point, 0: background point).

Assume ${t^{i - 1}}$ and ${t^{i}}$ are the beginning and end time of current frame $i$ respectively. Meanwhile, the sampling time of the k-th raw point ${}^l{{\textbf{p}}_{i,k}}$ in the lidar coordinate $ l$ is ${t_k}$ , and the relative timestamp ${}^r{t_k}$ used to correct motion distortion can be calculated as follows:

(1) \begin{align}{}^r{t_k} = \frac{{{t_k} -{t^i}}}{{{t^{i - 1}} -{t^i}}} \end{align}

Then, the current point cloud is uniformly segmented according to the scan line. Through the point-by-point verification of ground markings in the point cloud ${\boldsymbol{\Phi}}_g^i$ , it is possible to record whether each segment contains the ground point or background point labels. For segments containing ground or background points, the segment head and tail point ID pairs are stored in the ground container ${{\textbf{V}}_g}$ or background container ${{\textbf{V}}_b}$ , respectively.

Based on the calculated roughness (the calculation formula can be found in ref. [Reference Zhang and Singh1]) and the segmented point ID pairs contained in the ground container ${{\textbf{V}}_g}$ , ground feature set ${\boldsymbol{\chi }}_f^i$ can be extracted from point cloud segments containing ground points. The roughness prior ${R_{a}}$ used for subsequent inspection of background surface feature points is obtained as follows:

(2) \begin{align}{R_{a}} = \sum \limits _{o = 1}^N c_o\Big/N_c \end{align}

where ${c_o}$ is the $o$ -th ground point roughness. ${\boldsymbol{\chi }}_c^i$ represents the candidate ground feature set obtained by downsampling, and ${N_c}$ is the corresponding number of ground points.

For non-ground point cloud segments that are searched by ID pairs in the background container ${{\textbf{V}}_b}$ , only point roughness is calculated and sorted here. After that, the information fusion module will neatly select the background feature sets.

3.2.2. Semantic object detection

To identify the semantic objects and their corresponding 3D-BB in the point cloud, we use 3DSSD [Reference Yang, Sun, Liu and Jia45], a point-based single-stage object detection model, to perform these tasks. Assuming that k-th 3D-BB in frame $ i$ is:

(3) \begin{align}{{\boldsymbol{\tau }}^{k,i}} ={\left [{{x^{k,i}},{y^{k,i}},{z^{k,i}},{l^{k,i}},{w^{k,i}},{h^{k,i}},ya{w^{k,i}}} \right ]^T} \end{align}

where the object position in the lidar coordinate $ l$ is $ \left ({x,y,z} \right )$ . The length, width, and height of 3D-BB are $ \left ({l,w,h} \right )$ , and $ yaw$ is the yaw angle for the object in the lidar coordinate $ l$ . Assuming ${{\boldsymbol{\gamma }}^{k,i}}$ denotes the object point cloud enclosed by 3D-BB and each 3D point is marked with an independent object ID in ${\boldsymbol{\Phi}}_s^i$ . Object semantic confidence is represented as ${\mu ^{k,i}}$ .

3.2.3. Information fusion

The 3D-BB, obtained by the semantic detection module, usually encloses both object and ground points. Meanwhile, the ground feature set computed by Section 3.2.1 may also have points belonging to objects. If the object is in motion, the static ground points in the object point cloud and the moving object points in the ground feature set will lead to errors in the pose estimation of the object and the robot, respectively. Therefore, we need to eliminate ambiguous intersection information through a mutual correction step.

Specifically, the object points in the ground feature sets ${\boldsymbol{\chi }}_f^i$ and ${\boldsymbol{\chi }}_c^i$ are deleted by the marked object ID in ${\boldsymbol{\Phi}}_s^i$ . The ground point in object point cloud ${{\boldsymbol{\gamma }}^{k,i}}$ is removed through the label in ${\boldsymbol{\Phi}}_g^i$ . This ambiguous information is not considered in the subsequent SLAM and MOT tasks.

Based on the ID pairs contained in ${{\textbf{V}}_b}$ and computed point roughness, we perform two feature extractions: The candidate object edge feature set ${\boldsymbol{\gamma }}_c^{k,i}$ is extracted from the point clouds belonging to objects, which is used in the mapping module to eliminate accumulated errors. Subsequently, the background edge feature set ${\boldsymbol{\psi }}_f^i$ , surface feature set ${\boldsymbol{\zeta }}_f^i$ , and the corresponding candidate sets ${\boldsymbol{\psi }}_c^i$ , ${\boldsymbol{\zeta }}_c^i$ are extracted from the non-semantic and non-ground point clouds, and they will participate in both localization and mapping modules.

Finally, we define the point-to-line error term ${\textbf{e}}_{{e_k}}$ and the point-to-surface error term ${\textbf{e}}_{{s_k}}$ in the lidar coordinate $ l$ , which are used for ego pose estimation in Section 3.3.1:

(4) \begin{align} {{\textbf{e}}_{{e_k}}} = \frac{{\left |{({}^l{\textbf{p}}_{i,k}^e -{}^l\bar{\textbf{p}}_{i - 1,m}^e) \times ({}^l{\textbf{p}}_{i,k}^e -{}^l\bar{\textbf{p}}_{i - 1,n}^e)} \right |}}{{\left |{{}^l\bar{\textbf{p}}_{i - 1,m}^e -{}^l\bar{\textbf{p}}_{i - 1,n}^e} \right |}} \end{align}

(5) \begin{align} {{\textbf{e}}_{{s_k}}} = \frac{{\left | \begin{array}{c} \left ({{}^l{\textbf{p}}_{i,k}^s -{}^l\bar{\textbf{p}}_{i - 1,m}^s} \right )\\[5pt] ({}^l\bar{\textbf{p}}_{i - 1,m}^s -{}^l\bar{\textbf{p}}_{i - 1,n}^s) \times ({}^l\bar{\textbf{p}}_{i - 1,m}^s -{}^l\bar{\textbf{p}}_{i - 1,o}^s) \end{array} \right |}}{{\left |{({}^l\bar{\textbf{p}}_{i - 1,m}^s -{}^l\bar{\textbf{p}}_{i - 1,n}^s) \times ({}^l\bar{\textbf{p}}_{i - 1,m}^s -{}^l\bar{\textbf{p}}_{i - 1,o}^s)} \right |}} \end{align}

where $ k$ represents the edge or surface point indices in the current frame. $ m,n,o$ represent the point indices of matching edge line or planar patch in the last frame. For the raw edge point ${}^l{\textbf{p}}_{i,k}^e$ in the set ${\boldsymbol{\psi }}_f^i$ , ${}^l\bar{\textbf{p}}_{i - 1,m}^e$ and ${}^l\bar{\textbf{p}}_{i - 1,n}^e$ are the matching points in the set ${\bar{\boldsymbol{\psi }}}_c^{i - 1}$ that make up the edge line in the last frame without motion distortion. Similarly, ${}^l{\textbf{p}}_{i,k}^s$ is the raw surface point in the set ${\boldsymbol{\zeta }}_f^i$ . ${}^l\bar{\textbf{p}}_{i - 1,m}^s$ , ${}^l\bar{\textbf{p}}_{i - 1,n}^s$ , and ${}^l\bar{\textbf{p}}_{i - 1,o}^s$ are the points that consist of the corresponding planar patch in the set ${\bar{\boldsymbol{\chi }}}_c^{i - 1}$ or ${\bar{\boldsymbol{\zeta }}}_c^{i - 1}$ .

3.3.1. Robust ego odometry

When the semantic object detection module makes the missing detection problem, the accuracy of the ego-motion estimation will be reduced by some unknown moving object points included in the ground or background feature sets. Accordingly, we solve this problem by using Algorithm 1 to perform a geometric consistency checking on current feature sets ( ${\boldsymbol{\chi }}_f^i$ , ${\boldsymbol{\psi }}_f^i$ , ${\boldsymbol{\zeta }}_f^i$ ). The checked feature sets are defined as ( ${\tilde{\boldsymbol{\chi }}}_f^i$ , ${\tilde{\boldsymbol{\psi }}}_f^i$ , ${\tilde{\boldsymbol{\zeta }}}_f^i$ ).

Algorithm 1.

Geometric consistency feature checking.

Considering the different degrees of freedom (DOF) constraints provided by the ground and background feature sets in estimating the robot pose and the computational efficiency problem, a two-step algorithm proposed in ref. [Reference Shan and Englot2] is used to estimate the ego-motion. First, we compute 3-DOF increment $ [{t_z},{\theta _{roll}},{\theta _{pitch}}]$ by the checked ground feature set ${\tilde{\boldsymbol{\chi }}}_f^i$ and the candidate feature set ${\bar{\boldsymbol{\chi }}}_c^{i - 1}$ that corrected the motion distortion. Then, another 3-DOF increment $ [{t_x},{t_y},{\theta _{yaw}}]$ is estimated by surface feature points in ${\tilde{\boldsymbol{\zeta }}}_f^i$ with smaller roughness than ${R_{a}}$ , edge feature set ${\tilde{\boldsymbol{\psi }}}_f^i$ , and corresponding candidate sets. Finally, we accumulate the 6-DOF increment ${}_l^{i - 1}{{\boldsymbol{\delta }}^i} = [{t_x},{t_y},{t_z},{\theta _{roll}},{\theta _{pitch}},{\theta _{yaw}}]$ to compute the robot pose ${\textbf{T}}_{ol}^i$ in the odometry coordinate $ o$ .

3.3.2. Semantic-geometric fusion MOT

To track semantic objects robustly and accurately, a least-squares estimator combining semantic box planes and geometric object points is designed for the MOT module. Before entering the increment estimation process, we use the Kuhn-Munkres [Reference Kuhn46] algorithm to obtain the object data association between frames.

Specifically, we need to construct the association matrix using some error functions that can evaluate the similarity of the object between frames: (1) The position error term ${{\textbf{e}}_p}$ is computed by the distance between the object’s predicted position which is inferred by the last motion model and object’s detection position. (2) The yaw angle increment of the object forms the direction error term ${{\textbf{e}}_d}$ . (3) The difference in the semantic 3D-BB size between the objects constructs the scale error term ${{\textbf{e}}_s}$ . Further details on error calculation can be found in ref. [47].

For objects that are tracked in the last frame but no matching relationship is currently found, we use the last calculated motion model to infer their positions in the subsequent two frames. If a successful match is still not found, they are removed.

Considering that the object point cloud ${{\boldsymbol{\gamma }}^{i}}$ (object ID $ k$ is omitted) obtained by direct measurement has motion distortion when the robot and object are in motion, we use the VG-ICP [Reference Koide, Yokozuka, Oishi and Banno48] algorithm and the relative motion model based on linear interpolation to construct the geometric cost function.

Assuming the $j$ -th raw object point in the lidar coordinate $ l$ is ${}^l{\textbf{p}}_{i,j}^g$ , and it has a relative timestamp ${}^r{t_j}$ . ${}_l^{i - 1}{{\textbf{H}}^i} \in{\rm{SE}}(3)$ is the transformation between interval $ \left [{{t^{i - 1}},{t^i}} \right ]$ . Then, the distortion-corrected transformation ${}_l^{i - 1}{\textbf{H}}_j^i$ for each raw object point can be calculated:

(6) \begin{align}{}_l^{i - 1}{\textbf{H}}_j^i ={}^r{t_j}{{\textbf{H}}^i} \end{align}

We use ${}_l^{i - 1}{{\textbf{R}}^i}$ and ${}_l^{i - 1}{{\textbf{t}}^i}$ to represent the rotational and translational components of the increment ${}_l^{i - 1}{{\textbf{H}}^i}$ . The geometric error term ${{\textbf{e}}_{g_j}}$ from the distribution to the multi-distribution can be defined as follows:

(7) \begin{align}{{\textbf{e}}_{{g_j}}} = \frac{{\sum{{}^l\bar{\textbf{p}}_{i - 1,m}^g} }}{{{N_j}}} -{}_l^{i - 1}{\textbf{H}}_j^i{}^l{\textbf{p}}_{i,j}^g \end{align}

For the matched voxel ${}^l\bar{\textbf{v}}_{i - 1}^g$ of the current object point ${}^l{\textbf{p}}_{i,j}^g$ at the end time of the last frame, it encloses ${N_j}$ object points ${}^l\bar{\textbf{p}}_{i - 1,m}^g$ that have corrected the motion distortion. The covariance matrix ${{\boldsymbol{\Omega}}_j}$ corresponding to the error term ${{\textbf{e}}_{g_j}}$ is as follows:

(8) \begin{align}{{\boldsymbol{\Omega}}_j} = \frac{{\sum{C^g_{i - 1,m}} }}{{{N_j}}} -{}_l^{i - 1}{\textbf{H}}_j^i C_{i,j}^g{}_l^{i - 1}{\textbf{H}}{_j^{i }}^T \end{align}

$ C_{i - 1,m}^g$ and $ C_{i,j}^g$ denote the covariance matrix. They describe the surrounding shape distribution of the point in the matched voxel and point ${}^l{\textbf{p}}_{i,j}^g$ .

Note that objects detected in a single frame always have less constrained information. Meanwhile, the inaccurate initial value of the estimator at the beginning of the optimization may also lead to poor matchings. Eq. (7) sometimes fails to constrain the 6-DOF motion estimation. Therefore, while using geometric constraints of the point cloud, this work also introduces semantic constraints on increment estimation to ensure the correct direction of convergence.

Specifically, we model a 3D-BB ${{\boldsymbol{\tau }}^i}$ as a cuboid containing six planes. Each plane is represented by the normal vector $\textbf{n}$ ( ${\left \|{\textbf{n}} \right \|_2} = 1$ ) and the distance $d$ from the origin to the plane, that is ${\boldsymbol{\pi }} = [{\textbf{n}},d]$ . Unlike the geometric point cloud error with iterative matching for estimation, the matching relationships for 3D-BB planes are directly available. Then, using the closest point ${\boldsymbol{\Pi}} ={\textbf{n}}d$ proposed in ref. [Reference Geneva, Eckenhoff, Yang and Huang49], we can construct the plane-to-plane semantic error term ${{\textbf{e}}_{{b_r}}}$ :

(9) \begin{align}{{\textbf{e}}_{{b_r}}} ={}^l{\boldsymbol{\Pi}}_{i - 1,r}^b -{}^l{\boldsymbol{\hat{\Pi }}}_{i - 1,r}^b \end{align}

${}^l{\boldsymbol{\Pi}}_{i - 1,r}^b$ and ${}^l{\boldsymbol{\hat{\Pi }}}_{i - 1,r}^b$ are the r-th measured and estimated planes on 3D-BB in frame $ i$ in the lidar coordinate $ l$ . As described in ref. [Reference Hartley and Zisserman50], the motion transformation of a plane is defined as:

(10) \begin{align} \left [ \begin{array}{l}{}^l{\textbf{n}}_{i - 1,r}^b\\[5pt] {}^ld_{i - 1,r}^b \end{array} \right ] = \left [ \begin{array}{l@{\quad}l}{}_l^{i - 1}{{\textbf{R}}^i} & {\textbf{0}}\\[5pt] -{({}_l^{i - 1}{{\textbf{R}}^{i T}}{}_l^{i - 1}{{\textbf{t}}^i})^T} & 1 \end{array} \right ]\left [ \begin{array}{l}{}^l{\textbf{n}}_{i,r}^b\\[5pt] {}^ld_{i,r}^b \end{array} \right ] \end{align}

Taking Eq. (10) into ${}^l{\boldsymbol{\hat{\Pi }}}_{i - 1,r}^b$ , we can further obtain:

(11) \begin{align}{}^l{\boldsymbol{\hat{\Pi }}}_{i - 1,r}^b = \left ({{}_l^{i - 1}{\textbf{R}}_j^i{}^l{\textbf{n}}_{i,r}^b } \right )\left ({ -{}_l^{i - 1}{\textbf{t}}{{_j^i}^T}{}_l^{i - 1}{\textbf{R}}_j^i{}^l{\textbf{n}}_{i,r}^b + d} \right ) \end{align}

When using the least-squares method to estimate ${}_l^{i - 1}{{\textbf{H}}^i}$ , it is necessary to obtain the derivative of ${{\textbf{e}}_{{b_r}}}$ with respect to ${}_l^{i - 1}{{\textbf{H}}^i}$ :

(12) \begin{align} \frac{{\partial{{\textbf{e}}_{{b_r}}}}}{{\partial{\delta _{{}_l^{i - 1}{\boldsymbol{\theta }}_j^i}}}}= -{\left [{{\textbf{F}}{}_l^{i - 1}{\textbf{t}}{{_j^i}^T}{\textbf{F}}} \right ]_ \times }-{\textbf{F}{}_l^{i - 1}{\textbf{t}}{{_j^i}^T }{\left [{\textbf{F}} \right ]_ \times } } +{\left [{{\textbf{F}}d} \right ]_ \times } \end{align}

(13) \begin{align} \frac{{\partial{{\textbf{e}}_{{b_r}}}}}{{\partial{\delta _{{}_l^{i - 1}{\textbf{t}}_j^i}}}} ={\textbf{F}}{{\textbf{F}}^T} \end{align}

where ${\textbf{F}} ={}_l^{i - 1}{\textbf{R}}_j^i{}^l{\textbf{n}}_{i,r}^b$ , $ \frac{{\partial{{\textbf{e}}_{{b_r}}}}}{{\partial{\delta _{{}_l^{i - 1}{\boldsymbol{\theta }}_j^i}}}}$ , and $ \frac{{\partial{{\textbf{e}}_{{b_r}}}}}{{\partial{\delta _{{}_l^{i - 1}{\textbf{t}}_j^i}}}}$ represent the Jacobians of the error term ${{\textbf{e}}_{{b_r}}}$ w.r.t. the rotation and translation components. The detailed derivation of Eqs. (12) and (13) can be found in Appendix.

Finally, Eq. (14) is the comprehensive cost function for the estimation of ${}_l^{i - 1}{{\textbf{H}}^i}$ considering the semantic confidence $\mu ^i$ :

(14) \begin{align} \mathop{\arg \min }\limits _{{}_l^{i - 1}{{\textbf{H}}^i}} \frac{{2 -{\mu ^i}}}{{2{N_g}}}\sum \limits _{j = 1}^{{N_g}}{\left ({{N_j}{\textbf{e}}_{g_j}^T{\boldsymbol{\Omega}}_j^{ - 1}{{\textbf{e}}_{g_j}}} \right )} + \frac{{{\mu ^i}}}{{2{N_b}}}\sum \limits _{r = 1}^{{N_b}}{\left ({{\textbf{e}}_{b_r}^T{{\textbf{e}}_{b_r}}} \right )} \end{align}

where ${N_g}$ is the point number in the object point cloud. ${N_b}$ is the plane number on 3D-BB.

In this paper, we use the Levenberg-Marquardt [Reference Madsen, Nielsen and Tingleff51] algorithm to solve Eq. (14) and the estimated relative increment to get the motion-distorted object point cloud ${{\bar{\boldsymbol{\gamma }}}^i}$ and object edge feature set ${\bar{\boldsymbol{\gamma }}}_c^i$ . ${{\bar{\boldsymbol{\gamma }}}^i}$ and ${\bar{\boldsymbol{\gamma }}}_c^i$ will be used for object tracking in the next frame and accumulated error elimination in the mapping module.

3.3.3. Object odometry

The aim of object odometry is to get each object pose in odometry coordinate $o$ . First, using the results calculated above, we can compute the object motion increment ${}_l^i{{\textbf{X}}^{i - 1}}$ in the lidar coordinate $l$ :

(15) \begin{align}{}_l^i{{\textbf{X}}^{i - 1}} ={}_l^{i - 1}{{\boldsymbol{\delta }}^i}{}_l^i{{\textbf{H}}^{i - 1}} \end{align}

Suppose the transformation from object to robot in the frame $i-1$ is ${}^{i - 1}{{\textbf{L}}_{lt}}$ . The absolute increment of object motion in the body-fixed coordinate [Reference Henein, Zhang, Mahony and Ila52] can be expressed as:

(16) \begin{align}{}_t^{i - 1}{{\textbf{X}}^i} ={}^{i - 1}{\textbf{L}}_{lt}^{ - 1}{}_l^i{{\textbf{X}}^{i - 1}}{}^{i - 1}{{\textbf{L}}_{lt}} \end{align}

Finally, we accumulate the increment ${}_t^{i - 1}{{\textbf{X}}^i}$ to obtain the object pose ${\textbf{T}}_{ot}^i$ in the odometry coordinate $o$ . For an object detected at the first frame, we compute its initial pose ${{\textbf{T}}_{ot}}$ in the odometry coordinate $o$ based on the current robot pose ${{\textbf{T}}_{ol}}$ and the transformation ${{\textbf{L}}_{lt}}$ inferred from the semantic detection module.

3.4.1. Dynamic object detection

As shown in Fig. 5, the ATTL maintained in the mapping module is used to analyze the motion state of each instance object. First, each object in the current frame is matched with the unique ID generated by the SGF-MOT module and maintained in ATTL. Then, we use the current error correction matrix ${\textbf{T}}_{mo}^{i - 1}$ to predict the object pose ${\textbf{T}}_{mt}^{i\;\;'}$ in the map coordinate $m$ :

(17) \begin{align}{\textbf{T}}_{mt}^{i\;\;'} ={\textbf{T}}_{mo}^{i - 1}{\textbf{T}}_{ot}^i \end{align}

By calculating the motion increment of the predicted object pose and the static oldest object pose in ATTL, we can determine whether each object is in motion. Note that the detected object in the first frame is set to be static by default.

3.4.2. Static map creation

By matching the static object feature set ${\bar{\boldsymbol{\gamma }}}_c^i$ in the current frame with the accumulated object map, ground feature set ${\bar{\boldsymbol{\chi }}}_c^i$ , background edge feature set ${\bar{\boldsymbol{\psi }}}_c^i$ , and surface feature set ${\bar{\boldsymbol{\zeta }}}_c^i$ with their accumulated feature map, it is possible to obtain the corrected robot pose ${\textbf{T}}_{ml}^i$ in the map coordinate $m$ . Then, we align the non-semantic feature sets to the map coordinate $m$ to create a long-term static map. Readers can refer to ref. [Reference Zhang and Singh1] for more details about feature mapping and optimization.

Using the ego pose ${\textbf{T}}_{ol}^i$ in the odometry coordinate $o$ and the corrected pose ${\textbf{T}}_{ml}^i$ in the map coordinate $m$ , the updated accumulated error correction matrix ${\textbf{T}}_{mo}^i$ can be computed.

3.4.3. Tracking list maintain

The ATTL can be updated based on the motion state of each object. If the object is moving, the object map and tracking trajectory should be cleared first. Otherwise, they will remain unchanged. Then, we use ${\textbf{T}}_{mo}^i$ and ${\textbf{T}}_{ml}^i$ to convert the object odometry pose ${\textbf{T}}_{ot}^i$ and extracted object feature set ${\bar{\boldsymbol{\gamma }}}_c^{k,i}$ into the map coordinate $m$ , which will participate in the following mapping steps.