RGB-D SLAM in Indoor Planar Environments with Multiple Large Dynamic Objects

Dynamic SLAM methods can be categorised into outlier-based, semantic-based, multimotion and proprioception-aided methods.

Outlier-based methods assume that the static background occupies the major component in the camera view and dynamic objects can be robustly removed during camera tracking. Joint-VO-SF (JF) [6] over-segments images into clusters and classifies each cluster as either static or dynamic by comparing the depth and intensity residuals. StaticFusion (SF) [7] introduces a continuous score to represent the probability that a cluster is static. The scores are used to segment the static background for camera localisation and mapping, while the remaining dynamic parts are discarded as outliers. Co-Fusion (CF) [5] and MultiMotionFusion [8] can further model the outliers as new objects and track them independently.

Semantic-based methods directly detect dynamic objects from the semantic segmentation. Based on Mask R-CNN [9], which provides pixel-wise object segmentation, EM-Fusion [10] integrates object tracking and SLAM into a single expectation maximisation (EM) framework. ClusterVO [3] can track camera ego-motion and multiple rigidly moving clusters simultaneously by combining semantic bounding boxes and ORB features [11]. DynaSLAM II [12] further integrates the multi-object tracking (MOT) and SLAM into a tightly-coupled formulation to improve its performance on both problems. However, these methods require that the object detector is pre-trained on a dataset which includes these objects or that the object model is provided in advance.

Multimotion methods explicitly model the dynamic component as third-party rigid motions and segment dynamic objects by their different motions against the camera motion. Multimotion visual odometry (MVO) [4] is an online multimotion segmentation and tracking method based on sparse keypoints. It iteratively samples all keypoints using RANSAC to generate motion hypotheses and automatically merge them when the merging can decrease the total energy. In addition to RANSAC, motion hypotheses can be generated from the increased N-points using a grid-based scene flow [13]. Instead of merging motion hypotheses, DymSLAM [14] computes a residual matrix from hypothetical motion models and directly estimates the number of dynamic objects from the residual matrix. The dense segmentation of multiple dynamic objects based on super-pixels [15] is acquired after the number of objects is specified. However, all these methods assume that the largest body of motion in the camera view is the static background and they have not demonstrated their effectiveness when the major proportion of the camera view is occluded by dynamic objects.

Robot proprioception, such as IMU and wheel odometry, can be fused with visual sensors to improve the accuracy and robustness of localisation in dynamic environments [16, 17]. Kim et al. [16] uses the camera motion prior from an IMU to compensate for the camera motion and select static keypoints based on motion vectors. RigidFusion (RF) [17] uses the camera motion prior from wheel odometry and additional object motion priors to enable SLAM with single dynamic object reconstruction when the major part of the camera view is occluded. However, these methods are unable to track multiple dynamic objects independently.

Planar features have been widely used in indoor dynamic SLAM methods. Infinite planes can be used as landmarks in the pose graph SLAM problem [18]. Based on keyframe management, a global dense planar map can be reconstructed using only a single CPU [19]. Planes can also be combined with keypoints and lines [2, 1] for more robust camera tracking. In structured environments, planes have been demonstrated to significantly reduce accumulated rotational drift under Manhattan world (MW) assumption [1]. All these methods assume static environments, because planes in the indoor environment, like walls, are often static. However, this assumption is violated when planar objects, such as boxes, are transported or manipulated by humans or robots.

The overview of our pipeline is illustrated in Figure 2. Our method takes RGB-D image pairs from two consecutive frames. At the $t$-th frame, we have a depth image $D_{t}$ and an intensity image $I_{t}$ computed from the colour image.

A plane is represented in the Hessian form $\Pi=(\mathbf{n}^{T},d)^{T}$, where $\mathbf{n}=(n_{x},n_{y},n_{z})$ is the normal of the plane and $d$ is the perpendicular distance between the plane and camera origin. For each image frame, we extract planes directly from the depth map using PEAC [20], which can provide the number of planes $P$, the pixel-wise segmentation of planes $\mathbf{V}^{p}:\{V^{p}_{i}|i\in[1,P]\}$ and their corresponding plane parameters. After plane extraction, the remaining non-planar areas are over-segmented into $S$ super-pixels: $\mathbf{V}^{np}:\{V^{np}_{i}|i\in[1,S]\}$.

For the $i$-th plane, we extract a set of keypoints $\mathbf{K}_{i}$ using ORB features [21]. We then conduct multimotion segmentation on planar areas $\mathbf{V}^{p}$ and cluster the planes into $M$ planar rigid bodies of different motions. For simplicity, we name the camera motion relative to objects’ egocentric frames as object egocentric motion [4] and denote them as ${}^{ego}\mathbf{T}=\{\tilde{\xi}_{m}\in\mathfrak{se}(3)|m\in[1,M]\}$. Since the static background may not be the largest rigid body in the camera view, we use the camera motion prior $\tilde{\xi}_{c}$ with potential drift to simultaneously classify all planes and super-pixels into either static or dynamic, and estimate the camera ego-motion.

We use the score $\gamma_{i}\in[0,1]$ to represent the probability that a plane or super-pixel is static. The scores $\bm{\gamma}:\{\gamma_{i}|i\in[1,P+S]\}$ are assigned to each plane and super-pixel, where $\{\gamma_{i}|i\in[1,P]\}$ and $\{\gamma_{i}|i\in[P+1,P+S]\}$ represent the scores of planes and super-pixels respectively. At time $t$, the pixel-wise static dynamic segmentation $\Gamma_{t}\in\mathbb{R}^{w\times h}$ can be estimated from $\bm{\gamma}$. The static parts of intensity and depth images are used to estimate the camera motion $\xi_{c}\in\mathfrak{se}(3)$. The dynamic planar rigid bodies are used to track dynamic objects. The non-planar dynamic super-pixels, such as humans, are removed as outliers.

The world-, camera-, and the $m$-th object-frames are $W$, $C$ and $O^{m}$ respectively. The camera motion $T(\xi_{c})\coloneqq exp(\xi_{c})$ is $T_{C_{t-1}C_{t}}=T_{WC_{t-1}}^{-1}T_{WC_{t}}$, which transforms homogeneous coordinates of a point in the current camera frame $C_{t}$ to the previous frame $C_{t-1}$. The function $exp(\xi)$ is the matrix exponential map for Lie group $SE(3)$. The $m$-th object egocentric transformations is the camera motion relative to this object [4]:

To extract planes, we transform the depth map to a point cloud and cluster connected groups of points with close normal directions using the method from [20]. To match plane $i$ in the current frame with one in the previous frame, we first estimate the angle and point-to-plane distance between plane $i$ and all planes in the previous frame. A plane is chosen as a candidate if the angle and distance are below 10 degrees and 0.1 m respectively, which is the same as [2, 1]. However, rather than choosing the plane with minimal distance [2, 1], we further consider overlap proportion between two planes using the Jaccard index, $J(V_{1},V_{2})=\frac{|V_{1}\cap V_{2}|}{|V_{1}\cup V_{2}|}$, where $|V_{1}|$ is the number of pixels in planar segment $1$. We choose the candidate plane that has the maximal Jaccard index as the matched plane for plane $i$ and denote it as plane $i^{\prime}$.

To estimate object egocentric motion $T_{i}={{}^{i}}T^{-1}_{C_{t-1}C_{t}}$ of plane $i$, we extract and match ORB keypoints from plane $i$ and $i^{\prime}$. The error function is defined as:

where $\chi_{ii^{\prime}}$ is the set of keypoint matches between planes $i$ and $i^{\prime}$. $\mathbf{x}^{k}_{i}$ and $\mathbf{x}^{k}_{i^{\prime}}$ are homogeneous coordinates $\left[x,y,z,1\right]$ of the two matched keypoints. $\rho\left(\cdot\right)$ is the robust Huber error function [21]. $\lambda_{h}$ is the parameter to weight the error between the Hessian form of planes. $q(\Pi)=\left[\arctan(\frac{n_{x}}{n_{z}}),\arctan(\frac{n_{y}}{n_{z}}),d\right]$ avoids over-parametrisation of the Hessian form [1].

To cluster planes with similar motions, we introduce a score $b_{ij}\in[0,1]$ for each pair of neighbouring planes $i$ and $j$ in the current frame. $b_{ij}$ represents the probability that the motion of planes $i$ and $j$ can be modelled by the same rigid transformation. We further introduce a new formulation based on planes to jointly estimate motion of planes and merge planes into rigid bodies:

${}^{ego}\mathbf{T}$ is the set of egocentric transformations $\{T_{1},\cdots,T_{P}\}$ for all planes in the current frame. $\mathbf{b}=\{b_{ij}|(i,j)\in E_{p}\}$. $E_{p}$ is the connectivity graph of planes in the current frame and $(i,j)\in E_{p}$ means that planes $i$ and $j$ are connected in space. The first term $e_{i}(T_{i})$ is introduced in Equation 2. In the second term, we propose $f(T_{i},T_{j})=\left[e_{i}(T_{j})+e_{j}(T_{i})\right]-\left[e_{i}(T_{i})+e_{j}(T_{j})\right]$ to quantify the error between two planes with egocentric motion $T_{i}$ and $T_{j}$ respectively. The last term penalises the model complexity by maximising the sum of probabilities that neighbouring planes have similar motions.

The novelty of the formulation is that we treat each individual plane as a motion hypothesis and estimate the likelihood $\mathbf{b}$ of any two neighbouring hypotheses having the same motion. This is in contrast to MVO [4], which discretely decides whether two motion hypotheses are merged or not.

To minimise Equation 3, ${}^{ego}\mathbf{T}$ and $\mathbf{b}$ are decoupled. We firstly initialise all egocentric motions $T_{i}$ to identity and all scores $b_{ij}$ to 0. Then, at each iteration, we fix $\mathbf{b}$ and find optimal ${}^{ego}\mathbf{T}$ by optimising each transformation independently. $\mathbf{b}$ is analytically solved subsequently by fixing the optimised transformations. After minimisation, we set a threshold $\hat{b}=0.9$ and merge planes $i$ and $j$ if $b_{ij}>\hat{b}$. We therefore acquire $M$ planar rigid bodies and use RANSAC to estimate their prior egocentric motions $\{\tilde{\xi}_{1},\cdots,\tilde{\xi}_{M}\}$ respectively. However, since the dynamic objects can occupy the major part of the images, we still need to decide which planar rigid body belongs to the static background.

We jointly track the camera motion and segment the static background based on a hierarchical representation of planes and non-planar super-pixels. This representation is more efficient in planar environments than uniformly sampled clusters used in previous work [7, 17]. In addition, compared to RigidFusion [17], our method only requires the camera motion prior. The dynamic planar objects are detected by their different rigid motions compared to the camera motion while dynamic non-planar areas are removed by their high residuals. To achieve it, we propose to minimise a combined formulation that consists of three energy terms:

where $\bm{\gamma}$ is the full set of probabilities that each plane or super-pixel is static. $\xi_{c}\in\mathfrak{se}(3)$ is the camera ego-motion in the world frame. Importantly, planes that belong to the same planar rigid body are assigned with independent scores $\gamma$. The first term $R(\xi_{c},\bm{\gamma})$ aligns the static rigid body using weighted intensity and depth residuals. The second term $G(\xi_{c},\bm{\gamma})$ segments dynamic objects by either different motions or high residuals and maintains segmentation smoothness. The last regularisation term $H(\xi_{c})$ adds a soft constraint on the camera motion.

In the current frame $t$, after the minimisation of Equation 4, we acquire the optimised camera motion $\hat{\xi}_{c}$ and the static parts of intensity and depth images $(I^{s}_{t},D^{s}_{t})$. These images are used to reconstruct the static background and refine the camera pose $\xi_{c}$ using frame-to-model alignment. Concretely, we render an image pair $(I^{r}_{t-1},D^{r}_{t-1})$ from the current static background model at the previous camera pose. The rendered image pair $(I^{r}_{t-1},D^{r}_{t-1})$ is directly aligned with $(I^{s}_{t},D^{s}_{t})$ by minimising

The first term $R(\xi_{c},\bm{\gamma}=\mathbf{1})$ is the same as Equation 8 but the $\bm{\gamma}$ is fixed to $\mathbf{1}$ because the input should only contain the static background. We append $H(\xi_{c})$ in Equation 14 as a soft-constraint for the frame-to-model alignment and $\alpha_{s}$ is estimated from pixel-wise dynamic segmentation $\Gamma_{t}$. Since we have already solved Equation 4, we directly start from the finest level of the image pyramid and initialise the solver with the camera pose $\hat{\xi}_{c}$ for the solver of Equation 15. The refined camera pose is used to fuse the static images $(I^{s}_{t},D^{s}_{t})$ with the surfel-based 3D model as described in SF [7].

After removing the static planes, we further track dynamic planar rigid bodies independently. This is different to our previous work RigidFusion [17] which models the whole dynamic component with a single rigid transformation. For each dynamic planar rigid body $m$, we match it to the previous dynamic rigid bodies using the plane association and estimate the egocentric motion. If all the currently associated planes are static in the previous frame, we detect the dynamic planar rigid body $m$ as a new object and the initial pose of the object relative to the camera frame is denoted as $T_{init}$. If the initial time of frame for an object is $t_{0}$, the object pose in the object’s initial frame can be acquired by [14]:

The sequences for evaluation are collected with an Azure Kinect DK RGB-D camera which is mounted on an omnidirectional robot (Figure 3(a)). The camera produces RGB-D image pairs with a resolution of 1280 x 720 at 30 Hz. The images are down-scaled and cropped to 640 x 480 (VGA) to accelerate the speed of pre-processing (Figure 2), such as super-pixel and plane extraction. In the solver of Equation 4 and (15), we further down-scale images to 320 x 240 (QVGA).

The dynamic objects are created from stacked boxes and are either moved by humans or via a remotely controlled KUKA youBot (Figure 3). The ground truth trajectories of the camera and objects are collected using a Vicon system by attaching Vicon markers on the camera and dynamic objects. The camera motion prior is acquired by adding synthetic drift on camera ground truth trajectories with a magnitude of around 7 cm/s (trans.) and 0.4 rad/s (rot.).

For quantitative evaluation, we estimate the absolute trajectory error (ATE) and the relative pose error (RPE) [22] against the ground truth. The proposed method is compared with PlanarSLAM (PS) [1], EM-Fusion (EMF) [10], Joint-VO-SF (JF) [6], StaticFusion (SF) [7], Co-Fusion (CF) [5] and RigidFusion (RF) [17]. We additionally provide the camera motion prior with drift to CF as the variant CF^∗. The original RF uses motion priors for both camera and object. Here we only provide RF with the camera motion prior and denote it as RF^∗, while our method with the camera motion prior is denoted as ours^∗.

We collect eight sequences with various camera and object movements in different planar environments. For example, in the seq1, a human moves the taller box to clear way for both the robot and the other object so that the potential collision can be avoided, while in the seq5, the robot tries to overtake two dynamic objects ahead (Figure 4). All trajectories are designed such that the two dynamic objects and a human can be visible in the image at the same time and frequently occupy the major proportion of the camera view. We also run experiments on sequences sitting_xyz and walking_xyz from TUM RGB-D dataset [22] which includes a large proportion of non-planar areas and denote them as seq9 and seq10 respectively.

We estimate the ATE root-mean-square error (RMSE) and RPE RMSE between the estimated camera trajectories and ground truth (Table I). In planar dynamic environments (seq. 1-8), the evaluation demonstrates that our method outperforms all other state-of-the-art methods (Figure 5). With the help of the camera motion prior, our method achieves the best performance and corrects the large drift of the camera motion prior. Even without the camera motion prior, we still achieve better performance than the baseline of JF, SF and CF. PS is unable to estimate the correct camera pose because there are dynamic planes in the environment while PS assumes all planes are static. Our method also outperforms EMF because the semantic segmentation method [9] can only detect and segment certain categories of dynamic objects, like humans.

In non-planar dynamic environments (seq. 9-10), EMF outperforms all other methods because the dynamic humans can be directly segmented by Mask R-CNN [9]. However, even without relying on semantic segmentation, our method has close performance compared to StaticFusion. This is because our method can still detect dynamic super-pixels by their high residuals under the camera motion.

For planar environments, we visualise the segmentation results of our method and compare them with SF, RF^∗ and CF (Figure 6). SF is unable to detect all dynamic objects because they as a whole occlude a large proportion of the camera view, while RF^∗ tends to classify parts of the static background as dynamic. Both CF and our method can further distinguish between different dynamic objects. However, the segmentation of CF is not complete and CF tends to have a delay when detecting a new object. We use two different colours (green and purple) to represent that our method treats the taller object as a new one after it passes behind the front object. In non-planar environments, our method can still provide correct binary segmentation of the static and dynamic objects (Figure 7). However, we are unable to segment and track different non-planar dynamic objects independently.

We qualitatively evaluate the reconstruction result of seq3 (Figure 8). Since we have no ground truth segmentation, we re-collect a new sequence with the same camera trajectory but no dynamic objects to recover the true background. As shown in the results, RF^∗ maps the dynamic objects into the static background model. CF has mapped the same static object twice, which is caused by wrong camera pose estimation. Only our proposed method can remove all dynamic objects and correctly reconstruct the background.

For both objects, we compute the ATE RMSE between the estimated and ground-truth trajectories when they are in the camera view (Table II). Since the object can move out of or move into the camera view several times, one object trajectory can be divided into multiple parts. For each object, we, therefore, use the maximal ATE RMSE among the estimated trajectories of different parts for the final result. Our method can provide more accurate and complete object trajectories than CF, but loses track of a dynamic object when the object stops moving or is occluded by other objects (Figure 9).

We increase the drift magnitude of the camera motion prior to test our methods’ robustness to different levels of drift. By comparing the RPE RMSE of the camera motion prior and estimated trajectories, we find that our method can outperform Co-Fusion baseline with drift up to 24 cm/s (Figure 10). Even when the motion prior has a drift of nearly 30 cm/s, we can still reduce the drift to around 12 cm/s. Compared to Co-Fusion with camera motion prior, our method is always better using the motion prior with the same magnitude of drift.