Symmetry and Uncertainty-Aware Object SLAM
for 6DoF Object Pose Estimation

Since the keypoint $\bm{u}_{i}$ is the expected value of the distribution of 2D coordinates with probability mass given by the values of $p_{i}$, it is straightforward to estimate an uncertainty measure by the covariance of this distribution with the second moment about the mean

However, without any particular criteria for the covariance, there is nothing to enforce that the uncertainty actually captures the true error of the prediction. To this end, we propose to use a Gaussian maximum-likelihood estimator (MLE) loss to jointly optimize the keypoint coordinates as well as the covariance:

where $\bm{u}^{*}_{i}$ is the ground truth keypoint coordinate. From a high-level perspective, the first term enforces that the covariance bounds the true error of the prediction, while the second prevents it from becoming too large. This way, the network can predict its own uncertainty in the form of a Gaussian covariance matrix, which is trained to tightly bound the true error of the estimated keypoint.

While our network predicts a total of $N$ keypoints, only a subset of these, $\mathcal{K}(\ell)\subset\{1,2,\ldots,N\}$, are valid for a particular object $\ell$. Furthermore, considering a single image, only a subset of keypoints $\mathcal{B}\subseteq\mathcal{K}(\ell)$ lie within the bounding box for object $\ell$ (note that occluded keypoints are still predicted). However, during deployment, while $\mathcal{K}(\ell)$ is known from the object class and keypoint labeling, it may be impossible to know which keypoints lie within the detected bounding box. For this reason, we add another head onto the network to predict a sigmoid vector $\bm{m}\in[0,1]^{N}$, which is trained to estimate the ground-truth binary mask $\bm{m}^{*}\in\{0,1\}^{N}$, where $m^{*}_{i}=1$ if $i\in\mathcal{B}$ and 0 otherwise (see Fig. 3 for the architecture). Thus, for a single object, in a single image, the full loss becomes

where $\mathrm{BCE}(.)$ is the binary cross entropy loss function. For the rest of the paper, to simplify notation, we will denote $k\in\{1,2,\ldots,K\}$ as the indices for keypoints which pass the ground-truth mask $\bm{m}^{*}$ for training (i.e., the next section) or the estimated mask $\bm{m}$ (as well as the known $\mathcal{K}(\ell)$) for deployment in the SLAM system (Sec. 3.2).

Since we want to efficiently track the keypoints over time during deployment, it is convenient to obtain keypoint predictions that have a symmetry hypothesis that is consistent with the 3D scene. Inspired by [19], we opt to include $N$ extra channels as input to the keypoint network which contain a prior detection of the object’s keypoints. As shown in Fig. 2, during deployment in the SLAM system, the prior keypoint detections come from projecting the 3D keypoints from the global object frame into the current image once the corresponding camera pose is found (i.e., the second pass). With this paradigm, there are two main issues to address: how to create training examples of the prior detections (since the SLAM system is not run during training), and how to detect the initial keypoints on symmetric objects when there is not yet an object pose estimate available. Here we describe the training scheme used to address these issues.

To create the training prior, we simulate a noisy prior detection that the SLAM system would create by projecting the 3D keypoints from the object frame into the image plane with a perturbed ground truth object pose $\delta\mathbf{T}{}^{C}_{O}\mathbf{T}^{*}$ (see the supplementary Sec. A about the notation). To further ensure that the network can learn to follow the prior detections for the symmetry hypothesis, we utilize the set of symmetry transforms $\mathcal{S}=\{{}^{O}_{S_{1}}\mathbf{T},{}^{O}_{S_{2}}\mathbf{T},\ldots,{}^{O}_{S_{M}}\mathbf{T}\}$ that we expect to be available for each object (discretized for objects with continuous axes of symmetry). Each ${}^{O}_{S_{m}}\mathbf{T}\in\mathcal{S}$, when applied to the object CAD model, makes the rendering look (nearly) exactly the same, and in practice, these transforms can be manually chosen fairly easily. Thus, when constructing a training example with a prior detection, we pick a random symmetry transform and apply it to the ground-truth object pose before doing the projection.

In order for the network to learn to predict initial keypoints on symmetric objects (when no prior is available), we only provide this simulated prior randomly for roughly half of the examples. Without the prior detection, however, the network is left up to its own devices to reason about the absolute orientation for the object – which is theoretically impossible for symmetric objects without special care. As opposed to the mirroring technique and additional symmetry classifier proposed by [28], we instead teach the network to deal with this issue with a simple criteria of choosing the keypoints that correspond to the symmetrically-valid pose that is closest to a canonical view where the front of the object faces the camera, and the top of the object faces the top of the image. We refer the reader to the supplementary material (Sec. B) for more details on this procedure.

The first step of our front end is to split the bounding boxes detected in the current image into two information streams – the first for asymmetric objects and first-time detections of symmetric ones, and the second for symmetric objects that already have 3D estimates. Again, we expect the symmetry information (i.e., symmetric or not) to be included with each object class. The first information stream sends the images, cropped at the bounding boxes, to the keypoint network without any prior to detect keypoints and uncertainty. These keypoints are then used to estimate the pose of each asymmetric object ${}^{C}_{O}\mathbf{T}_{\mathrm{pnp}}$ in the current camera frame by using PnP with RANSAC. These PnP poses are then used to coarsely estimate the current camera pose and then initialize objects which do not yet have 3D estimates. See the supplementary material Sec. C for more details on how this is done as well as more detailed behavior of the front end.

With a rough estimate of the current camera, we move onto the second information stream of the front end. We use the coarse estimate of the camera pose to create the prior detections for the keypoints of symmetric objects by projecting the 3D keypoints for these objects into the current image, and constructing the prior keypoint heatmaps for network input. After running the keypoint network on these symmetric objects, we store the keypoint measurements from both information streams for later use in the global optimization.

The global optimization step runs periodically to refine the whole scene (object and camera poses) based on the measurements from each image. Rather than reduce the problem to a pose graph (i.e., using relative pose measurements from PnP), we keep the original noise model of using the keypoint detections as measurements, which allows us to weight each residual with the covariance prediction from the network. The global optimization problem is formulated by creating residuals that constrain the pose ${}^{C_{j}}_{G}\mathbf{T}$ of image $j$ and the pose ${}^{G}_{O_{\ell}}\mathbf{T}$ of object $\ell$ with the $k$th keypoint

where $\Pi_{j,\ell}$ is the perspective projection function for the bounding box of object $\ell$ in image $j$. Thus the full problem becomes to minimize the cost over the entire scene

where $\bm{\Sigma}_{j,\ell,k}$ is the $2\times 2$ covariance matrix predicted by the network for the keypoint $\bm{u}_{j,\ell,k}$, $s_{j,\ell,k}\in\{0,1\}$ is a constant indicator that is 1 if the measurement was deemed an inlier before the optimization started and 0 otherwise, and $\rho_{H}$ is the Huber norm which reduces the effect of outliers during the optimization steps. Both $\rho_{H}$ and $s_{j,\ell,k}$ use the same outlier threshold $\tau$ , which is derived from the 2-dimensional $\chi^{2}$ distribution, and is always set to the 95% confidence threshold $\tau={5.991}$. Thus we do not need to manually tune the outlier threshold as long as the covariance matrix $\bm{\Sigma}_{j,\ell,k}$ can properly capture the true error of keypoint $\bm{u}_{j,\ell,k}$.

While our design is agnostic to the choice of keypoint, to reduce the number of channels that the network needs to predict, we created a set of rules to annotate keypoints manually in such a way that each keypoint can be applied to multiple object instances, and the same rules can be applied to both the YCB-Video and T-LESS dataset. We manually label the 3D CAD models for both datasets, and project the keypoints from 3D to 2D to create the ground-truth keypoints described in Sec. 3.1. We refer the reader to the supplementary material Sec. D for more details on how we annotated the keypoints.

We implemented the keypoint network in PyTorch [24]. For all training, we used the Adam optimizer [11] with a learning rate of $10^{-3}$. For the YCB-Video dataset, we utilized real training data provided along with the official 80k synthetic images. Due to the high redundancy in the real training data, we used only every 5th image. We trained on this dataset for 60 epochs using a batch size of 24 with randomized backgrounds for the synthetic dataset as well as randomized bounding boxes, color, and image warping. For the T-LESS dataset, there are only real training images of single objects on a dark background , so for the synthetic data we opted to use the physics-based pbr rendered data provided by [8]. For both the real and pbr splits we augment the examples with randomized backgrounds, bounding boxes, color, and warping, as well as randomly pasted objects for the real data only – since it only contains images of isolated objects. We trained the TLESS model for 89 epochs with a batch size of 8, which was smaller than that for YCB-Video due to the higher image resolution of the pbr data.

Our SLAM system is implemented in Python. The GPU is only used for network inference while all other operations are performed on the CPU. All optimizations are implemented using Python wrappers for the g2o library [13]¹¹1https://github.com/uoip/g2opy, besides PnP, which is done using the Lambda Twist solver [27] with RANSAC²²2https://github.com/midjji/pnp. Our front-end tracking works on every incoming frame, while the back-end runs every 10th frame. Note that the testing sequences for both datasets are already provided as keyframes, so no keyframing procedure is needed. While for actual deployment it is ideal to run the back-end graph optimization on a separate worker thread, this would make reproducing the exact results impossible due to randomness in the operating system’s allocation of resources between the two threads. In order to make the results reproducible, we simply execute both the front-end and back-end on the main thread for evaluation. Our front-end tracking can typically run at 11Hz on our desktop with a GTX 1080Ti graphics card, and the back-end can run at an average speed of 2Hz.

The first ablation study is to run our same system without the prior detection. The results drop slightly, but this is expected on this dataset where only 5 out of 21 objects are considered symmetric, and only the bowl displays a continuous rotational symmetry. In the next section, we will see that the prior detection actually makes a much bigger difference on the T-LESS dataset, where most of the objects are symmetric, and the camera rotates many times completely around the scene – whereas the camera motion in YCB-Video is much simpler.

For the next ablation in Table 1, “manual cov”, we manually tune a weight to replace the covariance in the SLAM system’s residuals and outlier rejection mechanism. Here, we found that the weight corresponding to $2\times$ the average predicted standard deviation of the network (which was about 2.5 pixels) achieved the best scores. As observed, the results dropped significantly compared to using a network predicted covariance.

For the ablation labeled “no MLE loss”, we trained a network with the same procedure, but replaced the MLE loss with a fixed-variance loss with variance regulation similar to that used by the popular human pose estimation [21]. As observed, when placed in the SLAM system, the results are significantly lower than that with our network trained with the MLE loss. The qualitative results of this experiment are also in Fig. 4.

Beyond the accuracy of the SLAM system with this change, we have also tested the accuracy of the predicted covariance itself. To do so, we ran both of the networks (with and without MLE loss) on a separate set of simulated YCB-Video objects (the pbr data which was not used in training), which has perfect ground truth for the keypoints. Here, we ran the networks with the ground truth bounding boxes and no prior detection. To evaluate the accuracy of the predicted covariance, we plotted the keypoint error against the predicted standard deviation of the network. Ideally, the error will always lie above the cone $e_{r}<3\sigma$ if $e_{r}$ is the scalar $x$ or $y$ component of the error residual of the keypoint prediction. The results of this experiment can be viewed in Fig. 5. As observed, the network trained with the MLE loss has much more of the errors within the 3$\sigma$ cone. In fact, 91.0% of the data points on the left in Fig. 5 pass a 99% confidence $\chi^{2}$ test while only 7.1% pass from the points on the right. This shows that the predicted uncertainty describes the actual error distribution well (besides some expected outliers due to heavy occlusion and symmetry), and including the MLE loss is crucial to achieve this.

For the final ablation in Table 1, we ran just our single view network and compared the accuracy. Specifically, for each view we just ran PnP and refined it using the same procedure as Eq. 5, but with only one fixed camera pose per optimization. Clearly the full SLAM system is more accurate. It is interesting to note that the results for single view are actually more accurate than the SLAM results using the manual covariance or the fixed-variance network. This is most likely due to the fact that incorrect covariance in our SLAM system can cause the outlier rejection mechanism to be unreliable, and outliers can then pull the object pose in an incorrect direction and hurt the accuracy for all views despite the fact that most of the keypoints are correct.

The effect of initializing the camera poses with the poses provided by the dataset was minor in this experiment. Using the given camera poses the system achieved a 90.5 AUC of ADD-S score, while the system with the estimated camera poses scored the 90.3 shown in Table 1. This shows that the estimated camera poses are very accurate on this dataset.

To test the sensitivity to the training data, we train it on only the small real training split, which contains 1,231 images of each object on a dark background. From Table 2 we observe that, even with this small amount of data, we still beat all of the state-of-the-art methods besides CosyPose – all of which used large amounts of synthetic data on top of the real data. This shows the ability of our method to work with a limited amount of data which does not even cover all orientations of the objects.

On the T-LESS dataset, where most of the objects are symmetric in some way, the 63.7 recall score drops to 16.2 in Table 2 when the prior detection is removed. This shows that the prior detection is crucial for tackling these challenging T-LESS objects when the camera is orbiting around their axes of symmetry multiple times. Without the prior detection, the SLAM system’s outlier rejection simply rejects most of the keypoint measurements on the symmetric objects, as they do not correspond to the same 3D location. Fig. 6 also includes some qualitative results of this experiment.

Here again we set the covariance in the SLAM system’s residuals to a manually-tuned weight. The result in this case drop to a 13.8 recall, which further substantiates the usefulness of our covariance estimate in the SLAM system. Furthermore, we found that the optimal weight for this dataset was much larger than that for YCB-Video, which is not surprising, but shows that removing the need to manually tune weights by using the predicted covariance is a useful property of our system.

In this case, the single view result in Table 2 outperformed that from the SLAM system when it either used a manual covariance weight or no prior detections. Since the single-view results use no prior detection, this shows that the keypoints considered independently for each view are reasonable, while the prior detection is crucial for tracking them across time.