OrcVIO: Object residual constrained Visual-Inertial Odometry

Supplementary Material

Software and videos supplementing this paper are available at:

I Introduction

The foundations of visual environment understanding in robotics, machine learning, and computer vision lie in the twin technologies of inferring geometric structure and semantic content. Researchers have made significant progress in geometric structure reconstruction using Structure from Motion (SfM) and SLAM techniques. State of the art SLAM approaches work with monocular or stereo cameras , often complemented by inertial information . However, most real-time incremental SLAM techniques provide purely geometric representations, e.g., of points, lines, or planes, that lack semantic interpretation of the environment.

Recently, tremendous progress has been achieved in semantic scene understanding using deep neural networks for object detection , instance segmentation , and object tracking . Nevertheless, the literature in deep learning is sparse in techniques that provide global positioning of the detected and tracked objects to obtain an object-level map online.

This paper focuses on joint visual-inertial odometry and object mapping, bridging the gap between geometric and semantic inference in SLAM. Generating geometrically consistent and semantically meaningful maps allows compressed representation, improved loop closure (recognizing already visited locations), and robot mission specifications in terms of human-interpretable objects, e.g., for safe navigation, manipulation, multi-stage interaction . Our work introduces object states, modeling the position, orientation, and shape of object instances in the environment. We utilize a coarse category-agnostic and a fine category-specific model of object shape. The coarse model uses an ellipsoid to restrict an object’s pose variation and relate its coarse shape to bounding-box detections. The fine model uses a set of mid-level object parts (e.g., car wheels, windshield, doors), called semantic keypoints, to obtain a precise part-based shape description. Given streaming inertial measurement unit (IMU) and monocular camera measurements, we develop an algorithm to simultaneously estimate the IMU-camera trajectory and the states of the objects, detected and tracked in the camera images. The contributions of this paper are summarized as follows.

We introduce object states in the formulation of a SLAM problem, modeling position, orientation, coarse ellipsoid shape, and fine semantic-keypoint shape.

We define residuals relating object states and IMU-camera states to inertial measurements, geometric features, object semantic features, and object bounding-box detections, and explicitly derive their Jacobians.

We develop an extension of the multi-state constraint Kalman filter (MSCKF) to enable online tightly coupled estimation of object and IMU-camera states. Our innovations include closed-form mean and covariance propagation over the SE(3) pose and velocity manifold of the IMU-camera states and measurement updates based on our new semantic residuals with object states optimized over multiple views. Our algorithm is suitable for real-time incremental odometry and object mapping and is more efficient than nonlinear batch optimization.

We name our method Object residual constrained Visual Inertial Odometry (OrcVIO) to emphasize the role of the semantic residuals in the optimization process. OrcVIO is capable of producing meaningful object maps and estimating accurate sensor trajectories, as shown in Fig. 1.

II Related Work

Many visual SLAM approaches work with monocular or stereo cameras . Featureless approaches that minimize image intensity directly have been proposed, and inertial information is often used to complement the visual information . The MSCKF algorithm leverages both inertial data and visual features in an extened Kalman filter formulation. Each visual feature whose track is lost provides multi-frame constraints for a corresponding 3D landmark. The constraint residuals are linearized to perform the filter update step. A key idea is to marginalize the landmark states via projection to the null space of the visual feature Jacobians, allowing structureless estimation of the IMU-camera states only. A key extension of OrcVIO over the MSCKF and visual-inertial SLAM algorithms in general is to introduce object measurements (bounding boxes and semantic features) and object states whose residuals constrain the IMU-camera states.

Recent methods have considered learning to regress camera poses directly from raw images . For instance, monocular depth, optical flow, and ego-motion are jointly optimized from video in by relying on a view-synthesis loss. These unsupervised learning techniques have shown impressive performance in localization but do not generate global maps. In this paper, we focus on obtaining a joint geometric-semantic representations from measurements in real time, i.e., spatial perception . Prior works that utilize both spatial and semantic information include , but the spatial and semantic states are estimated independently and merged later. On the other hand, consider joint metric and semantic mapping. Recent works focus on the tightly coupled spatial and semantic estimation, and there are mainly two groups of object-based SLAM techniques: category-specific and category-agnostic.

Category-specific approaches optimize the pose and shape of object instances, using semantic keypoints or 3D shape models . For example, introduces a real-time joint 3D object pose and camera pose estimation via pose graph optimization. The objects stored in a database that are also present in the current frame are detected and optimized, using the vertex and normal map from a RGBD sensor. Object pose and shape are optimized in using semantic keypoints to provide additional error terms related to object pose in the SLAM factor graph. Visual-inertial information for object mapping is used in , relying on a database to retrieve object shapes. Hu et al. embed object priors into least-squares minimization to incrementally track and map chairs. The object shape represented by a binary voxel grid is compactly described by a latent code obtained from an auto-encoder, which is used for shape initialization and iterative residual minimization. These methods in general are computationally demanding due to iterative batch optimization and reliance on instance-specific CAD models.

Category-agnostic approaches use geometric shapes, such as spheres , cuboids , or ellipsoids , to represent objects. SSFM uses the tightest bounding cube enclosing an object to parameterize the object location and pose. The object measurements include the location, size of the object bounding box and the object pose obtained from a 3D object detector . Assuming the semantic measurements are consistent across frames, an object state is optimized via maximum likelihood estimation. CubeSLAM generates and refines 3D cuboid proposals using multi-view bundle adjustment without relying on prior models. QuadricSLAM uses an ellipsoid representation, suitable for defining a bounding-box detection model. Structural constraints based on supporting and tangent planes, commonly observed under a Manhattan assumption, have also been introduced . Using generic symmetric shapes, however, makes the orientation of object instances potentially irrecoverable. For instance, the front and back of an object become indistinguishable.

This paper extends our prior conference publication with several theoretical contributions and a large-scale experimental evaluation of OrcVIO. While used linear velocity measurements, this paper uses a complete description of all IMU states for odometry and derives a novel closed-form expression for covariance propagation. A new bounding-box residual is developed to ensure that it scales equivalently as the keypoint residual. In , the bounding-box residual was quadratic instead of linear as a function of the bounding-box lines. This version also implements a zero-velocity residual to reduce VIO drift when the sensor is completely static. Extensive evaluation is conducted on the KITTI dataset, a photo-realistic Unity dataset, and in indoor and outdoor real-time experiments. We also extend OrcVIO to handle multiple object classes, including cars, doors, barriers, monitors, chairs.

III Background and Notation

We denote the IMU, camera, object, and global reference frames as $\{I\}$, $\{C\}$, $\{O\}$, $\{G\}$, respectively. The transformation from frame $\{A\}$ to $\{B\}$ is specified by a $4\times 4$ matrix:

We define an infinitesimal change of pose $\mathbf{T}\in SE(3)$ using a right perturbation $\mathbf{T}\exp\left(\boldsymbol{\xi}_{\times}\right)\in SE(3)$ (see [73, Ch.7]).

where $\mathbf{I}_{3}$ is the $3\times 3$ identity matrix. A quadric shape [74, Ch.3] is a set $\left\{\mathbf{x}\mid\underline{\mathbf{x}}^{\top}\mathbf{Q}\underline{\mathbf{x}}\leq 0\right\}$, where $\mathbf{Q}$ is a symmetric matrix. Consider an axis-aligned ellipsoid centered at $\mathbf{0}$:

IV Problem Formulation

The shape of an object $\mathbf{o}_{i}$ in the global frame $\{G\}$ is obtained by deforming and transforming the semantic landmark positions, ${}_{O}\mathbf{T}\left(\underline{\mathbf{s}}_{l}+\underline{\delta\mathbf{s}}_{l}\right)$, and the dual ellipsoid, ${}_{O}\mathbf{T}\mathbf{Q}_{(\mathbf{u}+\delta\mathbf{u})}^{*}{}_{O}\mathbf{T}^{\top}$, using the instance pose ${}_{O}\mathbf{T}$ and deformations $\delta\mathbf{s}$, $\delta\mathbf{u}$. Fig. 2 shows an illustration for a car model with $12$ semantic landmarks.

Let $\mathds{1}_{k,m,n}\in\{0,1\}$ indicate whether the $n$-th geometric keypoint observed at time $t_{k}$ is associated with the $m$-th geometric landmark. Similarly, let $\mathds{1}_{k,i,j}\in\{0,1\}$ indicate whether the $j$-th object detection at time $t_{k}$ is associated with the $i$-th object instance. This data association information can be obtained by keypoint and object tracking as described in Sec. V-A. Given the associations, we introduce error functions:

for the inertial, geometric, semantic keypoint and bounding-box measurements, respectively, defined precisely in Sec. V. We also introduce an object shape regularization error term ${}^{r}\mathbf{e}\left(\mathbf{o}_{i}\right)$ to ensure that the instance deformations $(\delta\mathbf{s},\delta\mathbf{u})$ remain small, and consider the following problem.

Determine the sensor trajectory $\mathcal{X}^{*}$, geometric landmarks ${\cal L}^{*}$, and object states $\mathcal{O}^{*}$ that minimize the weighted sum of squared errors:

where ${}^{*}w$ are positive constants determining the relative importance of the error terms and ${}^{*}\mathbf{V}$ are positive-definite matrices specifying the covariances associated with the inertial, geometric, semantic, and bounding-box measurements. A measurement covariance $\mathbf{V}$ defines a quadratic (Mahalanobis) norm $\|\mathbf{e}\|_{\mathbf{V}}^{2}\triangleq\mathbf{e}^{\top}\mathbf{V}^{-1}\mathbf{e}$.

V Landmark and Object Reconstruction

Our approach consists for a front-end measurement generation stage and a back-end landmark state and sensor pose optimization stage. This section discusses the detection and tracking of geometric keypoint measurements ${}^{g}\mathbf{z}_{k,n}$ and object class ${}^{c}\mathbf{z}_{k,j}$, bounding-box ${}^{b}\mathbf{z}_{k,l,j}$, and semantic keypoint ${}^{s}\mathbf{z}_{k,l,j}$ measurements in the front-end. It also defines the error functions in (Problem) and their Jacobians needed for the back-end optimization. Finally, it presents the back-end optimization over the geometric landmarks ${\cal L}$ and the object states ${\cal O}$ for a given sensor sensor trajectory ${\cal X}$.

Geometric keypoints ${}^{g}\mathbf{z}_{k,n}$ are detected in the camera images using the FAST detector and are tracked temporally using the Lucas-Kanade (LK) algorithm . Keypoint-based tracking has lower accuracy but higher efficiency than descriptor-based methods, allowing our method to use a high frame-rate camera and process more keypoints. Outliers are eliminated by estimating the essential matrix between consecutive views and removing those keypoints that do not fit the estimated model. Assuming that the time between consecutive images is short, the relative orientation is obtained by integrating the gyroscope measurements ${}^{i}\boldsymbol{\omega}_{k}$ as described in Sec. VI-A and only the unit translation vector is estimated using two-point RANSAC .

The YOLO detector is used to detect object classes ${}^{c}\mathbf{z}_{k,j}$ and bounding-box lines ${}^{b}\mathbf{z}_{k,l,j}$. Semantic keypoints ${}^{s}\mathbf{z}_{k,l,j}$ are extracted within each bounding box using the StarMap stacked hourglass convolutional neural network . We augment the original StarMap network with dropout layers as shown in Fig. 3(b). Several stochastic forward passes may be preformed using Monte Carlo dropout to obtain semantic keypoint covariances ${}^{s}\mathbf{V}$, illustrated in Fig. 3(c).

The bounding boxes ${}^{b}\mathbf{z}_{k,l,j}$ are tracked temporally using the SORT algorithm , which performs intersection over union (IoU) matching via the Hungarian algorithm. The semantic keypoints ${}^{s}\mathbf{z}_{k,l,j}$ within each bounding box are tracked via a Kalman filter, which uses the LK algorithm for prediction and the StarMap keypoint detections for update.

V-B Landmark and Object Error Functions

where $\oplus$ emphasizes that some additions are over the $SE(3)$ manifold, defined as follows:

where we use right perturbations ${}_{I}\boldsymbol{\theta}_{\times}\in\mathfrak{so}(3)$, ${}_{C}\boldsymbol{\xi}_{\times}\in\mathfrak{se}(3)$, and ${}_{O}\boldsymbol{\xi}\in\mathfrak{se}(3)$ for the IMU orientation ${}_{I}\hat{\mathbf{R}}$, camera pose ${}_{C}\mathbf{T}$, and object pose ${}_{O}\hat{\mathbf{T}}$, respectively.

where $[\cdot]^{\odot}$ is defined in (3), $\frac{d\pi}{d\underline{\mathbf{s}}}(\underline{\mathbf{s}})$ is the Jacobian of $\pi(\underline{\mathbf{s}})$ and:

The Jacobians with respect to other perturbations in (7) are $\mathbf{0}$.

The semantic-keypoint error is defined as the difference between the projection of a semantic landmark $\mathbf{s}_{l}+\delta\mathbf{s}_{l}$ from the object frame to the image plane, using instance pose ${}_{O}\mathbf{T}$ and camera pose ${}_{C}\mathbf{T}$, and its corresponding semantic-keypoint observation ${}^{s}\mathbf{z}$:

The Jacobians with respect to other perturbations in (7) are $\mathbf{0}$.

We define the bounding-box error as the distance between the hyperplane $\underline{\mathbf{b}}=\left[\mathbf{b}^{\top}\ -b_{h}\right]^{\top}\triangleq{}_{O}\mathbf{T}^{\top}{}_{C}\mathbf{T}^{-\!\top}\mathbf{P}^{\top}{}^{b}\underline{\mathbf{z}}$ induced by projecting the bounding-box line ${}^{b}\mathbf{z}$ to the object frame and the closest hyperplane that is tangent to the quadric surface $\mathbf{Q}_{(\mathbf{u}+\delta\mathbf{u})}^{*}$ of object $\mathbf{o}$:

where $\operatorname*{sgn}(x)=\frac{\partial|x|}{\partial x}$ is the signum function.

where $\underline{\hat{\mathbf{b}}}={}_{O}\hat{\mathbf{T}}^{\top}{}_{C}\hat{\mathbf{T}}^{-\!\top}\mathbf{P}^{\top}{}^{b}\underline{\mathbf{z}}$ and with $\hat{\mathbf{U}}\triangleq\operatorname*{diag}(\mathbf{u}+\delta\hat{\mathbf{u}})$:

The Jacobians with respect to other perturbations in (7) are $\mathbf{0}$.

Finally, the object shape regularization error is defined as:

V-C Landmark and Object State Optimization

for all $l$ and all $k,j$ such that $\mathds{1}_{k,i,j}=1$, where the unknowns are ${}_{O}\hat{\mathbf{T}}$ and the semantic keypoint depths $\lambda_{k,l,j}$.

The least squares problem for semantic keypoints is a generalization of the pose from $n$ point correspondences (PnP) problem . While this system may be solved using polynomial equations , we perform a more efficient initialization by defining $\boldsymbol{\zeta}_{l}\triangleq{}_{O}\hat{\mathbf{R}}\mathbf{s}_{l}+{}_{O}\hat{\mathbf{p}}$ and solving the first set of (now linear) equations in (19) for $\boldsymbol{\zeta}_{l}$ and $\lambda_{k,l,j}$. We recover ${}_{O}\hat{\mathbf{T}}$ via the Kabsch algorithm between $\left\{\boldsymbol{\zeta}_{l}\right\}$ and $\left\{\mathbf{s}_{l}\right\}$. This approach works well as long as there is a sufficient number of semantic keypoints ${}^{s}\mathbf{z}_{k,l,j}$ (at least two per landmark across time for at least three semantic landmarks $\mathbf{s}_{l}$) associated with the object. If fewer semantic keypoints are available, ${}_{O}\hat{\mathbf{T}}$ can be recovered from the second equation in (19). Let $\mathbf{Q}^{*}\triangleq{}_{O}\hat{\mathbf{T}}\mathbf{Q}_{\mathbf{u}}^{*}{}_{O}\hat{\mathbf{T}}^{\top}$, ${\boldsymbol{\pi}}_{k,l,j}\triangleq{}^{b}\underline{\mathbf{z}}_{k,l,j}^{\top}\mathbf{P}{}_{C}\hat{\mathbf{T}}_{k}^{-1}$, then a linear system $\mathbf{M}\mathbf{w}=0$ can be formed:

for all $l$ and all $k,j$ such that $\mathds{1}_{k,i,j}=1$. The operator $\operatorname*{vec}(\cdot)$ vectorizes a matrix by stacking its columns into a single column vector and $\otimes$ is the Kronecker product. The solution can be found by:

where the equality constraint avoids the trivial solution. The minimization in (21) can done via SVD of the $\mathbf{M}$ matrix . The object pose ${}_{O}\hat{\mathbf{T}}$ can be recovered from $\hat{\mathbf{w}}=\operatorname*{vec}(\hat{\mathbf{Q}}^{*})$ by relating the estimated ellipsoid $\hat{\mathbf{Q}}^{*}$ in global coordinates to the ellipsoid $\mathbf{Q}^{*}_{\mathbf{u}}$ in the object frame:

The translation ${}_{O}\hat{\mathbf{p}}$ can be recovered from the last column of $\hat{\mathbf{Q}}^{*}$. To recover the rotation, note that $\mathbf{A}\triangleq\mathbf{P}\hat{\mathbf{Q}}^{*}\mathbf{P}^{\top}+{}_{O}\hat{\mathbf{p}}{}_{O}\hat{\mathbf{p}}^{\top}={}_{O}\hat{\mathbf{R}}\mathbf{U}\mathbf{U}^{\top}{}_{O}\hat{\mathbf{R}}^{\top}$ is a positive semidefinite matrix. Let its eigen-decomposition be $\mathbf{A}=\mathbf{V}\mathbf{Y}\mathbf{V}^{\top}$, where $\mathbf{Y}$ is a diagonal matrix containing the eigenvalues of $\mathbf{A}$. Since $\mathbf{U}\mathbf{U}^{\top}$ is diagonal, it follows that ${}_{O}\hat{\mathbf{R}}=\mathbf{V}$.

VI The OrcVIO Algorithm

Given time discretization $\tau_{k}$ and assuming ${}^{i}\boldsymbol{\omega}(t)$ and ${}^{i}\mathbf{a}(t)$ remain constant over the interval $t\in[t_{k},t_{k}+\tau_{k})$ with values ${}^{i}\mathbf{z}_{k}=({}^{i}\boldsymbol{\omega}_{k},{}^{i}\mathbf{a}_{k})$, we can compute the predicted IMU-camera state mean and covariance from (VI-A) and (24), respectively. Let $\hat{\mathbf{x}}_{k}$ and $\boldsymbol{\Sigma}_{k}$ be the prior mean and covariance.

The nominal dynamics (VI-A) can be integrated in closed-form to obtain the predicted mean $\hat{\mathbf{x}}_{k+1}^{p}$:

where $\mathbf{J}_{L}\left(\boldsymbol{\omega}\right)\triangleq{\mathbf{I}}_{3}+\frac{\boldsymbol{\omega}_{\times}}{2!}+\frac{\boldsymbol{\omega}_{\times}^{2}}{3!}+\ldots$ is the left Jacobian of $SO(3)$ and $\mathbf{H}_{L}\left(\boldsymbol{\omega}\right)\triangleq\frac{{\mathbf{I}}_{3}}{2!}+\frac{\boldsymbol{\omega}_{\times}}{3!}+\frac{\boldsymbol{\omega}_{\times}^{2}}{4!}+\ldots$. Both $\mathbf{J}_{L}\left(\boldsymbol{\omega}\right)$ and $\mathbf{H}_{L}\left(\boldsymbol{\omega}\right)$ admit closed-form (Rodrigues) expressions:

where ${}_{I}\boldsymbol{\Sigma}_{k}$ is the top-left $15\times 15$ block of $\boldsymbol{\Sigma}_{k}$ corresponding to the IMU state, ${}_{I}\mathbf{n}$ is white Gaussian noise with power spectral density $\mathbf{Q}=\operatorname*{diag}(\sigma_{\boldsymbol{\omega}}^{2}{\mathbf{I}}_{3},\sigma_{\mathbf{a}}^{2}{\mathbf{I}}_{3},\mathbf{0}_{3},\sigma_{g}^{2}{\mathbf{I}}_{3},\sigma_{a}^{2}{\mathbf{I}}_{3})$ and ${\mathbf{F}}(t)$ is:

where $\boldsymbol{\Phi}(t,s)$ is the transition matrixNote that ${}_{I}\hat{\mathbf{R}}(t)$ in $\mathbf{F}(t)$ is time varying. Time-invariant approximations of the transition matrix are presented in [86, App. B, E] using right-perturbation error dynamics and in using left-perturbation error dynamics. of (27).

The LTV SDE in (27) has a closed-form transition matrix:

where $\mathbf{w}={}^{i}\boldsymbol{\omega}_{k}-\hat{\mathbf{b}}_{g,k}$, $\mathbf{a}={}^{i}\mathbf{a}_{k}-\hat{\mathbf{b}}_{a,k}$ and the blocks are:

where $\Delta(t)=\left(\exp\left(t\boldsymbol{\omega}_{\times}\right)\left(\mathbf{I}_{3}-t\boldsymbol{\omega}_{\times}\right)-\mathbf{I}_{3}\right)$.

Using (28) and Proposition 5, we can approximate the discrete-time covariance propagation for the IMU state as ${}_{I}\boldsymbol{\Sigma}_{k+1}^{p}\approx\boldsymbol{\Phi}(\tau_{k},0)\left({}_{I}\boldsymbol{\Sigma}_{k}+\tau_{k}\mathbf{Q}\right)\boldsymbol{\Phi}(\tau_{k},0)^{\top}$. The full covariance propagation, accounting for the addition of the latest IMU pose ${}_{I}\hat{\mathbf{T}}_{k}$ to the sliding window and the dropping of the oldest IMU pose ${}_{I}\hat{\mathbf{T}}_{k-W}$, is:

VI-B Update Step

where $\mathbf{n}_{k,i}$ is the associated noise term with covariance $\mathbf{V}_{k,i}$. Stacking the errors for all camera poses in $\hat{\mathbf{x}}_{k+1}^{p}$ associated with $i$, leads to:

In Sec. IX-F, we also introduce a zero-velocity residual to account for complete stops of the system, which is common for autonomous ground and some aerial robots.

Finally, let $\hat{\mathbf{e}}$, $\mathbf{J}$, $\mathbf{V}$ be the stacked errors, Jacobians, and noise covariances (after null-space projection) across all geometric landmarks and object instances, whose tracks are lost at $t+1$. The updated IMU-camera mean and covariance are:

Note that the dimension of $\mathbf{J}$ can be reduced in the computation above via QR factorization as described in .

VII Evaluation

This section presents results from large-scale evaluation of OrcVIO using indoor and outdoor simulated and real data. We present experiments in photo-realistic Unity simulation (Fig. 5), on raw and odometry data sequences from the KITTI dataset , and on real data collected on UCSD’s campus.

We use two standard metrics for quantitative VIO evaluation: position Root Mean Square Error (RMSE) (also referred to as position ATE ) and KITTI’s translation error (TE) metric . Let $\operatorname{Trans}(\mathbf{T})$ return the position component of a pose $\mathbf{T}\in SE(3)$. Let ${}_{I}\mathbf{T}_{k}$ be the ground-truth pose trajectory and ${}_{I}\hat{\mathbf{T}}_{k}^{\prime}$ be the estimated pose trajectory. To measure error, the estimated trajectory is first aligned to the initial frame of the ground-truth trajectory via ${}_{I}\hat{\mathbf{T}}_{k}={}_{I}\mathbf{T}_{0}{}_{I}\hat{\mathbf{T}}_{0}^{-1}{}_{I}\hat{\mathbf{T}}_{k}^{\prime}$. After alignment, RMSE (m) and TE (%) are measured as:

where $\mathcal{F}$ is a set of frames with fixed distances $\operatorname{length}(i,j)$ over the values $\left\{100,...,800\right\}\,m$ .

The object estimates are evaluated using 3D Intersection over Union (IoU). A 3D bounding box $\hat{b}_{i}$ is obtained from each estimated object $\hat{\mathbf{o}}_{i}$, and IoU is defined as the ratio of the intersection volume over the union volume with respect to the bounding box $b_{i}$ of the closest ground truth object:

To understand the distribution of the object orientation and translation errors, we define an estimate as true positive if the closest ground-truth object pose is within a specific rotation or translation error threshold. Specifically, a rotation error of $\alpha^{\circ}$ means $\|\log({}_{O}\hat{\mathbf{R}}^{\top}{}_{O}\mathbf{R})^{\vee}\|_{2}\leq\alpha^{\circ}$, and translation error of $\beta\ m$ means $\|{}_{O}\mathbf{p}-{}_{O}\hat{\mathbf{p}}\|_{2}\leq\beta\ m$. We define precision as the fraction of true positives over all estimated objects and recall is the fraction of true positives over all ground-truth objects.

VII-B Unity Dataset Results

OrcVIO is evaluated in a Unity simulation containing 50 car, road barrier, and door object instances, shown in Fig. 5. A ROS bridge between Unity and Gazebo is used to simulate a quadrotor robot, navigating in the environment and providing IMU and camera measurements. The object map reconstructed by OrcVIO is shown in Fig. 5. Additional qualitative results and a video demo are available in the Supplementary Material. The estimated objects are generally quite close to the ground-truth ones. The object poses near the starting position are less accurate due to insufficient motion parallax, since the quadrotor performs a pure rotation in the beginning. The trajectory RMSE and TE are $0.97\,m$ and $0.40\%$, respectively. The odometry drift is mainly due to pure rotation maneuvers at planned path corners executed by the quadrotor controller.

The 3D IoU of the object estimates is $0.49$. The Precision and Recall of the object reconstruction is shown in Table I. Despite that doors and barriers have a thin structure, causing even small pose estimation drift to reduce the overlap with the ground-truth object instances, OrcVIO is able to produce an accurate object map with good 3D IoU.

VII-C KITTI Dataset Results

We also evaluate OrcVIO on the KITTI dataset , both qualitatively and quantitatively. We use the raw data sequences with object annotations to evaluate the object state estimation and the odometry sequences without object annotations for trajectory accuracy evaluation. Since the original inertial data from KITTI is not reliable, we use a VIO simulator to generate realistic noisy high-frequency IMU measurements at 250 Hz from the ground-truth poses.

Fig. 6 shows the IMU-camera trajectory and object states estimated on KITTI odometry sequence 07, and a video demo is provided in the Supplementary Material. The results demonstrate that OrcVIO produces meaningful object maps. We obtained ground-truth 3D annotations from the KITTI tracklets and the KITTI detection benchmark labels for quantiative evaluation of the object reconstruction. Table II reports 3D IoU results comparing OrcVIO against state-of-the-art methods, including a deep learning approach for single-view 3-D bounding-box estimation (SingleView ), and a multi-view bundle-adjustment algorithm that uses cuboids to represent objects (CubeSLAM ). OrcVIO has better 3D IoU than SingleView for the majority of the sequences (23, 36, 39, 64, 96) because, unlike SingleView, OrcVIO use multi-view optimization over the object states. The performances of OrcVIO and CubeSLAM are similar since both rely on point and bounding-box measurements to optimize the object states. CubeSLAM is better than OrcVIO in terms of 3D IoU, possibly because OrcVIO does not model dynamic objects. Table II also shows that OrcVIO is slightly better than CubeSLAM according to the TE odometry metric. In contrast with CubeSLAM, OrcVIO uses inertial residuals in addition to the geometric and object residuals.

In Table III, we compare the Precision and Recall of OrcVIO on the KITTI raw sequences (2011_09_26_00XX, XX = ) against a single-view, end-to-end object estimation approach (SubCNN ), and a visual-inertial object detector (VIS-FNL ). The first six rows are the Precision and Recall associated with different translation error (row) and rotation error (column) thresholds, whereas the last 3 rows ignore the rotation error. The results demonstrate that OrcVIO retrieves a reasonable amount of the ground-truth objects and provides a high-quality object map. When both rotation and translation errors are considered (first six rows), OrcVIO is better than SubCNN, since the latter does not rely on temporal object tracking. OrcVIO is comparable with VIS-FNL, even though VIS-FNL uses multiple object hypotheses while OrcVIO only keeps one object state. OrcVIO outperforms SubCNN and VIS-FNL when only translation error is taken into account (last three rows), which suggests that the object position estimates are accurate but the orientation estimates could be improved.

We evaluate the RMSE of the IMU-camera trajectory estimation in Table IV. OrcVIO is compared with two visual object SLAM methods: a monocular visual SLAM that integrates spherical object models to estimate the scale via bundle-adjustment (Object BA ), and CubeSLAM . Table IV shows that OrcVIO outperforms Object BA because spheres are a very coarse shape representation, compared to our ellipsoid and semantic keypoint representation, and thus Object BA cannot maintain the object scales as accurately as OrcVIO. The use of inertial data in OrcVIO leads to superior results as well. OrcVIO also outperforms CubeSLAM, possibly due to the incorporation of a zero-velocity update (Sec. IX-F), which is critical in driving datasets with frequent stops.

VII-D UCSD Campus Dataset Results

We also evaluated OrcVIO using real data collected with two commercial VIO sensors on UCSD’s campus. The results are qualitative due to the lack of ground truth information.

First, we used Intel RealSense D435i with image frequency of 30 Hz, image resolution of $640\times 480$, IMU frequency of 200 Hz in an indoor lab scene with chairs and monitors as shown in Fig. 7. The estimated sensor trajectory and reconstructed object map by OrcVIO are shown in the figure. A video demo can be found in the Supplementary Material. The results demonstrate that OrcVIO can map object instances from different categories and operates at real-time speed in a cluttered indoor scene. Since OrcVIO does not currently have a loop-closing mechanism for object re-identification, objects reappearing after getting lost will be mapped twice. Thus, there are more reconstructed chairs in Fig. 7 than in the reality.

Semantic keypoint detection is challenging due to occlusion, viewpoint change, and the lack of distinctive features on the monitors as shown in Fig. 7 (b). To handle the monitor class succesfully, we decreased the weight of the semantic keypoint residual in the object Levenberg-Marquardt optimization (17). Although removing the reliance on semantic keypoints leads to worse object orientation estimation, it allows OrcVIO to work with bounding-box detections only. This simplifies the front-end to an object detector and tracker and makes the algorithm more efficient and easier to deploy on resource-constrained robots. We release code for both the full OrcVIO algorithm and the OrcVIO-Lite version in the Supplementary Material.

Finally, we used an INDEMIND Binocular Visual-Inertial Camera to run OrcVIO outdoors with images at 25 Hz with resolution of $640\times 400$ and IMU measurements at 200 Hz. The sensor initially observes bikes and chairs, and then makes a transition into a parking structure, as shown in Fig. 8 (b), (c). A video demonstrating the performance of OrcVIO on this dataset can be found in the Supplementary Material. The resulting object map is shown in Fig. 8 (a), demonstrating that OrcVIO is able to estimate object states from different categories in both indoor and outdoor scenes. This experiment also shows that OrcVIO can handle large illumination changes, transitioning from direct outdoor sunlight to dim lighting inside the parking structure.

VIII Conclusion

This paper presented a unified formulation of ego-motion and object pose and shape estimation and developed a real-time simultaneous localization and object mapping algorithm. OrcVIO provides computationally constrained robot platforms with the ability to understand their surroundings at geometric and semantic levels, which may enable further capabilities such as object-level loop closure in challenging environments, collaborative semantic SLAM, and object interaction. Estimating object motion and performing collaborative mapping and loop closure are promising directions for future research.

IX Appendix

Similarly, approximating ${}_{I}\mathbf{T}$ using ${}_{I}\hat{\mathbf{T}}$ and ${}_{I}\boldsymbol{\xi}$ leads to:

Equating the two expression above, we get:

Next, we derive the expressions in (9). Note that:

where $(\mathbf{I}_{4}+\boldsymbol{\xi}_{\times})(\mathbf{I}_{4}-\boldsymbol{\xi}_{\times})=\mathbf{I}_{4}-\boldsymbol{\xi}_{\times}+\boldsymbol{\xi}_{\times}-(\boldsymbol{\xi}_{\times})^{2}\approx\mathbf{I}_{4}$ by discarding high-order terms in $\boldsymbol{\xi}$, and we used (7.159) in [73, Ch.7] in the last equality. Applying the chain rule to (8):

IX-B Proof of Proposition 2

IX-C Proof of Proposition 3

Given a hyperplane $\underline{\mathbf{b}}=[\mathbf{b}^{\top}\,-b_{h}]^{\top}$ and an origin-centered ellispoid as a dual quadric $\mathbf{Q}^{*}=\operatorname*{diag}(\mathbf{U}^{2},-1)$, there are two hyperplanes parallel to $\underline{\mathbf{b}}$ that are tangent to the ellipsoid: $\underline{\mathbf{t}}=\begin{bmatrix}\mathbf{b}^{\top}&\pm\sqrt{\mathbf{b}^{\top}\mathbf{U}^{2}\mathbf{b}}\end{bmatrix}^{\top}$ and the signed distance between $\underline{\mathbf{b}}$ and the closest tangent hyperplane is:

Given a hyperplane $\underline{\mathbf{b}}=[\mathbf{b}^{\top}\,-b_{h}]^{\top}$ and an origin-centered ellispoid as a dual quadric $\mathbf{Q}^{*}=\operatorname*{diag}(\mathbf{U}^{2},-1)$, the Jacobian of the distance $d(\underline{\mathbf{b}},\mathbf{U})$ in (44) with respect to $\underline{\mathbf{b}}$ is:

We rewrite $d(\underline{\mathbf{b}},\mathbf{U})$ explicitly in terms of $\underline{\mathbf{b}}$ by replacing $b_{h}=-\left[\mathbf{0}^{\top}\ 1\right]\underline{\mathbf{b}}=-\underline{\mathbf{0}}^{\top}\underline{\mathbf{b}}$:

Taking the derivative using the product rule leads to:

The derivative of $\frac{1}{\|\mathbf{b}\|}$ is:

The derivative of $d_{1}(\underline{\mathbf{b}},\mathbf{U})$ is:

Putting the derivatives in (45) leads to:

We proceed with the proof of Proposition 3. Note that ${}^{b}\mathbf{e}(\mathbf{x},\mathbf{o},{}^{b}\mathbf{z})=d(\underline{\mathbf{b}},\operatorname*{diag}(\mathbf{u}+\delta\mathbf{u}))$ in (44) with $\underline{\mathbf{b}}={}_{O}\mathbf{T}^{\top}{}_{C}\mathbf{T}^{-\!\top}\mathbf{P}^{\top}{}^{b}\underline{\mathbf{z}}$. The expression for $\frac{\partial{}^{b}\mathbf{e}}{\partial\underline{\mathbf{b}}}$, evaluated at $\underline{\hat{\mathbf{b}}}$ and $\hat{\mathbf{U}}$, follows directly from Lemma 2. The expressions for $\frac{\partial\underline{\mathbf{b}}}{\partial{}_{C}\boldsymbol{\xi}}$ and $\frac{\partial\underline{\mathbf{b}}}{\partial{}_{O}\boldsymbol{\xi}}$ are obtained using the perturbations ${}_{C}\mathbf{T}\approx{}_{C}\hat{\mathbf{T}}(\mathbf{I}_{4}+{}_{C}\boldsymbol{\xi}_{\times})$ and ${}_{O}\mathbf{T}\approx{}_{O}\hat{\mathbf{T}}(\mathbf{I}_{4}+{}_{O}\boldsymbol{\xi}_{\times})$, (7.159) in [73, Ch.7], and dropping second-order perturbation terms:

IX-D Proof of Proposition 4

Using that $\boldsymbol{\omega}_{\times}^{2n+1}=(-1)^{n}\|\boldsymbol{\omega}\|^{2n}\boldsymbol{\omega}_{\times}$ for $n\geq 0$:

The derivation for $\mathbf{H}_{L}(\boldsymbol{\omega})$ is equivalent. ∎

The result follows by integrating the terms of the Taylor series of $\exp(t\boldsymbol{\omega}_{\times})=\sum_{n=0}^{\infty}\frac{t^{n}\boldsymbol{\omega}_{\times}^{n}}{n!}$ and since $\mathbf{J}_{L}(\boldsymbol{\omega})$ and $\mathbf{H}_{L}(\boldsymbol{\omega})$ are well defined by Lemma 3. ∎

To obtain (4), we compute the solutions to the linear time-invariant (LTI) ordinary differential equations (ODEs) in (VI-A). With $\boldsymbol{\omega}={}^{i}\boldsymbol{\omega}_{k}-\hat{\mathbf{b}}_{g,k}$ and $t\in[0,\tau_{k})$, the solution to:

is ${}_{I}\hat{\mathbf{R}}(t)={}_{I}\hat{\mathbf{R}}_{k}\exp(t\boldsymbol{\omega}_{\times})$ and, hence:

Similarly, with $\mathbf{a}={}^{i}\mathbf{a}-\hat{\mathbf{b}}_{a}$, $t\in[0,\tau_{k})$, and initial condition ${}_{I}\hat{\mathbf{v}}(0)={}_{I}\hat{\mathbf{v}}_{k}$, the solution of the LTI ODE for ${}_{I}\hat{\mathbf{v}}$ in (VI-A) is:

where the second equality uses Lemma 4. Hence, ${}_{I}\hat{\mathbf{v}}_{k+1}^{p}={}_{I}\hat{\mathbf{v}}(\tau_{k})$ satisfies the second expression in (4). Also, by Lemma 4, for $t\in[0,\tau_{k})$ with initial condition ${}_{I}\hat{\mathbf{p}}(0)={}_{I}\hat{\mathbf{p}}_{k}$, the solution of the LTI ODE for ${}_{I}\hat{\mathbf{p}}$ in (VI-A) is:

Hence, ${}_{I}\hat{\mathbf{p}}_{k+1}^{p}={}_{I}\hat{\mathbf{p}}(\tau_{k})$ satisfies the third expression in (4). Finally, $\hat{\mathbf{b}}_{g}(t)=\hat{\mathbf{b}}_{g}(0)=\hat{\mathbf{b}}_{g,k}$ and $\hat{\mathbf{b}}_{a}(t)=\hat{\mathbf{b}}_{a}(0)=\hat{\mathbf{b}}_{a,k}$ for all $t\in[0,\tau_{k})$. The IMU pose history in (4) is updated by adding the pose $({}_{I}\hat{\mathbf{R}}_{k},{}_{I}\hat{\mathbf{p}}_{k})$ to the sliding window and dropping the oldest pose ${}_{I}\hat{\mathbf{T}}_{k-W}$.

IX-E Proof of Proposition 5

To integrate (55), observe that from $\boldsymbol{\omega}_{\times}^{3}=-\|\boldsymbol{\omega}\|^{2}\boldsymbol{\omega}_{\times}$:

By integrating the terms of the Taylor series of $s\exp(s\boldsymbol{\omega}_{\times})$ and using $\boldsymbol{\omega}_{\times}^{3}=-\|\boldsymbol{\omega}\|^{2}\boldsymbol{\omega}_{\times}$, we can verify that:

where $\Delta(t)$ is defined in (29). The second integral in (59) can be computed using $\mathbf{a}_{\times}\boldsymbol{\omega}_{\times}=\boldsymbol{\omega}\mathbf{a}^{\top}-(\mathbf{a}^{\top}\boldsymbol{\omega})\mathbf{I}_{3}$, $\exp(s\boldsymbol{\omega}_{\times})\boldsymbol{\omega}=\boldsymbol{\omega}$, $\exp(s\boldsymbol{\omega}_{\times})\exp(s\boldsymbol{\omega}_{\times})^{\top}=\mathbf{I}_{3}$, and Lemma 4, leading to:

where (63) and (65) follow from Lemma 4. To integrate (64), we use that:

IX-F Zero-Velocity Update

Zero-velocity conditions are frequently encountered in autonomous driving and autonomous flight applications, and thus the ability to determine whether the robot is static is important for reducing drift in VIO estimation . In OrcVIO zero-velocity is detected similarly as in by using pseudo zero inertial measurements and then checking the velocity magnitude. For the zero-velocity update, we need to compute the residuals and the Jacobians of the inertial measurements with respect to the state. Based on (VI-A) the residuals are:

The corresponding Jacobians are presented as follows:

References