{'fraction_non_alphanumeric': 0.032223105266960476, 'fraction_numerical': 0.009031139368542735, 'mean_word_length': 4.705482275350371, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Multi-object Monocular SLAM for Dynamic Environments

In this paper, we tackle the problem of multibody SLAM from a monocular camera. The term multibody, implies that we track the motion of the camera, as well as that of other dynamic participants in the scene. The quintessential challenge in dynamic scenes is unobservability: it is not possible to unambiguously triangulate a moving object from a moving monocular camera. Existing approaches solve restricted variants of the problem, but the solutions suffer relative scale ambiguity (i.e., a family of infinitely many solutions exist for each pair of motions in the scene). We solve this rather intractable problem by leveraging single-view metrology, advances in deep learning, and category-level shape estimation. We propose a multi pose-graph optimization formulation, to resolve the relative and absolute scale factor ambiguities involved. This optimization helps us reduce the average error in trajectories of multiple bodies over real-world datasets, such as KITTI. To the best of our knowledge, our method is the first practical monocular multi-body SLAM system to perform dynamic multi-object and ego localization in a unified framework in metric scale.


I. INTRODUCTION
Monocular SLAM research has significantly matured over the last few decades, resulting in very stable off-the-shelf solutions [1]- [3]. However, dynamic scenes still pose unique challenges for even the best SLAM systems. In this work, we tackle a more general version of the monocular SLAM problem in dynamic environments: multi-body visual SLAM. While monocular SLAM methods traditionally track the egomotion of a camera and discard dynamic objects in the scene, multi-body SLAM deals with the explicit pose estimation and tracking of dynamic objects (dynamic bodies). Tracking multiple objects could have several applications. For example, a car driving on a road will find it beneficial to keep tracking other traffic participants [4].
Concretely, multi-body monocular SLAM involves the estimation of the trajectories of multiple freely moving objects undergoing rigid transform using observations purely from a freely moving monocular camera.
Despite being an extremely useful problem, multibody visual SLAM has not received comparable attention to its  uni-body counterpart, wherein uni-body SLAM typically refers to SLAM in presence of the stationary world where all other moving rigid bodies have been removed from the scene. This can primarily be attributed to the ill-posedness of monocular multi-body Structure-from-Motion in general [5]. While the scale factor ambiguity of monocular SLAM is well-known [1]- [3], [6], the lesser-known-yet-well-studied relative scale problem persists with multi-body monocular SLAM [5], [7]- [12]. In a nutshell, relative-scale ambiguity refers to a phenomenon where the estimated trajectory is ambiguous and is recovered as a one-parameter family of trajectories relative to the ego-camera. Each dynamic body has a different, uncorrelated relative-scale, which renders the problem unobservable 1 [5]. Even in cases where it is observable, degeneracies exist [7], [9], [12].
This incites us to explore static feature correspondences in the environment to get ego and dynamic vehicle motion in metric scale 2 .
We investigate the benefits of our multi pose-graph optimization framework for dynamic objects in a scene. We show that this framework enables us to cast multiple objects including ego vehicle in a unified global frame in metric scale.
To the best of our knowledge, this is the first Monocular Multibody SLAM to represent moving obstacle trajectories We obtain dynamic-vehicle localizations via the modules explained in blue section. The mathematical representations to the same can be found in IV-C and IV-A. The green section illustrates our approach to obtain accurate odometry estimations in metric scale, as explained in IV-B. The orange section illustrates a part of the pose-graph structure where the gray, the orange and the purple nodes represent the nodes for ego-car and two dynamic vehicles in the scene respectively. Moreover, the black, the blue and the red edges represent the camera-camera, vehicle-vehicle and camera-vehicle edges respectively. in a unified global metric frame and benchmarked vis a vis ground truth. The quantitative results presented demonstrate the efficacy of the proposed formulation wherein the average trajectory error in metres for dynamic participants on KITTI sequences is well within acceptable limits considering the difficulty of the posed problem.
In the remainder of this paper, we elaborate upon the following key contributions: 1) Utilizing 3D depth in metric scale to get static groundplane feature correspondences to estimate odometry. 2) Formulate the relationships between various dynamic vehicles and the ego-vehicle appropriately in the form of a pose-graph. 3) Perform localization and mapping for multiple dynamic vehicles in scene and dynamic ego-vehicle using a single optimization problem. 4) Practicality: We evaluate our approach on challenging sequences from the KITTI driving dataset [13], and present the first practical monocular multi-body SLAM system.

II. RELATED WORKS
We begin with a discussion of prior art in multi-body monocular SLAM. The earliest approaches [9], [14]- [17] were based on segmenting multiple motions given a set of triangulated points. Extending epipolar geometry to multiple objects, multibody fundamental matrices were used in [9]- [11], [17].
Trajectory triangulation methods [8], [18], on the other hand, derive a set of constraints for trajectories of objects, and solve the multi-body SLAM problem under these constraints.
Ozden et al. [5] extend the multi-body Structure-from-Motion framework [15] to cope with practical issues, such as a varying number of independently moving objects, track failure, etc.
Still, others have relied on model selection methods to segment independently moving objects in a scene, and have then solved explicitly for relative scales [10], [14], [15].
Most of the above approaches operated offline, and extending them for online operation was non-trivial.
Kundu et al. [7] proposed a fast, incremental multibody SLAM system that leverages motion segmentation to assign feature tracks to dynamic bodies, and then solve independently for each relative-scale. Critical to their success is the underlying assumption of smooth camera motions. Later Namdev et al. [12] provided analytical solutions for a variety of vehicle motions, prominently linear, planar and nonholonomic. The underlying principle here is that since the problem of computing relative scale is fundamentally illposed it can be computed with restrictions on motions of the dynamic participants in the scene.
More recently, Ranftl et al. [19] present a dense monocular depth estimation pipeline targeted at dynamic scenes. Using optical flow and motion segmentation, they formulate a convex optimization problem that jointly estimates pixel correspondences across two-views. However, relative scale problems still persist. The authors tackle this ambiguity by defining support priors between an object and the environment. Furthermore, the relative-scale resolved reconstruction is not in metric scale. The authors in Cubeslam [20] propose an object SLAM framework for on-road scenes. However the problem is not cast into a Dynamic SLAM/ Multibody SLAM context and hence unlike the present work, Cubeslam [20] does not show trajectories of dynamic participants and benchmark it with respect to ground truth.
With the advent of deep learning, improvements to object detection [21]- [24] and motion segmentation have resulted in such methods directly being employed in multi-body SLAM.
Reddy et al. [25] and Li et al. [26] present approaches to multi-body SLAM, but using a stereo camera. In this case, however, the problem is observable, while we handle the harder, unobservable case.

III. OVERVIEW OF THE PROPOSED PIPELINE
The complete pipeline we follow can be summarized as follows: 1) We take a stream of monocular images as input to our pipeline. 2) We exploit 3D depth estimation to ground plane points as a source of vehicle localizations in ego-camera frame as explained in Sec. IV-A. 3) Alternatively, as explained in Sec. IV-C, we fit a base shape prior to each vehicle instance uniquely to obtain refined vehicle localizations in ego-camera frame. 4) To obtain accurate odometry estimations, we scale ORB-SLAM2 [27] to metric units by estimating egomotion with respect to detected road plane point correspondences over multiple frames in each sequence. Sec. IV-B describes this module of our pipeline mathematically. 5) Finally, our optimization formulation Sec. V uses the above localizations and odometry estimation to resolve cyclic-consistencies in the pose-graph. 6) This provides us with accurate multi-body localizations in a static global frame and consistent metric scale.

A. Depth Estimation for Points on Ground Plane
We make use of known parameters like the camera intrinsic parameters, ground plane normal, 2D bounding boxes [28] and camera height in metric unit to estimate the depth of any point on the ground plane 3 . This is based on the assumption that ego-camera is always at a constant height from the ground plane throughout the sequence. Given the camera height from the ground plane to be h metres, normal vector from the ground plane to be n, camera intrinsic calibration matrix to be K and 2D homogenous coordinates in image space to be x t , we obtain the 3D depths to the corresponding points in image space using the following method as shown in Song et al. [29].

B. Odometry Estimations
The initial input to our odometry pipeline (c.f. Fig. 2) comes from the ORB trajectory [27] which provides 3D ego vehicle poses in a static global frame of reference i.e., the ego-vehicle's pose in its first frame. However, the provided input odometry is in an ambiguous ORB scale as opposed to our requirement of metric scale.
We propose a method that scales ego-motion from the ORB-SLAM2 [27] 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: We now obtain ORB features to match point correspondences between the two frames t − 1 and t and use state-ofthe-art semantic segmentation network [21] 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 by estimating them in corresponding camera frames t − 1 and t using Song et al. [29] as explained in IV-A. To reduce the noise incorporated by the above method, we only consider points within a depth of 12 metres from the camera. Further, we obtain the required scale-factor α that scales odometry from Eqn. 2 using the following objective function: Here, R t−1 t and tr t−1 t represent the relative rotation matrix and translation vector respectively. After solving the above minimization problem, we finalize our scale factor α as the mean of solutions obtained from the following:

C. Pose and Shape Adjustments Pipeline
We obtain our object localizations by doing shape and pose adjustments using a method inspired from Murthy et al. [30]. Our object representation follows from [30]- [32], which is based on a shape prior consisting of K ordered keypoints, each of which represents a specific visually distinguishable feature from our objects which primarily consist of cars and minivans. For this work, we stick with the same 36 keypoints structure from Ansari et al. [32]. We obtain the keypoint localizations on the objects in 2D image space using a CNN based on stacked hourglass architecture [33] and use the same model trained on over 2.4 million rendered images for Ansari et al. [32].
Borrowing the notations from Murthy et al. [30], we begin with a basis shape prior for the object which can be represented as mean shape X ∈ R 3K . Let the B basis vectors be V ∈ R 3K×B and the corresponding deformation coefficients be Λ ∈ R B . Assuming that a particular object coordinate frame has a rotation of R ∈ SO(3) and translation of t ∈ R 3 with respect to the camera, any object instance X ∈ R 3K in the scene can be shown mathematically using the following shape prior model: represents the basis shape prior and the resultant shape for the object instance is obtained as X where each X i represents one of the K = 36 keypoints in 3D coordinate system from camera's perspective.
Let the ordered collection of keypoint localizations in 2D image space obtained from the stacked hourglass based . Now, fairly accurate estimates for the pose parameters (R, t) and the shape parameter (Λ) for the specific object instance can be obtained using an objective function defined as follows: Here π K represents the function which projects 3D coordinates onto 2D image space given the camera intrinsics µ = (f x , f y , c x , c y ). Mathematically, π K can be defined as: Minimizing the above objective function (c.f. Eqn 7) separately for pose parameters (R,t) and shape parameters (Λ) provides us with an optimal fitting of the shape prior over the dynamic object. We obtain the final orientation of the object as R after pose parameter adjustments and we obtain the object's 3D depth from camera as t , obtained from the mean of wheel centres in 3D coordinates.

V. MULTI-OBJECT POSE GRAPH OPTIMIZER
A. Pose-Graph Formulation  which defines the pose of node A with respect to a staticglobal frame of reference W. Meanwhile, a binary-edge from A to B is represented with a relative pose T A B ∈ SE(3) called the measurement which defines the pose of node A from another node B's perspective. 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 (i.e., T W c(t−1) and T W c(t) ) and measurement for the camera-camera edge (i.e. T c(t−1) c(t) ) from our odometry estimation (c.f. IV-B). We use this odometry to register dynamic object localizations from pose-shape adjustment pipeline as explained in IV-C [30]- [32] to provide for the vehicle node estimates T W v(t−1) and T W v(t) . We obtain measurement for the camera-vehicle edge (i.e., T v(t−1) c(t) ) from shape and pose adjustment (c.f. IV-C). Moreover, we use depth estimation on ground plane using Song et al. [29] as explained in IV-A as a source of vehicle localizations that is unique from the localizations obtained from IV-C. This registered with our odometry estimations provides for our vehicle-vehicle edge measurement i.e., T v(t−1) v(t) .

B. Cost Functions
As explained in V-A, given that W represents the static world coordinate frame of reference, nodes A and B are parameterized by poses T W A ∈ SE(3) and T W B ∈ SE(3) respectively, and a binary edge from node A to node B is parameterized using a pose T A B ∈ SE(3). Each binary edge between any pair of nodes represents a unique constraint between the respective nodes. This constraint can be mathematically represented as follows: Assuming relative correctness between each term in Eqn. 10, it results in an identity matrix I 4 ∈ SE(3) irrepective of the order of transformation. Thus, Eqn. 10 reduces to: Fig. 5: Dynamic multibody SLAM results on KITTI-Tracking sequences. Col 1 illustrates the input image stream with bounding boxes to specify the vehicles being mapped in Col 2 and Col 3. While Row 1 and Row 3 illustrate our performance on multi-vehicle road plane scenarios, Row 2 shows our results for a vehicle far away from the camera over a long sequence. Black vehicle represents the ego-vehicle and red, blue and green plots represents the unique vehicle instances in the scene. The dotted plots in the same colors represent the corresponding ground truths.
We can see that the order in which the transformations are applied do not change the consistency of the respective cycle in the pose-graph. Thus, Eqn. 11 can also be written as follows: Using Fig. 4 as the illustrative reference, let four nodes be c(t − 1), c(t), v(t − 1), v(t) which represents ego-camera at time instant t − 1 and t and dynamic vehicle in the scene at time instant t − 1 and t respectively. The node estimates for the four nodes are T W c(t−1) , T W c(t) , T W v(t−1) and T W v(t) respectively. Now, following the format of Eqn. 11, the cost function for each of the four binary-edges can be shown mathematically as: Here, Υ cc , Υ cv(t−1) , Υ cv(t) and Υ vv represent the cost functions for camera-camera, camera-vehicle at time instants t − 1 and t, and vehicle-vehicle edges respectively. Cumulatively, the above cost functions for a single loop illustrated in Fig. 4 can be represented as: On substituting Eqn. 13 in Eqn. 14, and further simplification, we obtain the resultant function for cumulative cost as follows: Clearly, Eqn. 15 defines the cyclic consistency within the loop defined by the four binary edges.

C. Confidence Parameterization
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 Ω E ∈ R N ×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 Ω E corresponding to each edge E in the whole pose graph.
We utilize the information matrix as a confidence parameterization for various sources of input-data for the posegraph. To make the most out of this scenario, we scale the information matrix for an edge E by a scale factor λ ∈ R such that effective information matrix Ω E that is finally passed as a parameter is obtained as: (16) Where Ω E corresponds to the information matrix initialization for the edge E. We categorize all the edges in our pose-graph formulation into three types namely cameracamera, 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 λ, 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 for camera-vehicle edges and vehicle-vehicle edges respectively as explained in V-A, we make use of this parameter to scale the information matrix corresponding to the two types of edges in each loop in our pose graph. It has been observed over a large number of vehicles in the data that localizations obtained from IV-C performs better than the localizations obtained from IV-A for vehicles that are at a relatively closer depth to the camera (up to about 45 metres). However, localizations from IV-A are relatively more accurate when compared to the same obtained from IV-C at depths far away from the camera (over 45 metres).
This can be attributed to the keypoint localizations being inaccurate for vehicles that are very far from the camera due to lack of clarity indistinguishable features on these vehicles. However, factors like visible features do not affect the dynamic vehicle localizations using IV-A as this method relies only on the camera intrinsic parameters, ground plane normal, camera height and 2D bounding boxes [28].

A. Dataset
We test our procedure over a wide range of KITTI-Tracking training sequences [13], 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 as well as the other vehicles under the ego-car's observation in a multi-object scenario.
The labels provided along with the training sequences for KITTI-Tracking dataset [13] are used as ground truth for getting 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.

1) Pose and Shape Adjustments:
We obtain accurate localizations in ego-camera frame by fitting base shape priors to each non-occluded and non-truncated vehicle in the scene irrespective of its orientation with respect to the ego-camera. While the pipeline is dependent on the keypoint localizations on these vehicles, factors like large depth from camera are bound to affect the accuracies with respect to ground truth. However, this approach ensures fairly accurate vehicle localizations for the pose-graph optimizer to apply its edge constraints on. Fig. 6 illustrates wireframe fitting and subsequent mapping in ego-frame for a traffic scenario consisting of multiple vehicles. Fig. 6: Localizations in ego-camera frame after pose and shape estimations for a dynamic multi-vehicle scenario.
2) Odometry Estimation: For accurate visual odometry, we exploit distinguishable static ORB [1], [27] features on road plane from entities like curbs, lane markers and any irregularities that may be present on the road to obtain quality point correspondences. While the approach is dependent on factors like irregularities on the road and reasonable visibility, we obtain robust performance over a diverse range of sequences many of which are over a 100 frames long. 3) Pose-Graph Optimization: We resolve for each cyclicloop created by the ego-camera and each vehicle (c.f. Eqn. 15) in the scene in our optimization formulation. The optimization problem runs for a maximum of 100 iterations. Our unified pose-graph based optimization formulation performs consistently well on a wide range of sequences irrespective of the length of the sequences, number of objects being localized and varying instance lengths for each vehicle in the same sequence. A unique pose-graph structure for all vehicles including ego-motion at each time instance ensures effective error re-distribution across all trajectories based on efficient confidence allocation in the form of information matrix scaling (c.f. Eqn. 16). Fig. 5 illustrates ego-motion as well as the motion of various vehicles over many sequences from the KITTI Tracking dataset [13].   C. Quantitative Results 1) Odometry Estimations: As an attempt to improve odometry estimations, we placed a threshold T on the depth from camera upto which we considered point correspondences. This has been set based on our observation that the accuracy of the 3D depth to the point correspondences lowers with depth from the camera. Table II summarizes our experiments with various threshold values before we finalize our threshold at T = 12 metres. While T = 12 meters delivers best results for most sequences mentioned in II, we see that T = 15 metres performs better for sequences 6 and 9, both of which involve the ego-vehicle taking a sharp turn at an intersection. This is because we rely on ground plane features including and largely contributed to by the lane markers on the road plane. Given that the segment of road plane in the scene at an intersection is devoid of any road/lane markers, we do not get enough feature correspondences from closer segments of the road. Meanwhile, increasing the threshold enables us to pick up points from the road plane continuing beyond the intersection which contains better scope for quality feature correspondences in the form of lane markers. Consequently, a relatively larger threshold performs better.  2) Pose-Graph Optimization: Our pose-graph formulation consists of three categories of edges namely cameracamera(C-C) edges, camera-vehicle(C-V) edges and Vehicle-Vehicle(V-V) edges. Each of these sets of edges are scaled with a unique confidence parameter λ. To analyse how each category of edges constraint our pose-graph and consequently affect our optimization results, we analyse the effect of removing the constraint put forth by each type of edges E ∈ {C-C, C-V, V -V } by setting the corresponding λ E = 0. Table I summarizes our experiments with the pose-graph optimizer and various combinations of its edgeparameterizations. It can be noted from Table I that few vehicles in sequence 3 and the ego-vehicle in sequence 4 perform better when C-C constraints are relaxed. This is because the odometry estimations are relatively less accurate in these sequences when compared to the other vehicles in the same sequence. The optimizer generally utilizes reliable edges in each loop of the pose-graph to improve the relatively less reliable edges, provided their information matrices are scaled appropriately. Given that the C-C edges are relatively less reliable in these sequences, relaxing its constraints from the optimization problem enables other edges to improve upon the overall error in these sequences. A similar explanation can be given for the errors shown by the ego-motion in sequence 18. Since C-V edges of the ego-motion is relatively more accurate than the corresponding C-V edges of other vehicles, we obtain a better result for the same when the C-V edge constraint is relaxed. Both C-C and V-V edges are generated using the odometry estimations and are thus influenced by its accuracy too.

D. Summary of Results
While Fig. 5 illustrates how our trajectories perform with respect to ground truth, Table I reaffirms how our pose-graph formulation successfully redistributes errors about constraints with high confidence parameters. The Table I vindicates 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 the table 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.

VII. CONCLUSIONS
Monocular Multi-body SLAM is ill-posed as it is impossible to triangulate a moving vehicle from a moving monocular camera. This observability problem manifests in the form of relative scale when posed into the Multibody framework.
While with the arrival of single view reconstruction methods based on Deep Learning some of these difficulties are alleviated, one is still entailed to represent the camera motion and vehicle in the same scale. This paper solves for this scale by making use of the ground plane features thereby initializing the ego vehicle and other dynamic participants with respect to a unified frame at metric scale. Further, a pose graph optimization over vehicle poses between successive frames mediated by the camera motion formalizes the Multibody SLAM framework.
We showcase trajectories of dynamic participants as well as the ego vehicle over sequences of more than a hundred frames in length with high fidelity ATE (Absolute Translation Error). To the best of our knowledge, this is the first such method to represent vehicle trajectories in the global frame over long sequences. The pipeline is able to accurately map trajectories of dynamic participants far away from the ego camera and its scalability to map multi-vehicle trajectories is another salient aspect of this submission.