Multi-object Monocular SLAM for Dynamic Environments

We utilize the known camera intrinsic parameters $K$, ground plane normal $n$, 2D bounding boxes[27] and camera height $h$ in metric unit to estimate the depth of any point on the ground plane²²2Flat-earth assumption: For the scope of this paper, we assume that the autonomous vehicle is operating within a bounded geographic area of the size of a typical city, and that all roads in consideration are somewhat planar, i.e., no steep/graded roads on mountains.. Given the 2D homogeneous coordinates to the point in image space to be $x_{t}$, we estimate the 3D depth to them using the following method as shown in Song et al.[28].

The initializations to our odometry pipeline (cf. Fig. 2) come from the ORB trajectory[26] in a static-global frame but in an ambiguous ORB scale as opposed to our requirement of metric scale.

We scale the ego-motion from the ORB-SLAM2[26] input by minimizing the re-projection error of the ground point correspondences between each pair of consecutive frames. Given frames $t-1$ and $t$, we have odometry initializations in 3D in ORB scale from ORB-SLAM2 as $T_{t-1}$ and $T_{t}$ respectively. We obtain the relative odometry between the two frames as follows³³3We use $\times$ to denote matrix multiplication for the scope of this paper.:

We now obtain ORB features to match point correspondences between the two frames $t-1$ and $t$ and use state-of-the-art semantic segmentation network[20] to retrieve points $x_{t-1}$ and $x_{t}$ that lie on the ground plane. We obtain the corresponding points $X_{t-1}$ and $X_{t}$ in 3D, given the camera height, via Eqn. 1 as explained in Sec. IV-A. To reduce the noise incorporated by the above method, we only consider points within a threshold in depth of T = 12 metres from the camera. Further, we obtain the required scale-factor $\alpha$ that scales odometry from Eqn. 2 via a minimization problem as shown in Eqn. 4, the objective function to which is elaborated as Eqn. 3:

Here, $R_{t}^{t-1}$ and $\mathbf{tr}_{t}^{t-1}$ represent the relative rotation matrix and translation vector respectively. After solving the above minimization problem, we finalize our scale factor $\alpha$ as the mean of solutions obtained from the following:

We obtain object localizations by using a method inspired from Murthy et al.[29]. Our object representation follows from [30, 29, 31], based on a shape prior consisting of $k$ ordered keypoints which represent specific visually distinguishable features on objects which primarily consist of cars and minivans. For this work, we stick with the 36 keypoints structure from Ansari et al.[31]. We obtain the keypoint localizations in 2D image space using a CNN based on stacked hourglass architecture[32] and use the same model trained on over 2.4 million rendered images for Ansari et al.[31].

Borrowing the notations from Murthy et al.[29], we begin with a basis shape prior for the object used as a mean shape $\overline{X}\in\mathbb{R}^{3k}$. Let $B$ basis vectors be $V\in\mathbb{R}^{3k\times B}$ and the corresponding deformation coefficients be $\Lambda\in\mathbb{R}^{B}$. Assuming that a particular object instance has a rotation of $R\in SO(3)$ and translation of $\mathbf{tr}\in\mathbb{R}^{3}$ with respect to the camera, its instance $X\in\mathbb{R}^{3k}$ in the scene can be shown mathematically using the following shape prior model:

Here, $\hat{R}=diag([R,R,R,...,R])\in\mathbb{R}^{3k\times 3k}$ and $\hat{\mathbf{tr}}=(\mathbf{tr}^{T},\mathbf{tr}^{T},\mathbf{tr}^{T},...,\mathbf{tr}^{T})^{T}\in\mathbb{R}^{3k}$. Also, $\overline{X}=(\overline{X}_{1}^{T},\overline{X}_{2}^{T},\overline{X}_{3}^{T},...,\overline{X}_{K}^{T})\in\mathbb{R}^{3k}$ represents the basis shape prior and the resultant shape for the object instance is $X=(X_{1}^{T},X_{2}^{T},X_{3}^{T},...,X_{k}^{T})\in\mathbb{R}^{3k}$ where each $X_{i}$ represents one of the k = 36 keypoints in 3D coordinate system from camera’s perspective. Now, Let the ordered collection of keypoint localizations in 2D image space be $\hat{x}=(\hat{x}_{1}^{T},\hat{x}_{2}^{T},\hat{x}_{3}^{T},...,\hat{x}_{k}^{T},)\in\mathbb{R}^{2k}$. Given that $\pi_{k}$ represents the function to project 3D coordinates onto 2D image space using the camera intrinsics $\mu=(f_{x},f_{y},c_{x},c_{y})$, fairly accurate estimates for the pose parameters (R, tr) and the shape parameter ($\Lambda$) for the object instance can be obtained using the following objective function:

Minimizing the objective function (cf. Eqn 7) separately for pose parameters (R, tr) and shape parameters ($\Lambda$) provides us with an optimal fitting of the shape prior over the dynamic object. We obtain the object orientation as $R$ after pose parameter adjustments. The object’s 3D coordinates from the camera $\mathbf{tr}^{\prime}$ is obtained from the mean of wheel centres.

Fig. 3 illustrates a simple pose-graph structure containing two nodes A and B and a binary-edge between them. Using the terminologies from g2o[33], any node A in the pose graph is characterized by a pose $T_{A}^{W}\in SE(3)$ called the estimate which defines its pose with respect to the static-global frame of reference W. Meanwhile, a binary-edge from A to B is represented with a relative pose $T_{B}^{A}\in SE(3)$ called the measurement which defines the pose of node B from node A’s perspective. Mathematically, the constraint introduced by the binary-edge is given as:

Assuming relative correctness between each term in Eqn. 9, it results in an identity matrix $I_{4}\in SE(3)$ irrepective of the order of transformation. Thus, Eqn. 9 reduces to:

Clearly, the order in which the transformations are applied do not change the consistency of the respective cycle in the pose-graph. Thus, Eqn. 10 can also be written as:

Fig. 4 illustrates the pose graph structure between every consecutive set of frames $t-1$ and $t$ containing four nodes and four edges between them. We obtain the estimates for camera nodes $c(t-1)$ and $c(t)$ (i.e., $T_{c(t-1)}^{W}$ and $T_{c(t)}^{W}$) and measurement for the camera-camera edge (i.e. $T_{c(t)}^{c(t-1)}$) from our odometry estimation (cf. Sec. IV-B). We use this odometry to register dynamic object localizations from pose-shape adjustment pipeline as explained in Sec. IV-C to provide for the estimates $T_{v(t-1)}^{W}$ and $T_{v(t)}^{W}$ to vehicle nodes $v(t-1)$ and $v(t)$. We obtain measurement for the camera-vehicle edge (i.e., $T_{c(t-1)}^{v(t-1)}$, $T_{c(t)}^{v(t)}$) from shape and pose adjustment (cf. Sec. IV-C). Moreover, we use depth estimation from ground plane using Song et al.[28] as explained in Sec. IV-A as a source of vehicle localization that is unique from the localizations obtained from Sec. IV-C. This registered with our odometry estimations provides for our vehicle-vehicle edge measurement i.e., $T_{v(t)}^{v(t-1)}$. Now, from Eqn. 10, the cost function for the above binary-edges, $\Upsilon_{cc}$, $\Upsilon_{cv(t-1)}$, $\Upsilon_{cv(t)}$ and $\Upsilon_{vv}$, can be defined mathematically as:

Cumulatively, the above cost functions for a single loop illustrated in Fig. 4 can be represented as:

On substituting Eqn. 12 in Eqn. 13, and on further simplification, we obtain the resultant function for cumulative cost which clearly defines the cyclic consistency within the loop defined by the four binary edges:

In addition to the relative pose between the participating graph nodes, the parameterization for each edge also includes a positive semi-definite inverse covariance matrix or the information matrix $\Omega_{E}\in\mathbb{R}^{N\times N}$ where E represents an edge in the pose graph and N represents the dimension of the Lie group in which the poses are defined. In this work, all poses and transformations are defined in SE(3), hence we can take N = 6 for the information matrix $\Omega_{E}$ corresponding to each edge E in the whole pose graph. We utilize this as a confidence parameterization for the sources of input-data into the pose-graph. To make the most out of this, we scale the information matrix for an edge $E$ by a scale factor $\lambda\in\mathbb{R}$ to get the effective information matrix $\overline{\Omega}_{E}$ that is finally sent as a parameter:

We categorize all the edges in our pose-graph formulation into three types namely camera-camera, camera-vehicle, and vehicle-vehicle edges. Each type of edges corresponds to a unique source of data to provide for the corresponding constraint. This formulation coupled with the corresponding confidence parameter $\lambda$, enables us to scale the effects of the respective categories of edges appropriately. Given that odometry estimates are fairly reliable, we assign a relatively high constant scaling to its information matrix for our experiments on all sequences.

Given that we obtain dynamic vehicle localizations in camera frame from two different sources as explained in Sec. V-A, we make intelligent use of the confidence parameter $\lambda$, to scale the information matrix corresponding to the camera-vehicle and the vehicle-vehicle edges in our pose graph. It has been observed over a large number of vehicles that localizations obtained from Sec. IV-C performs better than the those obtained from Sec. IV-A for vehicles closer to the camera (up to about 45 metres). This can be attributed to the keypoint localizations being inaccurate for far away objects. However, estimates from Sec. IV-A are more accurate at depths far away from the camera (over 45 metres). Factors like visible features on vehicles do not affect these estimates.

We test our procedure over a wide range of KITTI-Tracking training sequences[1], spanning over rural and urban scenarios with various number of dynamic objects in the scene. We perform localizations on objects primarily consisting of cars and mini-vans. Our localization pipeline provides accurate results over objects irrespective of the direction of motion and maneuvers undertaken by both the ego-car and the vehicles in the scene. The labels provided in the dataset are used as ground truth for getting the depth to the vehicle’s center from the camera. The corresponding ground truth for odometry comes from GPS/IMU data, which is compiled using the OXTS data provided for all the KITTI-Tracking training sequences.

While Fig. 5 illustrates how our trajectories perform with respect to the ground truth, Table III reaffirms how our pose-graph formulation successfully redistributes errors about constraints with high confidence parameters. Table II reports our pipeline’s performance with respect to Namdev et al.[12]. Tables III and II vindicate the efficacy of the proposed pipeline as the absolute translation error(ATE) are typically around 3m for sequences more than 100m in length. The last row of Table III denotes the percentage error, which is significantly low for fairly long sequences at an average of 3.11%, considering that the original problem is intractable and hard to solve.