Consensus-Informed Optimization Over Mixtures for
Ambiguity-Aware Object SLAM

Object pose estimation considering symmetry or ambiguity is becoming increasingly popular in computer vision and robotics. A natural and ordinary idea is to represent symmetry in an unambiguous manner (e.g. [12, 13]). Rad and Lepetit [12] restricts the pose of a rotationally symmetric object within a unambiguous range. Any pose measurement beyond the range is returned as its counterpart.

Another popular class of methods make multi-hypothesis pose predictions via view matching. The visual appearances of an object from arbitrary viewpoints are compared against the captured image. Multiple poses are returned when there exist one-to-many view-pose correspondences. Manhardt et al. [2] retrieve 6D poses for ambiguous objects based on learned multi-hypothesis view-pose mapping. Sundermeyer et al. [3] encode visual appearances using an auto-encoder and perform similarity checks in the feature space.

Other ambiguity resolving strategies incorporate regressing pose distributions [4] and training symmetry predictors [5]. Beyond that, multi-shot pose estimators are developed to gradually recover ambiguous object poses. The predicted poses are tracked and updated from successive observations by non-Gaussian filters [6, 7, 8] or a learned tracking network [9].

Our primary interest is to tolerate and utilize the local, potentially ambiguous, object pose measurements to build a globally consistent world representation.

We review the three most relevant methods for multi-modal inference under measurement ambiguity.

A natural solution would be to select the most likely hypothesis for each ambiguous measurement and solve an uni-modal inference problem, which we refer to as single-hypothesis method (e.g. nearest neighbor for data association [14]). With the loss of information in discarded hypotheses, it can fail quietly with noise- or outlier-corrupted measurements.

Multi-hypothesis tracking (MHT) based methods[15, 16, 17] maintain most probable hypotheses and solve the corresponding set of uni-modal inference problems. B-iMAP [16] is the first smoothing-based incremental MHT SLAM algorithm, where probable hypotheses are selected analytically. MH-iSAM2 [17] utilizes the Bayes tree and Hypo-tree to perform efficient incremental multi-modal inference with rule-based hypothesis pruning. MHT-based methods provide multiple state estimations including the temporary optimum. However, the rapid hypothesis accumulation necessitates hypothesis pruning. Designing an optimality-guaranteed pruning strategy still remains challenging.

Max-mixtures model [10] provides an efficient solution to point estimates in multi-modal inference. However, optimization over max-mixtures can be trapped in a local minimum with a poor initialization. Wang and Olson [18] propose to solve the max-mixtures model with stochastic gradient decent (SGD) and obtain increased robustness to poor initializations on pose graph optimizations. However, SGD is sensitive to learning rate and lacks global understanding of dominant modes. Instead of local search, we use the consensus of pose hypotheses to guide optimization towards the global optimum, to obtain a robust and efficient solution to pose ambiguity corrupted SLAM.

SLAM systems utilizing object 6D pose representations (e.g. [19, 20, 21, 22]) have great promise for robot-object interactions. But the initializations of landmark poses, especially for ambiguous objects is a challenging but under-examined problem. One way of handling ambiguities in single-shot observations is to delay object registrations until robust 6D pose estimations are available (e.g. [19, 20]). Another approach is to improve the one-shot pose estimates. NodeSLAM [22], for instance, trains a coffee mug rotation estimator for reliable mug orientation initializations.

Instead of relying on the first measurement to provide satisfactory initialization or delaying object registrations, we enable the flexibility to adjust the landmark initializations during the incremental multi-modal inference process.

While max-mixtures was initially introduced as an approximation of sum-mixtures, we show that it can be independently derived by variable elimination of the MAP. With the robot and object landmark poses, $X,L$, being of key concern in SLAM, we can marginalize out the hypothesis selections $H$ from (2) with the max-product algorithm:

Under the Bayes rule, the joint posterior in (3) can be factored as:

with independent joint priors and the $H$ irrelevant priors taken out of the $\max$ operator.

Assuming the measurements are independent, the joint probabilities can be decomposed as the product of factors (prior factors on $X,L$ omitted):

where $\phi(x_{t-1},x_{t})$ represents odometry factors that are irrelevant to pose hypotheses $H$, and $\psi(x_{t},l_{j},h_{tj})$ corresponds to landmark pose measurements, which following the joint probabilities in (4) takes the form of:

Since the hypotheses $h_{tj}$ are conditionally independent from one another given $x_{t}$ and $l_{j}$, the $\max$ operator can be pulled inside the product operator in front of $\psi$ in (5), yielding the final form of the landmark-measurement factor:

Assuming the measurement model for each pose hypothesis is Gaussian, we obtain a max-mixture type factor, in which $p(z_{tj}|x_{t},l_{j},h_{tj})$ represents the Gaussian observation likelihood for $h_{tj}=1\sim N$ and $p(h_{tj})$ denotes the discrete hypothesis weights. The $\max$-operator selects the best hypothesis given the latent variables $X,L$.

Finally, the formulation for the max-mixtures model can be expressed as:

With Gaussianity locally preserved by the selected mixture components in $\phi(x_{t},l_{j})$, the max-mixtures model is amenable to efficient solution by nonlinear least squares optimization. In this study, we use the incremental SLAM framework iSAM2 [11] to solve (8) for better scalability.

Meanwhile, after hypothesis decisions, other subdominant modes are still kept alive in the latent space. The best components will be re-evaluated at each optimization step [10]. Therefore, all the pose hypotheses are implicitly tracked forward and the exhaustive search over hypothesis space is replaced by the local hypothesis selections.

However, efficiency of the max-mixtures model is achieved at the expense of potential local optimality. As the number of ambiguous observations increases, modes accumulate rapidly in the posterior distribution. Without a good initial value, the solution can easily get trapped in an incorrect mode. Therefore, we propose to use consensus over pose hypotheses to directly guide optimization over max-mixtures into the global optimum.

A good initial value is typically unachievable for an ambiguous landmark variable. As the first one-shot object observation has multiple hypotheses, it is usually hard to determine which one represents the real pose. Even worse, a sub-dominant pose hypothesis could become dominant later. An arbitrary or a temporarily optimal initialization may trap the solution in a local minimum. In general, it is difficult for the solution to escape the incorrect mode and converge to the global optimum.

Therefore, we perform dynamic landmark re-initialization, forcing some variables in the least-square optimization to “restart” inside the dominant mode. The procedure is summarized in Alg. 1

An object SLAM experiment is conducted using playing card landmarks, modelling the symmetric property of ordinary objects. As illustrated in Fig. 5, we equip the SwarmRobot [25] with a forward-pointing ZED camera for visual odometry [26], and a downward-looking (30$\deg$ to the ground) Blackfly [27] camera for image taking. 40 playing cards are placed on the ground with a 5$\times$8 configuration. They are drawn from 22 classes (number+suit defines a class), where 4 cards are unique and the rest appear in pairs. The robot is remotely controlled to follow a lawnmower-patterned round-trip path. Each card is first observed from one view angle and revisited later from the opposite. Identical card appearances in the two encounters lead to ambiguities in pose estimations.

In the roughly 10min long test, 8522 odometric measurements (ZED odometry) and 17369 images are collected. The images with cards detected are illustrated in Fig. 5. We utilize the odometry and estimated relative card poses to gradually recover a world map consisting of robot trajectory and 6D card poses. The Vicon MoCap system [28] is employed to obtain the ground truth data.

A SIFT feature [29] based algorithm is developed using OpenCV for cards identification and pose estimation [30]. It is able to return the two centrally symmetric card pose hypotheses (Fig. 5). We design a number of “key-pose” criteria to filter out spurious pose measurements and use only key-poses in optimization. The measurement-card correspondences (data association) are inferred with the classic nearest neighbor approach. We adopt both the proposed max-mixture algorithm and a single-hypothesis system to solve the optimization. The latter incorporates a single-hypothesis estimator, that predicts the most likely card pose, and a uni-modal back-end optimizer.

As shown in Fig. 7, the single-hypothesis method fails catastrophically due to oblivion of the pose ambiguity. At the first card re-observation, i.e. JC at bottom left of Fig. 7, the estimator returns near-identical relative orientation compared to the card’s first encounter. However, the true relative orientation is reversed with robot viewing it from an opposite viewpoint. As a consequence, the inconsistency in JC pose understanding leads to a large deviation in robot localization, which corresponds to the separation point in Fig. 8. Even worse, the localization failure also induces disastrous performance in data associations. Many cards are registered more than twice in the world map.

On the contrary, the proposed algorithm maintains every pose hypothesis and managed to realize the pose consistency. It correctly handles the decision making in data associations and pose hypothesis selections. Loop closures at the JC re-observation and subsequent card revisits constantly adjust the trajectory and minimize estimation errors (Fig. 8). In this feature-scarce and ambiguity-rich environment, as Fig. 6 demonstrates, the max-mixture method still attains good localization and mapping performance.

We conduct a simulated SLAM experiment in the Unreal Engine [31] with 10 coffee mugs as landmarks (Fig. 9(a)). The mug model is adapted from the YCB data set and has been scaled by 50 folds to match the environment [32]. A mobile robot equipped with a monocular camera is integrated in the environment via the AirSim car simulator[33]. The robot odometry, relative poses to the mugs, and camera images are collected via AirSim to estimate the robot trajectory and mug poses. A synthetic pose estimator is created to detect ambiguous poses due to occlusion. In a handle-occluded case, as shown in Fig. 9(b), a relative pose measurement with three pose hypotheses will be added to the data. Two of the hypotheses are spurious and are derived by corrupting the ground truth pose with pre-determined rotations ($\pm$30 degrees around the vertical axis). The noise for perturbing those pose hypotheses is sampled from a Gaussian distribution in the tangent space about identity pose transformation. The default covariance, $\Sigma$, has diagonal entries of 0.2, 0.2, 0.02, 0.01, 0.01, and 0.01 and zeros on off-diagonal entries (we use the same ordering of Lie algebra components as [34]). If the handle is visible to the camera, the relative pose measurement will comprise a single hypothesis synthesizing the ground truth pose with noise. There are 857 time steps along the robot trajectory in total. 148 of them are associated with handle-visible detections while 119 of them have handle-occluded detections (see Fig. 9(c)). Note that the small number of detections reflects the sparsity of features in the environment and the large ratio between handle-occluded and handle-visible detections signifies the richness of pose ambiguity.

Fig. 10 shows the estimates at the final time step by four methods: single-hypothesis, max-mixtures, MH-iSAM2²²2We use the MH-iSAM2 implementation at https://bitbucket.org/rpl_cmu/mh-isam2_lib. The threshold number of tracked hypotheses $n_{\text{limit}}$ is set to 20 (default). The solution from the most probable hypothesis is presented., and max-mixtures with dynamic re-initialization. Single-hypothesis refers to permanently admitting a random hypothesis from each measurement with pose ambiguity. It causes large deviations in the trajectory since some measurements are purely represented by spurious hypotheses. Since the posterior distribution is highly multi-modal in the case considering all hypotheses, the max-mixture estimate can be trapped in local optima due to bad initial values. MH-iSAM2 is supposed to be more robust to initial values as it tracks multiple solutions; however, the hypothesis set for optimal estimate can be pruned even at the very early stage of trajectory, resulting in inaccurate estimates eventually. Intermediate estimates as the robot proceeds are compared to reveal the evolution of errors. Fig. 11 shows that max-mixtures with re-initializing maintains relatively low errors during the whole task.

To imitate real-world pose detectors with larger errors, we scale the covariance in the default pose perturbation model up and generate more datasets with a larger amount of noise. Table I shows estimation errors and runtime. Max-mixtures with re-initialization performs consistently well in those settings. Since MH-iSAM2 essentially solves a number of point-estimate problems, it is expected to be slower than others. Although re-initialization in our method is an extra step and causes overhead, it does not necessarily have a big impact on the overall runtime as better initial values can reduce the search time in optimization.

Is landmark re-init an over-treatment while a batch optimization step naturally supports full re-initialization? To address this question, we case study the runtime performance of landmark re-init. We build a factor graph with 1000 robot pose variables connected in series. A number of landmark pose variables are connected with them through randomly generated edges. The factor generation and variable initialization are randomized. We vary the numbers of landmarks and edges to carry out a series of runtime tests.

We solve the optimizations with iSAM2 [11] and a batch optimizer (Gauss-Newton). At the last step of iSAM2 optimization, a random landmark variable is re-initialized. We time both the normal iSAM2 steps and the last re-init iSAM2 step for evaluation. The last batch optimization step is also timed for a fair comparison with the re-init iSAM2 step. We repeat the optimizations 10 times for each parameter combination, and average the time costs.

In summary, the steps in Fig. 12 respectively incorporate:

• •

  iSAM2 step: factor graph update + estimate computation

• •

  iSAM2 step w/ re-init: landmark re-init + iSAM2 step

• •

  batch step: factor graph re-construction + estimate computation

We learn from the results that an occasional re-init iSAM2 step does not bring about too much overhead as long as the factor graph is not densely connected. In the object-based SLAM context we seldom encounter the scenarios where a large number of objects are concurrently observed at all times. That said, we can conclude that in most cases iSAM2 with re-init is much more efficient than a batch update.