Paper deep dive
Semantic Semi-Incremental Data-Association-Free Object SLAM
Yihao Zhang, Jungseok Hong, John J. Leonard
Intelligence
Status: not_run | Model: - | Prompt: - | Confidence: 0%
Entities (0)
Relation Signals (0)
No relation signals yet.
Cypher Suggestions (0)
No Cypher suggestions yet.
Abstract
Abstract:Data association between landmark measurements and landmark variables has long been a central challenge in SLAM, as estimation accuracy depends critically on associating measurements with the correct landmark variables. Recent advances in deep learning have created new opportunities for the problem; data association can now leverage not only positional measurements but also semantic information about object landmarks, such as class labels from neural object detectors and feature vectors from visual foundation models. In this paper, we present a generalized data-association-free SLAM framework that jointly estimates data associations, robot poses, landmark positions, and landmark semantics from odometry, and positional and semantic measurements of landmarks. The proposed framework (i) creates a synergy between data association and landmark semantics estimation; (ii) adopts a semi-incremental estimation scheme for improved accuracy and computational efficiency; and (iii) provides a principled justification, guidelines, and heuristics for landmark-number estimation, improving the interpretability and practical usability of the framework. The proposed framework and algorithms are evaluated on synthetic and real-world datasets with two types of semantic information, class labels and real-valued feature vectors, and demonstrate superior performance compared to strong baselines.
Tags
Links
- Source: https://arxiv.org/abs/2607.23384v1
- Canonical: https://arxiv.org/abs/2607.23384v1
Trouble viewing inline? Open PDF directly →
Full Text
94,131 characters extracted from source content.
Expand or collapse full text
Semantic Semi-Incremental Data-Association-Free Object SLAM Yihao Zhang1∗, Jungseok Hong1∗, John J. Leonard1 1Yihao Zhang, Jungseok Hong, and John Leonard are with the Computer Science and Artificial Intelligence Laboratory, Massachusetts Institute of Technology (yihaozh, jungseok, jleonard@mit.edu). * indicates equal contribution. Abstract Data association between landmark measurements and landmark variables has long been a central challenge in SLAM, as estimation accuracy depends critically on associating measurements with the correct landmark variables. Recent advances in deep learning have created new opportunities for the problem; data association can now leverage not only positional measurements but also semantic information about object landmarks, such as class labels from neural object detectors and feature vectors from visual foundation models. In this paper, we present a generalized data-association-free SLAM framework that jointly estimates data associations, robot poses, landmark positions, and landmark semantics from odometry, and positional and semantic measurements of landmarks. The proposed framework (i) creates a synergy between data association and landmark semantics estimation; (i) adopts a semi-incremental estimation scheme for improved accuracy and computational efficiency; and (i) provides a principled justification, guidelines, and heuristics for landmark-number estimation, improving the interpretability and practical usability of the framework. The proposed framework and algorithms are evaluated on synthetic and real-world datasets with two types of semantic information, class labels and real-valued feature vectors, and demonstrate superior performance compared to strong baselines. I Introduction Object SLAM builds a map with objects as landmarks. It is a step beyond traditional geometry-only SLAM toward semantic scene understanding. Objects are typically detected by a neural module, which also provides semantic information about them. Recent work [1, 2, 3, 4] explores the use of such semantic information to facilitate data association of landmark measurements. Our work advances this line of research. In particular, building on the data-association-free landmark-based SLAM (DAF-SLAM) framework, we develop a semantic data-association-free object SLAM framework that jointly estimates robot poses, landmark positions, landmark semantics, data associations, and the number of landmarks given odometry measurements, landmark position measurements, and landmark semantic measurements. DAF-SLAM [5] studies a minimal problem in which semantic information is not available; therefore, data association has to be inferred solely from odometry and landmark position measurements. The authors recognize that when data association is unknown, the number of landmarks in the environment is also unknown and may be as large as the number of landmark measurements received. They split the problem into an inner problem that estimates the data associations given the number of landmarks and an outer problem that estimates the number of landmarks, assuming the inner problem can be solved. Various algorithms have been developed to solve these sub-problems. While DAF-SLAM establishes a promising foundation, extending it to an efficient and accurate object SLAM framework introduces several important challenges. First, the DAF-SLAM framework does not exploit semantic information that is often available from modern object detectors and visual foundation models. Second, its batch optimization strategy can suffer from accumulated odometry drift when pose estimates are initialized from odometry over long trajectories. Third, a deeper understanding of the landmark-number estimation mechanism is desirable for formulation interpretation and parameter selection. In this paper, we extend the DAF-SLAM framework and address the aforementioned challenges through four main contributions. (i) We provide a principled analysis of the landmark-number estimation mechanism based on local optimality conditions. (i) We develop a semi-incremental (or block-incremental) processing scheme inspired by the classical divide-and-conquer strategy to reduce drift and improve computational efficiency. (i) We integrate semantic information into the estimation process, enabling joint inference of data associations and landmark semantics. The semantic measurements may take the form of either real-valued feature vectors or one-hot vectors, which are compatible with the outputs of visual foundation models and object detectors. (iv) We evaluate the proposed algorithms extensively on synthetic datasets, a real-world dataset using an object detector to extract semantic measurements, and a real-world underwater dataset using a visual foundation model to extract semantic measurements. The proposed algorithms outperform strong baselines across these diverse datasets. Together, these contributions advance the development of a more general and efficient data-association-free object SLAM framework. I Related Work Research on data association in SLAM has a long history. Neira and Tardós [6] and Montemerlo et al. [7] provide an early review of data association methods in SLAM, including maximum likelihood, joint compatibility branch and bound, combined constraint data association, iterative closest point, and multi-hypothesis tracking. Traces of these methods can still be seen in modern geometry-only and object SLAM methods. For example, object SLAM pipelines [3, 2] belong to the maximum likelihood category. They compute the Mahalanobis distance, derived from maximum likelihood estimation, to handle the positional component, and the cosine similarity (or inner product) to handle the semantic feature-vector component of data association. Iterative-style methods such as Max-Mixture [1], DC-SAM [8], and DAF-SLAM [5] are, in principle, similar to iterative closest point or coordinate descent. However, how variables are split can affect the algorithm performance. Both Max-Mixture and DC-SAM split the variables into discrete (i.e., data associations) and continuous (i.e., landmark positions and robot poses) variables, and solve them separately in an alternating fashion. Whereas DAF-SLAM considers a smarter split, where one step solves data associations and landmark positions, and another step solves robot poses and again landmark positions. Mature algorithms exist to solve both sub-problems, yielding improved performance. A recent approach [9] solves the discrete-continuous problem with semi-definite relaxation but has limited scalability. Although still running maximum likelihood or maximum a posteriori estimation, a distinct line of object SLAM work [10, 11, 12] models data association as a Dirichlet process so that landmarks with more associated measurements are more likely to be associated again. Another line of object SLAM work [13, 4] takes an expectation over data associations in an expectation-maximization style to eliminate the discrete data association variables in the inference. When building object SLAM systems, researchers often employ sensor-dependent front-end association methods. For example, SLAM++ [14] uses image-based projective data association; room numbers on door signs are used for association in [15]; ODAM [16] (mapping-only) makes associations with a graph neural network that takes in different image-based attributes and features. These front-end methods can be easily incorporated as a module in a SLAM system. However, mistakes made in these front-end associations are not recoverable in the back end. In addition, a redesign of the data association module is required for these sensor-dependent methods when the sensor is changed. In this paper, we focus on estimating associations in the sensor-agnostic back-end solver. In particular, we extend and enhance the DAF-SLAM framework [5] to develop a more general and efficient object SLAM framework. I Background and DAF-SLAM Recap The data-association-free SLAM (DAF-SLAM) problem is named in [5] to indicate a setup where only odometry and landmark measurements are available to a SLAM solver, but not data associations of the landmark measurements. Mathematically, we denote the robot trajectory as =xii=1N x=\x_i\_i=1^N, where xi∈SE(d)x_i (d) is the unknown i-th pose of the robot in dimension d=2d=2 or 33, and xi=(Ri,ti)x_i=(R_i,t_i) are the rotation and translation components. The unknown landmark positions are denoted by =yjj=1K y=\y_j\_j=1^K where yj∈ℝdy_j ^d. Finally, the given landmark measurements are ¯=z¯kk=1M z=\ z_k\_k=1^M. z¯k∈ℝd z_k ^d is assumed to be generated from the standard model: z¯k=Rik(yjk−tik)+ϵk z_k=R_i_k T(y_j_k-t_i_k)+ _k (1) where a measurement z¯k z_k subject to zero-mean Gaussian noise ϵk∼(0,Σ) _k N(0, ) is made in the local frame of robot pose iki_k. This k-th measurement can be associated with the corresponding pose xikx_i_k via timestamp synchronization. However, the data association, i.e., the index of the landmark being measured (jkj_k), is unknown. Furthermore, since data associations are unknown, the number of landmarks (K) in the environment is also unknown. It is proposed in [5] to split the DAF-SLAM problem into an inner problem and an outer problem. The inner problem takes the form: min∈SE(d)N∈ℝd×Kfodom()+∑k=1Mminjk∈[K]‖Rik(yjk−tik)−z¯k‖Σ2 _ subarrayc x (d)^N\\ y ^d× K subarrayf_odom( x)+ _k=1^M _j_k∈[K]\|R_i_k T(y_j_k-t_i_k)- z_k\|_ ^2 (2) where [K]=1,…,K[K]=\1,…,K\. (2) is a function of the number of landmarks (K). Given K, it minimizes the residuals of landmark measurements over data associations (jkj_k), which can be viewed as a max-mixture model [17]. (2) is solved by alternating minimization between solving jk,\j_k, y\ through k-means and k-means++ [18] and solving ,\ x, y\ through a SLAM solver [19]. This alternating scheme is shown to be advantageous over alternating between jkj_k alone and ,\ x, y\. In addition, k-means++ eliminates the need of an initial guess for y so only odometry is needed to initialize x. Let the optimal objective value of (2) be fslam⋆(K)f_slam (K). The outer problem of estimating the number of landmarks (K) is minK∈1,…,Mfslam⋆(K)+βK _K∈\1,…,M\f_slam (K)+β K (3) where β is a hyperparameter. (3) is solved with multi-resolution gridding, which is a zeroth-order method that uses the inner problem (2) solver to evaluate fslam⋆(K)f_slam (K). The full algorithm is named kSLAM for its use of k-means. IV Method IV-A Local Optimality Condition One important but missing analysis in [5] is the local optimality condition for (3). This optimality condition not only plays a role in finding heuristics for setting β but also helps explain the working principle of the outer problem formulation. Suppose the actual number of landmarks is K⋆K . In order for the minimizer of (3) to be K⋆K , at least the following necessary local conditions have to be met: fslam⋆(K⋆+1)+β≥fslam⋆(K⋆) f_slam (K +1)+β≥ f_slam (K ) (4) fslam⋆(K⋆−1)−β≥fslam⋆(K⋆) f_slam (K -1)-β≥ f_slam (K ) (5) Combining (4) and (5), we can derive a condition on β: fslam⋆(K⋆−1)−fslam⋆(K⋆)≥β≥fslam⋆(K⋆)−fslam⋆(K⋆+1)f_slam (K -1)-f_slam (K )≥β≥ f_slam (K )-f_slam (K +1) (6) fslam⋆(K⋆−1)f_slam (K -1) is the optimal SLAM objective value with one landmark variable fewer than the actual number so the measurements for this missing landmark have to be associated incorrectly to other landmarks (if we assume that fslam⋆(K⋆)f_slam (K ) contains all correct associations). The residuals Rik(yjk−tik)−z¯kR_i_k T(y_j_k-t_i_k)- z_k for these measurements do not follow the Gaussian model since they are caused by association errors rather than sensor noise. When the inter-landmark spacing is large relative to the landmark measurement noise (Fig. 1(a)), mis-associating measurements with a distant wrong landmark can cause large residuals in the objective function. This is where formulation (3) works robustly (see the visualization of fslam⋆(K)f_slam (K) in the Appendix). In this case, the large upper bound βub=fslam⋆(K⋆−1)−fslam⋆(K⋆) _ub=f_slam (K -1)-f_slam (K ) creates a favorable gap above the lower bound βlb=fslam⋆(K⋆)−fslam⋆(K⋆+1) _lb=f_slam (K )-f_slam (K +1) for β to satisfy (6). Since any mis-association is unlikely to be in the optimal solution, the one extra landmark in fslam⋆(K⋆+1)f_slam (K +1) would only split the measurements of a landmark. Under additional assumptions (see details in the Appendix), such as moderate odometry noise and negligible residual changes from the other measurement terms, the reduction in the objective value (i.e., βlb _lb) due to the split is therefore capped by the maximum residuals of all measurements associated with a landmark: βlb≤maxjk⋆∑k∈ℳ(jk⋆)‖Rik⋆(yjk⋆−tik⋆)−z¯k‖Σ2 _lb≤ _j _k _k (j _k)\|R _i_k T(y _j _k-t _i_k)- z_k\|_ ^2 (7) where ℳ(jk⋆)M(j _k) is the set of measurements associated with landmark jk⋆j _k and all the optimal values are computed at fslam⋆(K⋆)f_slam (K ). (a) Large inter-landmark spacing and small measurement noise. (b) Small inter-landmark spacing and large measurement noise. Figure 1: Pre-optimization errors induced by mis-associations after removing the blue landmark. The crosses are measurements of landmarks. The length of each dashed magenta line is the magnitude of a mis-association error. While the upper bound βub _ub is high, to ensure that β is greater than βlb _lb, the right-hand side of (7) can be used as a heuristic to set β so as to satisfy (6). Although argmaxjk⋆ \! _j _k and ℳ(jk⋆)M(j _k) are not known beforehand, we can estimate the number of measurements for a landmark (|ℳ^(jk⋆)|| M(j _k)|) from prior information such as average robot speed, field of view, sensor range, and sensor frame rate (see the Appendix). Since ∑k∈ℳ(jk⋆)‖Rik⋆(yjk⋆−tik⋆)−z¯k‖Σ2 _k (j _k)\|R _i_k T(y _j _k-t _i_k)- z_k\|_ ^2 follows a χ2χ^2 distribution due to (1), β can be heuristically set by the inverse χ2χ^2 distribution with d×|ℳ^(jk⋆)|d×| M(j _k)| degrees of freedom and a sufficiently large probability threshold to be robust to violations of the assumptions underlying (7). The preceding analysis provides a more principled explanation of the heuristic in [5]. When the inter-landmark spacing is small relative to the landmark measurement noise, measurements from different landmarks may mix together (Fig. 1(b)). Mis-associating measurements with a nearby landmark incurs a cost similar to the original sensor noise error. Thus, fslam⋆(K⋆−1)f_slam (K -1) is close to fslam⋆(K⋆)f_slam (K ), and the upper bound βub _ub is lower. In the extreme case where two landmarks are on top of each other, removing one does not increase the objective value. Therefore, β is set empirically in this regime because setting β using the previous heuristic (7) may cause it to exceed βub _ub. However, a starting value for β (2|ℳ^(jk⋆)|π 2| M(j _k)|π) based on quantitative modeling of the effect of adding a landmark (fslam⋆(K⋆+1)f_slam (K +1)) by splitting Gaussian samples, along with additional guidelines provided in the Appendix, is used in an experiment (Section V-B) where landmarks are considered clustered relative to the measurement noise. IV-B Incorporating Semantic Feature Vectors DAF-SLAM studies the setting where only spatial measurements of landmarks are available. However, for applications such as object SLAM, we may also have landmark feature vectors predicted by an object detector (e.g., one-hot classification vectors from Mask R-CNN [20]) or a foundation model (e.g., real-valued vectors from DINO [21]). To enable synergy between data association and feature vector estimation, we incorporate these vectors by assuming a Gaussian noise model (which may be relaxed in practice in Section IV-D): z¯s,k=ys,jk+ϵs,k z_s,k=y_s,j_k+ _s,k (8) where z¯s,k∈ℝs z_s,k ^s is the semantic measurement (i.e., a feature vector of length s) of the landmark semantics (ys,jk∈ℝsy_s,j_k ^s), corrupted by zero-mean Gaussian noise ϵs,k∼(0,Σs) _s,k N(0, _s). The inner problem (2) is modified to include a semantic residual: min∈SE(d)N∈ℝd×K∈ℝs×Kfodom()+∑k=1Mminjk∈[K]‖Rik(yjk−tik)−z¯k‖Σ2+ws2‖ys,jk−z¯s,k‖Σs2 _ subarrayc x (d)^N\\ y ^d× K\\ y_s ^s× K subarrayf_odom( x)+ _k=1^M _j_k∈[K]\\ \\|R_i_k T(y_j_k-t_i_k)- z_k\|_ ^2+w_s^2\|y_s,j_k- z_s,k\|_ _s^2 \ (9) where ws2w_s^2 is a weighting term used to normalize the feature-vector residual by length (if the residual scales with length) and to have a tunable balance between the spatial residual and the semantic residual. In the alternating minimization, the association step now solves the new objective function: min∈ℝd×K∈ℝs×K∑k=1Mminjk∈[K]‖[Rikz¯k+tikσswsz¯s,k]−[yjkσswsys,jk]‖2 _ subarrayc y ^d× K\\ y_s ^s× K subarray _k=1^M _j_k∈[K] \| bmatrixR_i_k z_k+t_i_k\\ σ _sw_s z_s,k bmatrix- bmatrixy_j_k\\ σ _sw_sy_s,j_k bmatrix \|^2 (10) where we assume isotropic noise covariances (Σ=(σ) = diag(σ) and Σs=(σs) _s= diag( _s)) to simplify the exposition, although the anisotropic case can be derived in a similar fashion. In practice, σs _s is often unknown so σsws σ _sw_s can be lumped into a single hyperparameter. Equation (10) stacks the spatial and semantic residuals so that they can be solved jointly in a single pass of k-means (with k-means++ [18]). The data association (jkj_k) becomes the cluster assignment, and the cluster centers are the landmark positions (yjky_j_k) concatenated with the weighted feature vectors (σswsys,jk σ _sw_sy_s,j_k). In the estimation step, we take the data association (jkj_k) results from the association step and solve ,\ x, y\ using a SLAM solver [19]. For the semantic part, once the data associations are given, it is decoupled from ,\ x, y\ in (9) so it can be solved independently. The solution to min∑k=1M‖ys,jk−z¯s,k‖2 _ y_s _k=1^M\|y_s,j_k- z_s,k\|^2 is simply the average of z¯s,k z_s,k’s associated with each ys,jky_s,j_k, which is already computed as the cluster center in the k-means step. For the outer problem, we retain the original objective function (3) because β is set according to the Gaussian assumption of the generative process (Section IV-A), and the Gaussian assumption for the semantic part (8) may be less reliable than that for the positional part (1). Therefore, the semantic part does not enter the outer problem. Remark. The feature vector estimation can be a simple add-on to the original algorithm. Instead of performing k-means in the d-dimensional space, we now run it in the (d+sd+s)-dimensional space. This allows both spatial information and semantic information to facilitate data association during the association step, while the association results can in turn help better estimate the feature vectors of landmarks. For example, a semantically erroneous feature vector may be corrected if the measurement is correctly associated based on its positional component. The updated algorithm is named skSLAM for its additional semantic component. IV-C Semi-Incremental Processing The original DAF-SLAM work [5] adopts a batch processing scheme so that the entire trajectory is processed all at once and all the data associations are jointly determined, thereby avoiding the difficulty of correcting wrong past data associations in a pure incremental scheme. However, drift accumulates along a trajectory and leads to large errors in the initial guess for x (i.e., the odometry chain) toward the end of the trajectory when the trajectory is long or the odometry noise is high, causing lower accuracy for the batch scheme. To reduce drift, we propose a semi-incremental scheme following the divide-and-conquer strategy. Specifically, we define intervals of robot poses indexed by n, n=xii=(n−1)Nn+1min(nNn,N) x_n=\x_i\_i=(n-1)N_n+1 (nN_n,N), where n∈1,…,⌈NNn⌉n∈\1,…, NN_n \ and Nn∈1,…,NN_n∈\1,…,N\ is the interval length. These intervals divide the trajectory into ⌈NNn⌉ NN_n segments. We first run skSLAM on each segment independently (either sequentially if the measurements arrive sequentially or in parallel if they are available at once). In the heuristic for β (Section IV-A), the number of measurements is taken as NnN_n for the segment processing if |ℳ^(jk⋆)|>Nn| M(j _k)|>N_n, because NnN_n poses can at most produce NnN_n observations of a landmark. Once all segments have been processed, we chain them together by connecting the last pose of a segment to the first pose of the next segment using odometry and propagating all pose updates along the chain. We then perform global optimization with skSLAM on the chained segments, which now represent the entire trajectory (Fig. 2). The key saving is that the outer problem solution space over the entire trajectory is reduced from K∈1,…,MK∈\1,…,M\ to 1,…,∑nK^n\1,…, _n K_n\, where K^n K_n is the estimated number of landmarks for segment n. We name the updated algorithm skid-SLAM with “i” standing for (semi-)incremental and “d” standing for divide-and-conquer. It is semi-incremental because poses are processed block-wise in segments. Remark. The segment processing first reduces drift within each segment. Therefore, the initial guess for the trajectory in the global optimization contains less accumulated error than the raw odometry chain. This scheme is analogous to the local and global bundle-adjustment in ORB-SLAM [22]. In terms of computation cost, the outer problem solver originally requires (log(M))O( (M)) evaluations of fslam⋆(K)f_slam (K), each of which requires solving a nonlinear SLAM problem multiple times in the alternating minimization steps. The SLAM solver complexity can be as high as cubic in the problem size. Overall, this is computationally expensive and becomes especially worse as the number of poses (N) and the number of measurements (M) increase. By performing the proposed semi-incremental scheme, we have ⌈NNn⌉ NN_n small problems that keep the (log(Mn))O( (M_n)) and cubic scaling low. Additionally, the global optimization can take advantage of the already estimated numbers of landmarks for the segments ∑nK^n _n K_n, which is typically much smaller than M because each landmark usually has tens or hundreds of measurements for a high-frame-rate sensor such as a camera. The global optimization does not inherit the association results from the segment processing to avoid convergence to local minima. IV-D Practical Considerations One-hot feature vectors. In practice, researchers may want to use one-hot classification vectors from an object detector (e.g., Mask R-CNN [20]) to describe object landmarks. Although our formulation and algorithm are derived from the real-valued feature vector assumption, our algorithm also works functionally for one-hot vectors. During the k-means association step (Section IV-B), the use of one-hot vectors would violate the Gaussian residual assumption (8) and the physical meaning of σs _s, but σsws σ _sw_s can be treated as a hyperparameter to balance the spatial and semantic terms. In the estimation step, taking the average of the one-hot vectors associated with a landmark and selecting the argmax \! element in the average vector is equivalent to majority voting for the landmark class. Therefore, the same algorithm can be applied to one-hot feature vectors without any modification. Parallel global optimization. The global optimization (Section IV-C) can run in parallel with the segment processing. This parallel scheme runs the global optimization on a different thread whenever there are more than two processed segments available. The output of the parallel global optimization is stored as a segment so that it will be combined with the subsequent segments and jointly optimized together with them in the next global optimization. Fig. 2 shows a schematic of the different processing schemes. The parallel scheme is advantageous if the up-to-date pose estimates, incorporating the latest global optimization results, are required in real time. In principle, it should also be more accurate than the series scheme (Section IV-C) because drift is reduced further through the parallel global optimization before the final whole-trajectory global optimization. This parallel implementation is named skill-SLAM, with “l” standing for parallel. overpic[width=411.93767pt,tics=5]includes/figures/illustration/semiinc.png (-1.0,45.0)$x_1$ (3.1,45.0)$x_2$ (7.3,45.0)$x_3$ (11.4,45.0)$x_4$ (15.6,45.0)$x_5$ (21.0,45.0)$...$ (98.0,45.0)$x_N$ (0.0,37.0)kSLAM: batch scheme (0.0,28.7)skid-SLAM: segment processing + final global optimization (0.0,16.2)skill-SLAM: segment processing + parallel global optimization (34.8,9.3)parallel global (68.0,5.0)parallel global overpic Figure 2: Different processing schemes. Detector mistakes. Object detectors, and similarly visual foundation models, may make three types of errors: detecting an irrelevant object as one of the target object classes (false positives), missing detections, and mislabeling objects as one of the other target classes (wrong labels). We discuss their effects and possible treatments. Sporadic false positives that are spatially close to true positives tend to be merged with the true positives (Section V-C) because the reduction in fslam⋆(K)f_slam (K) by forming a new landmark for them usually does not exceed the penalty β in (3). Handling false positives is an interesting direction for future work. For occasional missed detections, if their corresponding landmark can still be estimated from detections at other viewing angles, we can render the estimated object in the frames where it is missed to correct the missing detections in post-processing. For mislabeling, a similar assumption is that the object is correctly labeled in the majority of frames from other viewing angles and correctly associated using the spatial information. In such a case, our algorithm can natively correct incorrect labels by selecting the most-voted class as the final semantic class estimate. V Experiments V-A Synthetic Datasets and Ablation Study Benchmarking and ablation experiments are conducted on the same 2D and 3D synthetic grid datasets as those in [5], but with the addition of one-hot and real-valued semantic feature vectors. We compare against three baselines, (i) raw odometry (i) an alternating minimization algorithm (named Oracle) that is given the ground-truth number of landmarks and an initial guess for each landmark [8, 5], and (i) the original batch processing kSLAM algorithm [5]. We also evaluate two ablated versions of our proposed algorithms, (i) running kSLAM repetitively every NnN_n poses on all poses seen so far to (semi-)incrementally process the trajectory (ikSLAM), and (i) enabling the semantic feature vector processing (Section IV-B) on top of ikSLAM (iskSLAM). The expectation is that ikSLAM should achieve higher accuracy than kSLAM because odometry drift is reduced by incrementally rerunning the algorithm instead of performing one-time batch processing. iskSLAM may achieve the highest accuracy among all since it is the most redundant version. However, both ikSLAM and iskSLAM are computationally expensive and even prohibitive on large datasets due to the naive incremental rerunning scheme. We increase the number of inner solver iterations for all kSLAM variants on the synthetic datasets to improve convergence and obtain a more accurate characterization of the trend. The performance of all methods is measured in terms of absolute trajectory error (ATE) after SIM(3)SIM(3) alignment between the estimated trajectory and a reference trajectory, which is obtained by optimizing the same measurements given the ground-truth data associations. We typically find that ATE is positively correlated with other metrics such as landmark position error. We sweep various dataset parameters in Fig. 3 – 6. Each data point in the plots is the median result of 20 simulations (different realizations of the noise) and the shaded region shows the 25% – 75% quartile range. We use the heuristic in Section IV-A to set β. Detailed dataset settings, algorithm parameter settings, and evaluation protocols are in the Appendix. Odometry noise. The proposed semi-incremental scheme (Section IV-C) is designed to reduce drift while keeping the computational load low. In Fig. 3, we show that all the semi-incremental methods (ikSLAM, iskSLAM, skid-SLAM, and skill-SLAM) generally outperform the batch kSLAM algorithm. Further, the incorporation of semantic feature vectors (iskSLAM, skid-SLAM, and skill-SLAM) results in even lower ATE, particularly when the odometry noise is high. The baseline alternating minimization algorithm (Oracle) is almost always inferior, which is consistent with the results reported in [5]. Figure 3: Absolute trajectory error (ATE) in log scale for different odometry noise levels on the 3D grid dataset. The noise factor is multiplied to the default noise level (translational std = 0.05 (m) and rotational std = 0.005). Runtime. Although the naive incremental rerunning scheme (ikSLAM and iskSLAM) also reduces ATE, it has worse runtime (Fig. 4). Among the algorithms that incorporate semantic information (iskSLAM, skid-SLAM, and skill-SLAM), iskSLAM is clearly the slowest. skill-SLAM is slower than skid-SLAM because the parallel global-optimization thread has to wait for the last global optimization to finish before starting the final whole-trajectory optimization, whereas skid-SLAM starts the whole-trajectory optimization immediately after the segment processing is completed. Among all the algorithms, Oracle is the fastest because it is given the ground-truth number of landmarks and does not spend time estimating it. On the smaller 3D grid dataset (216 poses), kSLAM is faster than our proposed algorithms. However, on the larger 2D grid dataset (500 poses), the savings from our divide-and-conquer strategy (Section IV-C) become apparent and enable the proposed algorithms to match or surpass the other baselines, particularly when more landmarks are present. The runtime advantage becomes even more significant when the number of measurements per landmark is higher (|ℳ(jk⋆)|=10|M(j _k)|=10 here), resulting in a large total number of measurements (M) (Section V-B). Figure 4: Runtime for different numbers of landmarks. Semantic noise. We vary the nominal detector mislabeling probability for one-hot semantic vectors (Fig. 5) and the Gaussian noise standard deviation for real-valued semantic feature vectors (Fig. 6). The nominal detector mislabeling probability specifies the probability with which each semantic measurement is intentionally corrupted. The “Raw” curve in each figure denotes the actual semantic error in the measurements after the noise is realized. The remaining curves show the errors of the estimated landmark semantics. The algorithms that process semantic information (iskSLAM, skid-SLAM, and skill-SLAM) generally achieve lower ATE than those do not (kSLAM and ikSLAM), but the benefits diminish as the semantic information becomes less reliable. On the other hand, joint estimation of semantics yields substantial reductions in landmark semantic estimation errors in both the one-hot and real-valued cases, as evidenced by the large differences between the raw semantic errors and the estimation errors. Figure 5: Absolute trajectory error (ATE) in log scale and mislabeling rate for different nominal detector mislabeling probabilities on the 2D grid dataset with one-hot feature vectors. Figure 6: Absolute trajectory error (ATE) in log scale and feature vector estimation error (Euclidean norm) for different semantic noise standard deviations on the 2D grid dataset with real-valued feature vectors. V-B Kitchen Datasets with One-Hot Feature Vectors We evaluate our algorithms on in-house indoor kitchen datasets and designed to test their ability to incorporate object detections and one-hot semantic vectors. We place various objects in a kitchen environment and remotely operate an RB-Y1 humanoid robot equipped with a ZED stereo camera, guiding it through the kitchen over multiple passes to mimic a household robot performing daily tasks. Two baselines are included: (i) the batch processing kSLAM algorithm [5], and (i) an object-centric SLAM method [2]. The input odometry is computed using RGB-D visual odometry [23, 24]. The landmark measurements are taken as the centroids of segmented objects in point clouds back-projected from depth images, where the object masks are obtained from Mask R-CNN [20] for kSLAM and our proposed algorithms, and from YOLOv8 for the object-centric SLAM baseline [2]. To provide ground truth for evaluation, we place a unique AprilTag [25] close to each object (rather than on the object itself, to avoid interfering with the object detector). These tags serve as pseudo-landmarks whose measurements are extracted in a similar manner from the back-projected depth images. The identities of the tags allow us to associate the tag measurements and run standard landmark-based SLAM. The resulting estimated trajectory is used as the pseudo-ground-truth trajectory for evaluation, and the estimated tag positions serve as qualitative references for the object positions because they are slightly offset from the objects, making them unsuitable for quantitative evaluation). The objects are strategically arranged in two layouts: (i) clusters of objects from different classes, and (i) clusters of objects from the same class. As discussed in Section IV-A, for environments of this type, where landmarks are close to one another relative to the landmark measurement noise, β is set empirically. However, following the modeling results and guidelines presented in the Appendix, a reasonable starting value for β is 2|ℳ^(jk⋆)|π 2| M(j _k)|π, which is used for the kitchen datasets. As shown in Fig. 7 and Table I, our proposed algorithms (skid-SLAM and skill-SLAM) are able to recover from several errors, such as missing landmarks and extra landmarks, made by kSLAM and reduce the trajectory error in both cases (i) and (i). In addition, our proposed algorithms exhibit a clear speed advantage on these larger datasets due to the savings discussed in Section IV-C, continuing the trend observed in Section V-A. The more challenging layout in case (i) does degrade performance, causing landmarks of the same class to be merged rather than correctly distinguished. However, the estimated landmark labels are always correct. The object-centric SLAM baseline [2] clearly performs worse. We find that it relies heavily on the manually set covariance matrices and thresholds. TABLE I: Absolute trajectory error (m) and runtime (s) on the kitchen datasets. Dataset N KgtK_gt M Odometry Object-Centric [2] kSLAM skid-SLAM skill-SLAM Diff Class 3719 21 6552 0.264 0.361 0.034 / 429 0.033 / 325 0.033 / 364 Same Class 4009 21 6872 0.200 0.147 0.039 / 503 0.039 / 321 0.037 / 342 (a) Raw data (b) Object-Centric SLAM [2] (c) kSLAM [5] (d) skid-SLAM (e) skill-SLAM Figure 7: Visualization of the kitchen results. Circles indicate pseudo-ground-truth landmark positions (which are slightly offset from the actual landmark positions). Crosses represent landmark measurements. Stars indicate estimated landmark positions. The colors of landmarks denote their class labels (if applicable). The blue and black trajectories are the pseudo-ground-truth and estimated trajectories, respectively. Top: Diff Class. Bottom: Same Class. V-C Pool Dataset with Real-Valued Feature Vectors The kitchen dataset evaluates our algorithm with one-hot feature vectors from a closed-set object detector, where landmark classes are predefined and semantically distinct by construction. To evaluate our algorithms under the more challenging open-set setting, where object classes are unknown and real-valued feature vectors are required, we use the indoor pool dataset from [2]. This dataset was collected with a BlueROV2 equipped with a monocular camera and an Oculus m1200d multibeam sonar while navigating an indoor pool that contains diverse objects (e.g., lobster cages, seaweed, tires, etc.) (Fig. 8(a)). To represent the semantic identities of the objects, DINO features [21], extracted via unsupervised segmentation, are used instead of one-hot feature vectors, yielding real-valued descriptors for each detected object. Since ground-truth trajectory data is not available for this dataset, we evaluate the results through qualitative visualization of the estimated maps. (a) Pool dataset setup CageSeaweedTireHand NetTowfishPipeTireCageTowfishPipeSeaweedCage (b) Raw data of odometry and landmark measurements (c) kSLAM [5] (d) skill-SLAM Figure 8: Pool dataset results. The high-dimensional feature-vector measurements are reduced to three-dimensional vectors using PCA and mapped to RGB colors for visualization. The raw data has a few serious errors indicated by the red circles in Fig. 8(b). In the kSLAM and skill-SLAM results, the magenta lines represent the estimated data associations, and the colored circles indicate the estimated landmark positions and semantics. In Fig. 8, our proposed skill-SLAM is compared with the batch-processing kSLAM [5], but readers can also refer to [2] for additional comparison with their results. The heuristic in Section IV-A is used to set β since the landmarks in this dataset are well-separated. In Fig. 8(b), the raw data contains several errors, particularly for small and hard-to-see objects such as the hand net and the pipes. Both kSLAM and skill-SLAM merge these erroneous measurements into nearby landmarks. We hypothesize that the outer algorithm (3) decides not to introduce more landmarks for these sporadic outliers because the objective value reduction in fslam⋆(K)f_slam (K) for doing so does not outweigh the penalty βKβ K in (3); thus, the inner algorithm (9) is forced to assign them incorrectly to nearby landmarks. Owing to the noisy depth measurements from the sonar and accumulated odometry drift over time, when the robot loops back and re-observes a previous landmark (e.g., the seaweed and cage on the right side in Fig. 8(b)), its measurements projected from the raw odometry can be quite far away from its measurements in the first pass, making them difficult to associate correctly. In Fig. 8(c), kSLAM erroneously merges the cage and seaweed together and makes similar mistakes elsewhere (e.g., between the cage and seaweed on the left side). However, skill-SLAM correctly associates the measurements of these objects by leveraging the semi-incremental scheme (Section IV-C) to reduce drift and semantic feature vectors to better distinguish between objects. We observe that the semantic information is critical here for data association, as the positional measurements are too noisy. VI Conclusion We extended the DAF-SLAM framework [5] and developed the skid-SLAM and skill-SLAM algorithms, with skill-SLAM performing slightly better than skid-SLAM. The extended framework and its algorithms additionally process and estimate semantic vectors of object landmarks, operate in a semi-incremental manner to reduce drift, and provide principled guidelines and heuristics for setting a key parameter. They outperform the original kSLAM algorithm within the DAF-SLAM framework as well as several strong baselines on both synthetic and real-world datasets. An important future direction is handling both positional (e.g., spurious depths) and semantic (e.g., false positives) outlier measurements, potentially by modeling outliers in the generative models and using robust optimization objectives. Another future direction is incorporating shape estimation to enable a synergy between shape estimation and data association, and move beyond point-landmark representations. References [1] K. J. Doherty, D. P. Baxter, E. Schneeweiss, and J. J. Leonard, “Probabilistic data association via mixture models for robust semantic SLAM,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, p. 1098–1104. [2] K. Singh, J. Hong, N. R. Rypkema, and J. J. Leonard, “Opti-acoustic semantic SLAM with unknown objects in underwater environments,” in 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2024, p. 1169–1176. [3] J. Hong, R. Choi, and J. J. Leonard, “Semantic enhancement for object slam with heterogeneous multimodal large language model agents,” in 2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2025, p. 9036–9043. [4] E. Michael, T. Summers, T. A. Wood, C. Manzie, and I. Shames, “Probabilistic data association for semantic SLAM at scale,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022, p. 4359–4364. [5] Y. Zhang, O. A. Severinsen, J. J. Leonard, L. Carlone, and K. Khosoussi, “Data-association-free landmark-based SLAM,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, p. 8349–8355. [6] J. Neira and J. D. Tardós, “Data association in stochastic mapping using the joint compatibility test,” IEEE Transactions on Robotics and Automation, vol. 17, no. 6, p. 890–897, 2001. [7] M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit et al., “FastSLAM: A factored solution to the simultaneous localization and mapping problem,” Aaai/iaai, vol. 593598, no. 2, p. 593–598, 2002. [8] K. J. Doherty, Z. Lu, K. Singh, and J. J. Leonard, “Discrete-continuous smoothing and mapping,” IEEE Robotics and Automation Letters, vol. 7, no. 4, p. 12 395–12 402, 2022. [9] V. Korotkine, M. Cohen, and J. R. Forbes, “Globally optimal data-association-free landmark-based localization using semidefinite relaxations,” IEEE Robotics and Automation Letters, 2025. [10] B. Mu, S.-Y. Liu, L. Paull, J. Leonard, and J. P. How, “SLAM with objects using a nonparametric pose graph,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016, p. 4602–4609. [11] J. Zhang, L. Yuan, T. Ran, Q. Tao, and L. He, “Bayesian nonparametric object association for semantic SLAM,” IEEE Robotics and Automation Letters, vol. 6, no. 3, p. 5493–5500, 2021. [12] T. Ran, L. Yuan, J. Zhang, L. He, R. Huang, and J. Mei, “Not only look but infer: Multiple hypothesis clustering of data association inference for semantic SLAM,” IEEE Transactions on Instrumentation and Measurement, vol. 70, p. 1–9, 2021. [13] S. L. Bowman, N. Atanasov, K. Daniilidis, and G. J. Pappas, “Probabilistic data association for semantic SLAM,” in 2017 IEEE international conference on robotics and automation (ICRA). IEEE, 2017, p. 1722–1729. [14] R. F. Salas-Moreno, R. A. Newcombe, H. Strasdat, P. H. Kelly, and A. J. Davison, “SLAM++: Simultaneous localisation and mapping at the level of objects,” in Proceedings of the IEEE conference on computer vision and pattern recognition, 2013, p. 1352–1359. [15] J. G. Rogers, A. J. Trevor, C. Nieto-Granda, and H. I. Christensen, “Simultaneous localization and mapping with learned object recognition and semantic data association,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2011, p. 1264–1270. [16] K. Li, D. DeTone, Y. F. S. Chen, M. Vo, I. Reid, H. Rezatofighi, C. Sweeney, J. Straub, and R. Newcombe, “ODAM: Object detection, association, and mapping using posed RGB video,” in Proceedings of the IEEE/CVF International Conference on Computer Vision, 2021, p. 5998–6008. [17] E. Olson and P. Agarwal, “Inference on networks of mixtures for robust robot mapping,” The International Journal of Robotics Research, vol. 32, no. 7, p. 826–840, 2013. [18] D. Arthur and S. Vassilvitskii, “k-means++: The advantages of careful seeding,” Stanford, Tech. Rep., 2006. [19] F. Dellaert and M. Kaess, “Georgia tech smoothing and mapping (gtsam),” 2019, 2019. [20] K. He, G. Gkioxari, P. Dollár, and R. Girshick, “Mask R-CNN,” in Proceedings of the IEEE international conference on computer vision, 2017, p. 2961–2969. [21] M. Caron, H. Touvron, I. Misra, H. Jégou, J. Mairal, P. Bojanowski, and A. Joulin, “Emerging properties in self-supervised vision Transformers,” in Proceedings of the IEEE/CVF international conference on computer vision, 2021, p. 9650–9660. [22] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE transactions on robotics, vol. 31, no. 5, p. 1147–1163, 2015. [23] Q.-Y. Zhou, J. Park, and V. Koltun, “Open3D: A modern library for 3D data processing,” arXiv preprint arXiv:1801.09847, 2018. [24] F. Steinbrücker, J. Sturm, and D. Cremers, “Real-time visual odometry from dense RGB-D images,” in 2011 IEEE international conference on computer vision workshops (ICCV Workshops). IEEE, 2011, p. 719–722. [25] J. Wang and E. Olson, “AprilTag 2: Efficient and robust fiducial detection,” in 2016 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2016, p. 4193–4198. Assumptions in Deriving the Upper Bound for βlb _lb Several assumptions used to reach (7) are briefly touched on in Section IV-A. We make them more explicit in this section. First, K⋆K is defined to be the ground-truth number of landmarks. However, the optimal configuration fslam⋆(K⋆)f_slam (K ) may not contain all ground-truth associations if our objective function (2) does not model the real world accurately. Assuming fslam⋆(K⋆)f_slam (K ) contains all correct associations makes our reasoning much easier because we no longer need to consider the case where fslam⋆(K⋆)f_slam (K ) already includes mis-associations. The validity of this assumption lies in the soundness of the modeling choices (i.e., Gaussian noise and the max-mixture model for data association) and is beyond the scope of this paper. Second, the validity of (7) assumes that fslam⋆(K⋆+1)f_slam (K +1) also does not contain incorrect associations. In other words, the extra landmark would only split the measurements associated with a landmark in fslam⋆(K⋆)f_slam (K ). This assumption is consistent with our working regime of large inter-landmark spacing relative to the landmark measurement noise, because any mis-association in this regime would introduce large residuals, which are avoided by the optimization. As illustrated in Fig. 9(b), the added landmark splits the two measurements associated with the left landmark in Fig. 9(a), which is possible under our assumption. However, the added landmark instead merges two measurements previously associated with two different landmarks in Fig. 9(c), which is not possible under our assumption. When the inter-landmark spacing is small relative to the landmark measurement noise, this assumption starts to break down (Fig. 10), but this also falls outside of the working regime of our heuristic. (a) Configuration for fslam⋆(K⋆)f_slam (K ). (b) Possible fslam⋆(K⋆+1)f_slam (K +1). (c) Impossible fslam⋆(K⋆+1)f_slam (K +1). Figure 9: Our assumption about the possible configuration for fslam⋆(K⋆+1)f_slam (K +1). The circles are robot poses. The triangles are landmarks. The crosses are measurements of landmarks. (a) fslam⋆(K⋆)f_slam (K ). (b) fslam⋆(K⋆+1)f_slam (K +1). Figure 10: An imaginary example where our assumption breaks. The magenta lines indicate data associations. The added landmark in 10(b) merges two measurements previously associated with two different landmarks. Third, while not emphasized in the main paper, the odometry noise has to be moderate. In general, we find that increasing odometry noise has the opposite effect of increasing landmark measurement noise. In other words, it tends to increase βlb _lb and βub _ub (Fig. 12) so β is more likely to fall below βlb _lb, causing overestimation of K. We hypothesize two possible mechanisms. (i) The SLAM optimization only cares about the relative weighting (or relative noise magnitude) between the odometry term and the landmark measurement term, because scaling the objective function (2) up or down does not change the result. Therefore, increasing odometry noise can be viewed as increasing the weighting of the landmark measurement term. The non-Gaussian residual increase and reduction for fslam⋆(K⋆−1)f_slam (K -1) and fslam⋆(K⋆+1)f_slam (K +1) relative to fslam⋆(K⋆)f_slam (K ) are thus magnified, and βlb _lb and βub _ub increase. (i) Large odometry noise creates more trajectory drift, and landmark measurements projected from the trajectory (i.e., the robot poses) have greater dispersion than that implied by the nominal landmark measurement noise. Without known data associations, the larger spread shifts the residuals at each K in fslam⋆(K)f_slam (K) upward. Fourth, when adding a landmark, besides the direct residual reduction from the landmark measurement terms, there is a nonlinear relaxation effect on the residuals of the odometry measurements and other landmark measurements, as the trajectory becomes less constrained. We assume this effect is negligible compared to the drop in the landmark measurement residuals. In practice, we use a large probability value for our χ2χ^2 heuristic so that β is considerably larger than the right-hand side of (7) to compensate for the unaccounted-for relaxation effect. Under the assumptions made above, the best possible outcome of splitting the measurements associated with one landmark across two landmarks is to drive the residuals for all the measurements associated with that landmark to zero. The exception in Fig. 10 is a violation, since the residuals for all the measurements associated with the two landmarks are nearly driven to zero in Fig. 10(b), which is even better than the outcome predicted by our upper bound (7). Low Inter-Landmark Spacing Scenario In Section IV-A, we mention that when the inter-landmark spacing is small relative to the landmark measurement noise, β is set empirically because several assumptions underlying the β heuristic no longer hold. However, useful guidelines and a reasonable starting point still exist. We discuss them in this section. βub _ub is, in general, difficult to quantify since it depends on the environment-dependent mis-association errors. We thus focus on βlb _lb. It is easy to show, without additional assumptions, that βlb≥maxk‖Rik⋆(yjk⋆−tik⋆)−z¯k‖Σ2 _lb≥ _k\|R _i_k T(y _j _k-t _i_k)- z_k\|_ ^2 (11) because the one extra landmark can always be associated with a single measurement to drive its residual to zero. The right-hand side of (11) follows a χ2χ^2 distribution with degrees of freedom d. We can use the inverse χ2χ^2 distribution and a large probability value to compute a conservative value for the right-hand side of (11), and β needs to be greater than this value according to (6). As discussed in Section IV-A, βub _ub is small in the regime of low inter-landmark spacing, and thus, we would like a value smaller than the right-hand side of (7) to set β. In this regime, fslam⋆(K⋆+1)f_slam (K +1) may contain mis-associations such as the scenario in Fig. 10. However, it is still useful to consider the case where there are no mis-associations and the added landmark only splits the measurements associated with a landmark in fslam⋆(K⋆)f_slam (K ), which is essentially a restriction of the solution space of minfslam(K⋆+1) f_slam(K +1), and the resulting objective value will be larger than fslam⋆(K⋆+1)f_slam (K +1). While (7) gives an upper bound on the residual reduction for splitting the measurements, the goal here is to quantify the reduction more exactly to set a smaller β. We thus model splitting the measurements of a landmark as splitting Gaussian samples. When the number of samples is large (i.e., |ℳ(jk⋆)|→∞|M(j _k)|→∞), the maximum reduction in the residuals (i.e., analogous to fslam⋆(K⋆)−fslam⋆(K⋆+1)f_slam (K )-f_slam (K +1)) is approximately 2|ℳ(jk⋆)|π 2|M(j _k)|π, which can be a starting point for tuning β. We leave the derivations in the final section. We summarize the findings here: (i) β should be chosen to be greater than the right-hand side of (11). (i) In the modeling above, we have restricted the solution space, and a lower fslam⋆(K⋆+1)f_slam (K +1) may actually be achievable. In other words, βlb=fslam⋆(K⋆)−fslam⋆(K⋆+1) _lb=f_slam (K )-f_slam (K +1) may actually be greater than 2|ℳ(jk⋆)|π 2|M(j _k)|π, and setting β to this value may cause β to fall below βlb _lb. (i) The relaxation effect of adding a landmark on the odometry terms and other measurement terms is not considered. In principle, the relaxation effect would make fslam⋆(K⋆+1)f_slam (K +1) even smaller, which may cause β=2|ℳ(jk⋆)|πβ= 2|M(j _k)|π to fall below βlb _lb. (iv) The result assumes a large number of samples (measurements of a landmark). This is typically true with a high-frame-rate sensor. When this assumption is violated, a lower fslam⋆(K⋆+1)f_slam (K +1) may be achievable, and β=2|ℳ(jk⋆)|πβ= 2|M(j _k)|π may fall below βlb _lb. Applicable Ranges of the β Heuristics It is interesting to examine what the inverse χ2χ^2 heuristic in Section IV-A and setting β=2|ℳ(jk⋆)|πβ= 2|M(j _k)|π lead to under different inter-landmark spacing (relative to the landmark measurement noise). To this end, we first summarize the properties of our datasets (Section V) in Tables I and I. The kitchen datasets (i.e., Diff Class and Same Class) clearly have their measurement standard deviation σ on the same order of magnitude as the mean and minimum inter-landmark spacing, whereas the other datasets have σ orders of magnitude smaller than the spacing. Accordingly, we use β=2|ℳ(jk⋆)|πβ= 2|M(j _k)|π for the kitchen datasets (see the actual parameters used in Table V). [b] TABLE I: Dataset properties to the best of our estimates. Dataset N KgtK_gt M |ℳ(j)|avg|M(j)|_avg σ Spacing1 (mean, min, max) Diff Class 3719 21 6552 312 0.1 0.38, 0.10, 1.44 Same Class 4009 21 6872 327 0.1 0.31, 0.11, 1.88 Pool 2335 12 (10)2 1104 92 (110)2 0.3, 0.3, 0.63 1.74, 1.17, 2.94 Grid 3D 216 43 430 10 0.05 1.06, 0.49, 2.004 Grid 2D 500 100 1000 10 0.05 1.33, 0.18, 3.514 1 Defined as the distance to the closest landmark for every landmark. 2 Two landmarks are almost entirely missing in the measurements (Fig. 8(b)). 3 In the order of x, y and z axes in the robot camera frame. 4 Example numbers from one simulation instance. [b] TABLE I: Additional dataset properties to the best of our estimates. Dataset Avg. Odom. Std. x y z r p y (×10−3× 10^-3) Trajectory Size1 Mislabeling Rate2 Semantic Std. Diff Class 2.5, 1.6, 1.8, 3.6, 4.7, 2.3 3.6 × 4.6 - - Same Class 2.6, 1.7, 1.8, 3.6, 4.7, 2.2 3.4 × 4.6 - - Pool 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 20.0 × 1.9 - - Grid 3D 50, 50, 50, 5.0, 5.0, 5.0 6 × 6 × 6 0.1 0.1 Grid 2D 50, 50, 5.0 20 × 25 0.1 0.1 1 If the trajectory is mostly on a principal plane, this is the span of the trajectory on the plane. 2 Nominal mislabeling rate to simulate an object detector. We also perform a set of small-scale experiments for simple validation in Table IV. In Experiments 1 – 3, we see that β has to scale with the number of measurements per landmark (|ℳ(j)||M(j)|) to estimate K correctly. The 2|ℳ(j)|π 2|M(j)|π heuristic consistently sets β too small, leading to overestimation of K in Experiments 4 – 6, which is expected because our assumptions used to derive the heuristic tend to underestimate what β should ideally be (as explained in the section “Low Inter-Landmark Spacing Scenario”). In Experiments 7 – 14, as σ approaches the same order of magnitude as the mean inter-landmark spacing, the inverse χ2χ^2 heuristic starts to underestimate K because of the drop in βub _ub (Section IV-A), while the 2|ℳ(j)|π 2|M(j)|π heuristic begins to estimate K correctly. In Experiments 10 and 14, σ=1.5σ=1.5 is considered extremely large, but the 2|ℳ(j)|π 2|M(j)|π heuristic manages to correctly estimate K. [b] TABLE IV: Results using the two β heuristics on a small-scale 3 × 2 × 3 simulated 3D Grid dataset (18 poses). Exp. KgtK_gt |ℳ(j)||M(j)| σ Spacing1 (mean, min, max) Heuristic β KestK_est 1 3 100 0.05 1.26, 1.16, 1.47 (χ2)−1(0.999,300)(χ^2)^-1(0.999,300) 381.4 3 (0) 2 3 500 0.05 0.94, 0.43, 1.96 (χ2)−1(0.999,1500)(χ^2)^-1(0.999,1500) 1675 3 (0) 3 3 800 0.05 1.03, 0.91, 1.26 (χ2)−1(0.999,2400)(χ^2)^-1(0.999,2400) 2620 3 (0) 4 3 100 0.05 1.26, 1.16, 1.47 2×100π 2× 100π 63.7 6 (+3) 5 3 500 0.05 0.94, 0.43, 1.96 2×500π 2× 500π 318.3 4 (+1) 6 3 800 0.05 1.03, 0.91, 1.26 2×800π 2× 800π 509.3 4 (+1) 7 10 500 0.1 0.57, 0.22, 1.18 (χ2)−1(0.999,1500)(χ^2)^-1(0.999,1500) 1675 9 (-1) 8 10 500 0.5 0.57, 0.24, 1.22 (χ2)−1(0.999,1500)(χ^2)^-1(0.999,1500) 1675 4 (-6) 9 10 500 0.8 0.57, 0.25, 1.24 (χ2)−1(0.999,1500)(χ^2)^-1(0.999,1500) 1675 4 (-6) 10 10 500 1.5 0.48, 0.26, 1.11 (χ2)−1(0.999,1500)(χ^2)^-1(0.999,1500) 1675 3 (-7) 11 10 500 0.1 0.57, 0.22, 1.18 2×500π 2× 500π 318.3 18 (+8) 12 10 500 0.5 0.57, 0.24, 1.22 2×500π 2× 500π 318.3 14 (+4) 13 10 500 0.8 0.57, 0.25, 1.24 2×500π 2× 500π 318.3 11 (+1) 14 10 500 1.5 0.48, 0.26, 1.11 2×500π 2× 500π 318.3 10 (0) 1 Distance to the closest landmark for every landmark, calculated from the estimated landmark positions given the ground truth data associations, to reflect the configuration of fslam⋆(K⋆)f_slam (K ). Estimating the Number of Measurements per Landmark In Section IV-A, to compute a heuristic for setting β, we need to estimate the number of measurements per landmark. We consider a forward-moving model (Fig. 11(a)) for the kitchen dataset (Section V-B) and a sideward-moving model (Fig. 11(b)) for the pool dataset (Section V-C). These are simple 2D models used to simplify the calculations. θ (a) forward-moving model. vvθ (b) sideward-moving model. Figure 11: Simple 2D models to estimate the number of measurements per landmark. The sensor is represented as a gray rectangle. The yellow diamond represents a landmark. In Fig. 11(a), the distance (l) while the landmark is in view, is l=r−ccot(θ2)l=r-c ( θ2) (12) where r is the cutoff depth, θ is the field of view, and c may be computed as an average over all landmark measurements. In Fig. 11(b), the in-view distance (l) is l=2ctan(θ2)l=2c ( θ2) (13) where c is also computed as an average over all landmark measurements, with c corresponding to the depth component here. The estimated number of observations is then computed as fps×l/vfps× l/v where fps is the sensor fps and v is the robot’s average speed computed from odometry measurements. These models assume that the robot passes by each landmark only once, which is generally true for the segment processing stage (Section IV-C). If there is a strong belief that the robot passes by each landmark multiple times, we can set a different β for the whole-trajectory global optimization. Otherwise, we rely on the robustness of formulation (3) to correctly estimate K. For the segment processing, the number of observations should also not exceed the number of poses in the segment, assuming each pose can produce at most one observation of a landmark. Algorithm Parameters We provide our parameter settings in Table V. [b] TABLE V: Motion model parameters (calculated based on prior information or from measurements) and algorithm parameters. Dataset v θ r c fps |ℳ(j)|est|M(j)|_est Heuristic βglb _glb σsws σ _sw_s1 NnN_n βseg _seg2 NinnerN_inner3 NLMN_LM4 k-means5 Grid 3D - - - - - 10 (χ2)−1(0.999,30)(χ^2)^-1(0.999,30) 60 0.1 100 60 30 100 3, 15, 0.1 Grid 2D - - - - - 10 (χ2)−1(0.999,20)(χ^2)^-1(0.999,20) 45 0.1 100 45 30 100 3, 15, 0.1 Diff Cls 0.247 120 3 0.642 20 213 2×213π 2× 213π 136 0.5 100 64 15 100 3, 15, 0.1 Same Cls 0.246 120 3 0.698 20 211 2×211π 2× 211π 134 0.5 100 64 15 100 3, 15, 0.1 Pool 0.186 80 - 2.62 3.81 90 (χ2)−1(0.999,270)(χ^2)^-1(0.999,270) 347 100 100 347 15 100 3, 15, 0.1 1 Real-valued feature vectors are additionally normalized by length. 2 Calculated using NnN_n instead of |ℳ(j)|est|M(j)|_est if Nn<|ℳ(j)|estN_n<|M(j)|_est. 3 Number of inner solver iterations. The argmin \! solution across iterations (not necessarily the final iteration due to k-means++ randomness) is selected. 4 Number of Levenberg-Marquardt optimizer iterations for SLAM. 5 Number of k-means attempts (the argmin \! result is kept), termination iteration and termination tolerance. Landmark Evaluation We provide additional details about the evaluation of landmark estimation on the synthetic datasets. The data association is unknown to our algorithms, and therefore, we need to match the estimated landmarks to the reference landmarks. The reference landmarks are obtained along with the reference trajectory, by optimizing the measurements given the ground-truth data associations. We first compute the SIM(3)SIM(3) alignment between the estimated trajectory and the reference trajectory, and apply it to the estimated landmark positions. A linear assignment problem is then solved to match the estimated landmark positions to reference landmark positions. Only min(Kref,Kest) (K_ref,K_est) pairs are matched, and any extra landmarks are not included in the evaluation. The estimation error is computed over all matched pairs. Visualization of fslam⋆(K)f_slam (K) It is interesting to plot fslam⋆(K)f_slam (K) as a function of K. Computing the true fslam⋆(K)f_slam (K) is combinatorial, therefore we use our inner algorithm (2) to approximate its value. We show three cases under different noise levels in Fig. 12. In the case of the normal noise level, we can clearly observe the elbow point at the true K, which is consistent with our discussions in Section IV-A. The case of high odometry noise exhibits the opposite trend compared to the case of high landmark measurement noise. We provide two possible explanations in the previous section “Assumptions in Deriving the Upper Bound for βlb _lb”. Additionally, the case of high odometry noise appears to be “noisier”, which reveals that the algorithm more easily returns a suboptimal solution, potentially due to the drift accumulation effect of odometry noise that does not occur with landmark measurement noise. Figure 12: Evaluating fslam⋆(K)f_slam (K) using the inner algorithm (2) on the synthetic 3D grid dataset of size 6×6×66× 6× 6. The normal case uses a landmark measurement standard deviation 0.1 and odometry translation and rotation standard deviations of 0.05 and 0.005, respectively. The high landmark noise case uses a landmark measurement standard deviation 0.5, which is fairly large with respect to the map scale. The high odometry noise case uses translation and rotation standard deviations of 0.15 and 0.015, respectively. The true K is 43. There are two effects that lead to the differences between the normal case and the case of high landmark measurement noise. First, the nonlinear optimization will result in a lower SLAM objective value as the landmark measurement noise increases. We show a simple experiment in Table VI for a standard landmark-based SLAM problem (given data associations). The data are generated synthetically from the Gaussian noise models that exactly match the probabilistic models assumed by the SLAM objective function. Since the covariance matrices and the probabilistic models are exact, the covariance matrices perfectly normalize the measurement residuals (1), and thus, given that the ground truth is used as the initial values for the optimization variables, the initial errors (i.e., objective function values) are all the same. However, after optimization, due to the nonlinear interplay between the odometry term and the landmark measurement term, the optimal objective values are smaller at higher landmark measurement noise. This explains why the case of high landmark measurement noise has lower fslam⋆(K)f_slam (K) than the normal case at the true K in Fig. 12. TABLE VI: Factor graph errors (i.e. objective function values) of a standard landmark-based SLAM problem (given data associations) at different landmark measurement noise levels. The synthetic 3D grid dataset is used. Landmark measurements are generated from the Gaussian generative model (1), so the covariance matrices are exact. Ground truth is used as the initial guess. LM Std 0.001 0.01 0.05 0.1 0.2 0.3 0.4 0.5 Initial Error 2607 Final Error 1230 1222 1219 1203 1183 1174 1169 1167 Second, when the landmark measurement noise is high (relative to the inter-landmark spacing), the unnormalized (i.e., not normalized by the covariance matrix) residual ‖Rik(yjk−tik)−z¯k‖2||R_i_k T(y_j_k-t_i_k)- z_k||^2 of an association error may have a similar magnitude as the residual due to pure sensor noise. Therefore, even when K is below the true K and more association errors are introduced in computing fslam⋆(K)f_slam (K), we do not observe as sharp a transition as in the normal case in Fig. 12. This is where the gap between βlb _lb and βub _ub becomes narrow, and our algorithm (3), using β set by the inverse χ2χ^2 heuristic in (7), begins to underestimate K. Kitchen Dataset Panoramas For direct visualization of the kitchen datasets, we provide panoramas of the setups in Fig. 13. (a) Kitchen - Diff Class. (b) Kitchen - Same Class. Figure 13: Panoramas of the kitchen environment and the experiment setups. In the datasets, we have removed the laptop detections, because the laptop screens caused erroneous depth measurements. Splitting Gaussian Samples Following up on the previous section “Low Inter-Landmark Spacing Scenario”, we model the addition of a landmark as splitting Gaussian samples to approximate the objective value change from fslam⋆(K⋆)f_slam (K ) to the pre-optimization (non-optimal) value of fslam(K⋆+1)f_slam(K +1). Fig. 14 shows a schematic for illustration. For clarity, we hereafter use notation that is independent of the rest of the paper. Figure 14: Splitting Gaussian samples to model fslam(K⋆+1)f_slam(K +1). We show two different ways of splitting. -A Problem Setup Consider a landmark at position μ∈ℝdμ ^d observed n times. Let u1,…,un∈ℝdu_1,…,u_n ^d denote the measurements for this landmark. We assume an isotropic noise covariance. The anisotropic case can be derived from the isotropic case with additional tools such as PCA. Further, without loss of generality, we assume that the noise standard deviation is unity; otherwise, the residuals in our derivations below would be scaled by the inverse variance throughout (since SLAM optimization assumes a fixed covariance), but the conclusion in Subsection -C remains unchanged due to cancellation. In short, u1,…,un∼i.i.d.(μ,Id).u_1,…,u_n i.i.d. N(μ,I_d). In this model, we use the average as our estimate, u¯=1n∑i=1nui. u= 1n _i=1^nu_i. The total residual (after normalizing by the variance) for all the measurements is r0=∑i=1n‖ui−u¯‖2,r_0= _i=1^n\|u_i- u\|^2, (14) which is analogous to the total residual for all measurements of a landmark in SLAM. -B Reduction Formula for Any Split Suppose the measurements are split into two nonempty groups G1G_1 and G2G_2 with sizes n1=|G1|n_1=|G_1| and n2=|G2|n_2=|G_2| (n1+n2=n_1+n_2=n). Each has its own average estimate, u¯1=1n1∑i∈G1ui,u¯2=1n2∑i∈G2ui. u_1= 1n_1 _i∈ G_1u_i, u_2= 1n_2 _i∈ G_2u_i. The total residual after splitting is now rsplit=∑i∈G1‖ui−u¯1‖2+∑i∈G2‖ui−u¯2‖2.r_ split= _i∈ G_1\|u_i- u_1\|^2+ _i∈ G_2\|u_i- u_2\|^2. (15) Lemma 1 (Residual Reduction) For any split G1G_1 and G2G_2, r0−rsplit=n1‖u¯1−u¯‖2+n2‖u¯2−u¯‖2.r_0-r_ split=n_1\| u_1- u\|^2+n_2\| u_2- u\|^2. Equivalently, r0−rsplit=n1n2n‖u¯1−u¯2‖2.r_0-r_ split= n_1n_2n\| u_1- u_2\|^2. (16) Proof: Expanding ∑i∈G1‖ui−u¯1‖2 _i∈ G_1\|u_i- u_1\|^2, we obtain ∑i∈G1‖ui‖2+∑i∈G1‖u¯1‖2−2∑i∈G1uiu¯1 _i∈ G_1\|u_i\|^2+ _i∈ G_1\| u_1\|^2-2 _i∈ G_1u_i T u_1 =∑i∈G1‖ui‖2+n1‖u¯1‖2−2n1‖u¯1‖2 = _i∈ G_1\|u_i\|^2+n_1\| u_1\|^2-2n_1\| u_1\|^2 =∑i∈G1‖ui‖2−n1‖u¯1‖2, = _i∈ G_1\|u_i\|^2-n_1\| u_1\|^2, and similarly for ∑i∈G2‖ui−u¯2‖2 _i∈ G_2\|u_i- u_2\|^2. After merging terms, rsplit r_ split =∑i=1n‖ui‖2−n1‖u¯1‖2−n2‖u¯2‖2 = _i=1^n\|u_i\|^2-n_1\| u_1\|^2-n_2\| u_2\|^2 =∑i=1n‖ui−u¯‖2−n1‖u¯1‖2−n2‖u¯2‖2 = _i=1^n\|u_i- u\|^2-n_1\| u_1\|^2-n_2\| u_2\|^2 −n‖u¯‖2+2n‖u¯‖2 \ \ \ -n\| u\|^2+2n\| u\|^2 =r0+n1(‖u¯‖2−‖u¯1‖2)+n2(‖u¯‖2−‖u¯2‖2). =r_0+n_1(\| u\|^2-\| u_1\|^2)+n_2(\| u\|^2-\| u_2\|^2). Rearranging terms, r0−rsplit r_0-r_ split =n1(‖u¯1‖2−‖u¯‖2)+n2(‖u¯2‖2−‖u¯‖2). =n_1(\| u_1\|^2-\| u\|^2)+n_2(\| u_2\|^2-\| u\|^2). (17) For the two terms in Lemma 1, n1‖u¯1−u¯‖2 n_1\| u_1- u\|^2 =n1‖u¯1‖2+n1‖u¯‖2−2n1u¯1u¯, =n_1\| u_1\|^2+n_1\| u\|^2-2n_1 u_1 T u, n2‖u¯2−u¯‖2 n_2\| u_2- u\|^2 =n2‖u¯2‖2+n2‖u¯‖2−2n2u¯2u¯. =n_2\| u_2\|^2+n_2\| u\|^2-2n_2 u_2 T u. Note the following equivalence, 2n1u¯1u¯+2n2u¯2u¯=2∑i=1nuiu¯=2(n1+n2)‖u¯‖2,2n_1 u_1 T u+2n_2 u_2 T u=2 _i=1^nu_i T u=2(n_1+n_2)\| u\|^2, which makes r0−rsplitr_0-r_ split in Lemma 1 the same as (17), n1(‖u¯1‖2−‖u¯‖2)+n2(‖u¯2‖2−‖u¯‖2)n_1(\| u_1\|^2-\| u\|^2)+n_2(\| u_2\|^2-\| u\|^2) This completes half of the proof. For the equivalence part in Lemma 1, note the following, u¯=n1u¯1+n2u¯2n, u= n_1 u_1+n_2 u_2n, plugging which into (17) leads to r0−rsplit=n1‖u¯1‖2+n2‖u¯2‖2−‖n1u¯1+n2u¯2‖2n r_0-r_ split=n_1\| u_1\|^2+n_2\| u_2\|^2- \|n_1 u_1+n_2 u_2\|^2n =(nn1−n12)n‖u¯1‖2+(nn2−n22)n‖u¯2‖2−2n1n2nu¯1u¯2 = (n_1-n_1^2)n\| u_1\|^2+ (n_2-n_2^2)n\| u_2\|^2-2 n_1n_2n u_1 T u_2 Noting that nn1−n12=n1(n−n1)=n1n2n_1-n_1^2=n_1(n-n_1)=n_1n_2 and similarly for n2n_2, we can easily show that r0−rsplit r_0-r_ split =n1n2n(‖u¯1‖2+‖u¯2‖2−2u¯1u¯2) = n_1n_2n(\| u_1\|^2+\| u_2\|^2-2 u_1 T u_2) =n1n2n‖u¯1−u¯2‖2, = n_1n_2n\| u_1- u_2\|^2, which completes the proof. ∎ We remark that r0−rsplitr_0-r_ split is analogous to fslam⋆(K⋆)−fslam(K⋆+1)f_slam (K )-f_slam(K +1), where the second term is not necessarily optimal since SLAM optimization would distribute some residuals into the odometry term and other measurement terms so that the projected landmark positions could shift. Our splitting model is analogous to the pre-optimized value fslam(K⋆+1)f_slam(K +1). -C Geometric Half-Space Split We now consider a geometric split that divides the measurements into two halves, which is similar to the right schematic in Fig. 14. We start by choosing a unit vector v∈ℝd,‖v‖=1.v ^d, \|v\|=1. Define the scalar projection si=vui.s_i=v Tu_i. Without loss of generality, we center our measurements ui∼(0,Id),u_i N(0,I_d), which leads to si∼(0,1).s_i N(0,1). Split the samples according to the sign of this projection: G+=i:si≥0,G−=i:si<0.G_+=\i:s_i≥ 0\, G_-=\i:s_i<0\. An important assumption is that the number of measurements is large, and therefore, u¯≈μ=0 u≈μ=0. Because of this assumption, additionally we also have (I−vv)u¯+≈(I−vv)u¯−≈(I−vv)u¯≈0(I-v T) u_+≈(I-v T) u_-≈(I-v T) u≈ 0. This implies that the residual reduction r0−rsplitr_0-r_ split is only contributed by the component along the v direction, since the sum of the residuals in all orthogonal directions is unchanged from r0r_0 (14) to rsplitr_ split (15). Furthermore, due to this assumption, we have s¯+=1n+∑i∈G+si≈[s∣s≥0] s_+= 1n_+ _i∈ G_+s_i [s s≥ 0] and s¯−=1n−∑i∈G−si≈[s∣s<0] s_-= 1n_- _i∈ G_-s_i [s s<0], where s follows (0,1) N(0,1) and s=vus=v Tu for u following (0,Id) N(0,I_d). More formally, consider u, which follows (0,Id) N(0,I_d), decomposed into the component along v and the component orthogonal to v: u=sv+z,z⟂v.u=sv+z, z v. Note that z=(I−vv)uz=(I-v T)u and s=vus=v Tu are jointly Gaussian. Ffor this zero-mean isotropic Gaussian case, it can be shown that the covariance between z and s is zero, COV(s,z) (s,z) =[sz]−[s][z]=[vu(I−vv)u] =E[sz]-E[s]E[z]=E[v Tu(I-v T)u] =[(I−vv)uuv]=(I−vv)[uu]v =E[(I-v T)u Tv]=(I-v T)E[u T]v =(I−vv)(COV(u)+[u][u])v =(I-v T)(COV(u)+E[u]E[u] T)v =v−vvv=v−v‖v‖2=0. =v-v Tv=v-v\|v\|^2=0. For jointly Gaussian variables, z is then independent of s, which makes [z∣s≥0]=[z∣s<0]=[z]=0.E[z s≥ 0]=E[z s<0]=E[z]=0. Therefore, [u∣s≥0]=[sv+z∣s≥0]=[s∣s≥0]v,E[u s≥ 0]=E[sv+z s≥ 0]=E[s s≥ 0]v, and similarly, [u∣s<0]=[s∣s<0]v.E[u s<0]=E[s s<0]v. By definition, [s∣s≥0]=∫0∞s12πexp(−s22)sℙ(s≥0).E[s s≥ 0]= _0^∞s 1 2π (- s^22 )dsP(s≥ 0). Since ℙ(s≥0)=12,P(s≥ 0)= 12, we get [s∣s≥0]=2∫0∞s12πexp(−s22)s.E[s s≥ 0]=2 _0^∞s 1 2π (- s^22 )ds. Let q=s22,dq=sds.q= s^22, dq=s\,ds. Then [s∣s≥0]=22π∫0∞e−qq.E[s s≥ 0]= 2 2π _0^∞e^-qdq. Since ∫0∞e−qq=1, _0^∞e^-qdq=1, we obtain [s∣s≥0]=2π.E[s s≥ 0]= 2π. By symmetry, [s∣s<0]=−2π.E[s s<0]=- 2π. Therefore, [u∣s≥0]=2πv,E[u s≥ 0]= 2πv, and [u∣s<0]=−2πv.E[u s<0]=- 2πv. Due to the assumption of a large number of measurements, u¯+≈[u∣s≥0]=2πv, u_+ [u s≥ 0]= 2πv, and u¯−≈[u∣s<0]=−2πv, u_- [u s<0]=- 2πv, Also due to the large-sample assumption and the symmetry of an isotropic Gaussian, approximately half of the samples fall into each group: n+≈n2,n−≈n2.n_+≈ n2, n_-≈ n2. We substitute these results into Lemma 1 and obtain r0−rsplit r_0-r_ split =n+n−n‖u¯+−u¯−‖2 = n_+n_-n\| u_+- u_-\|^2 =n4‖2πv+2πv‖2 = n4\| 2πv+ 2πv\|^2 =2nπ‖v‖2 = 2nπ\|v\|^2 =2nπ. = 2nπ. -D Optimality of the Half-Space Split When the number of measurements n is large (n→∞n→∞), the half-space split maximally reduces the residual (i.e., maximizes r0−rsplitr_0-r_ split). Intuitively, in (16), n1n2=n1(n−n1)n_1n_2=n_1(n-n_1) is maximized (simply by setting the derivative to zero) when n1=n2=n2n_1=n_2= n2, which is achieved by the half-space split when n is large. When moving away from the center split, the decrease in n1n2n_1n_2 dominates the growth in ‖u¯1−u¯2‖2\| u_1- u_2\|^2 (one can reason about n1n_1 and n2n_2 through Gaussian CDF and about u¯1 u_1 and u¯2 u_2 using the previous derivations but with a non-zero threshold). Under non-ideal conditions such as n↛∞n →∞ and an anisotropic data covariance (Fig. 15 for illustration), v may not be an arbitrary direction, but the principal direction of maximum data variance. Splitting at s=0s=0 may not be optimal since the best split may shift toward outliers to maximize ‖u¯1−u¯2‖2\| u_1- u_2\|^2. The directions orthogonal to v may also contribute to the residual reduction under non-ideal conditions. Figure 15: Splitting Gaussian samples under non-ideal conditions. We now provide a more thorough derivation of the optimality of the half-space split under ideal conditions. Our objective is to maximize (16), which is valid for all splits. For convenience, we repeat it below: r0−rsplit=n1n2n‖u¯1−u¯2‖2.r_0-r_ split= n_1n_2n\| u_1- u_2\|^2. There are two parts: (i) showing that splitting at s=0s=0 is optimal among all the half-space splits, and (i) showing that the half-space splits are optimal among all splits. For the first part, we consider a general half-space split at s=ts=t instead of at s=0s=0: G1=i:si<t,G2=i:si≥t.G_1=\i:s_i<t\, G_2=\i:s_i≥ t\. The objective is to show that t=0t=0 maximizes r0−rsplitr_0-r_ split. Before proceeding with the derivation, we introduce several definitions and state a few properties that will be useful later in the derivation. Let ϕ(s)φ(s) denote the standard Gaussian probability density function, and let Φ(s) (s) denote the cumulative distribution function of a standard Gaussian. It is easy to show that ϕ′(s)=−sϕ(s),φ (s)=-sφ(s), (18) Φ′(s)=ϕ(s). (s)=φ(s). (19) Our objective function maxtr0−rsplit _tr_0-r_ split can be written as maxtn1n2n‖u¯1−u¯2‖2=n(maxtn1n2n2‖u¯1−u¯2‖2). _t n_1n_2n\| u_1- u_2\|^2=n ( _t n_1n_2n^2\| u_1- u_2\|^2 ). Assuming a large number of measurements, n1n n_1n ≈∫−∞tϕ(s)s=Φ(t), ≈ ^t_-∞φ(s)\,ds= (t), n2n n_2n ≈1−Φ(t), ≈ 1- (t), u¯1 u_1 ≈[u∣s<t]=∫−∞tsϕ(s)s∫−∞tϕ(s)sv=(18)(19)−ϕ(t)Φ(t)v, [u s<t]= ^t_-∞sφ(s)\,ds ^t_-∞φ(s)\,dsv eq: gausspdfderiv eq: gausscdfderiv= -φ(t) (t)v, u¯2 u_2 ≈[u∣s≥t]=∫t∞sϕ(s)s∫t∞ϕ(s)sv=(18)(19)ϕ(t)1−Φ(t)v. [u s≥ t]= ^∞_tsφ(s)\,ds ^∞_tφ(s)\,dsv eq: gausspdfderiv eq: gausscdfderiv= φ(t)1- (t)v. We substitute the above equalities into the objective function: maxtn1n2n2‖u¯1−u¯2‖2 _t n_1n_2n^2\| u_1- u_2\|^2 (20) ≈maxtΦ(t)[1−Φ(t)]ϕ2(t)[−1Φ(t)−11−Φ(t)]2‖v‖2 ≈ _t (t) [1- (t) ]φ^2(t) [- 1 (t)- 11- (t) ]^2\|v\|^2 =maxtϕ2(t)Φ(t)[1−Φ(t)]=maxth(t). = _t φ^2(t) (t) [1- (t) ]= _th(t). Using the following properties: ϕ(−t)=ϕ(t),φ(-t)=φ(t), (21) Φ(−t)=1−Φ(t), (-t)=1- (t), (22) h(t)h(t) is symmetric about t=0t=0. To show that argmaxth(t)=0 _th(t)=0, it is sufficient to show (i) dlogh(t)dt|t=0=0 . d h(t)dt |_t=0=0, and (i) dlogh(t)dt|t>0<0 . d h(t)dt |_t>0<0. Taking the derivative: logh(t)=(22)2logϕ(t)−logΦ(t)−logΦ(−t), h(t) eq: gausscdfsymm=2 φ(t)- (t)- (-t), dlogh(t)dt d h(t)dt =(18)(19)−2tϕ(t)ϕ(t)−ϕ(t)Φ(t)+ϕ(t)Φ(−t) eq: gausspdfderiv eq: gausscdfderiv=-2 tφ(t)φ(t)- φ(t) (t)+ φ(t) (-t) (23) =−2t+ϕ(t)[1Φ(−t)−1Φ(t)]. =-2t+φ(t) [ 1 (-t)- 1 (t) ]. From (23), it is easy to check that dlogh(t)dt|t=0=0 . d h(t)dt |_t=0=0. Next, we show dlogh(t)dt|t>0<0 . d h(t)dt |_t>0<0, i.e., ϕ(t)[1Φ(−t)−1Φ(t)]<2tfort>0.φ(t) [ 1 (-t)- 1 (t) ]<2t \ t>0. (24) We define the inverse Mills ratio, λ(t)=ϕ(t)Φ(−t),λ(−t)=(21)ϕ(t)Φ(t).λ(t)= φ(t) (-t), λ(-t) eq: gausspdfsymm= φ(t) (t). Therefore, ϕ(t)[1Φ(−t)−1Φ(t)] φ(t) [ 1 (-t)- 1 (t) ] =λ(t)−λ(−t) =λ(t)-λ(-t) (25) =∫−tλ′(x)xfort>0. = ^t_-tλ (x)\,dx \ t>0. From (25), to show that dlogh(t)dt|t>0<0 . d h(t)dt |_t>0<0, or equivalently (24), we require λ′(t)<1λ (t)<1 and t>0t>0 (so that the integral from −t-t to t is valid). We therefore take the derivative of λ(t)λ(t): λ′(t)= λ (t)= ϕ′(t)Φ(−t)+ϕ(t)Φ′(−t)Φ(−t)2 φ (t) (-t)+φ(t) (-t) (-t)^2 (26) =(18)(19) eq: gausspdfderiv eq: gausscdfderiv= −tϕ(t)Φ(−t)+ϕ(t)ϕ(t)Φ(−t)2 -tφ(t) (-t)+φ(t)φ(t) (-t)^2 = = ϕ(t)Φ(−t)[ϕ(t)Φ(−t)−t] φ(t) (-t) [ φ(t) (-t)-t ] = = λ(t)[λ(t)−t]. λ(t) [λ(t)-t ]. Note the following equalities: [s∣s≥t]=∫t∞sϕ(s)s∫t∞ϕ(s)s=(18)(19)(22)ϕ(t)Φ(−t)=λ(t).E[s s≥ t]= ^∞_tsφ(s)\,ds ^∞_tφ(s)\,ds eq: gausspdfderiv eq: gausscdfderiv eq: gausscdfsymm= φ(t) (-t)=λ(t). [s2∣s≥t] [s^2 s≥ t] =∫t∞s2ϕ(s)s∫t∞ϕ(s)s=∫t∞s2ϕ(s)sΦ(−t) = ^∞_ts^2φ(s)\,ds ^∞_tφ(s)\,ds= ^∞_ts^2φ(s)\,ds (-t) =−sϕ(s)|t∞+∫t∞ϕ(s)sΦ(−t) = .-sφ(s) |^∞_t+ ^∞_tφ(s)\,ds (-t) =tϕ(t)+Φ(−t)Φ(−t) = tφ(t)+ (-t) (-t) =tλ(t)+1, =tλ(t)+1, where we have used integration by parts. Further, Var(s∣s≥t) (s s≥ t) =[s2∣s≥t]−[s∣s≥t]2 =E[s^2 s≥ t]-E[s s≥ t]^2 =tλ(t)+1−λ(t)2>0, =tλ(t)+1-λ(t)^2>0, since Var(s∣s≥t)>0Var(s s≥ t)>0. The above inequality leads to λ(t)2−tλ(t)=(26)λ′(t)<1.λ(t)^2-tλ(t) eq: lambdaderiv=λ (t)<1. Therefore, using (25), ϕ(t)[1Φ(−t)−1Φ(t)]=λ(t)−λ(−t) φ(t) [ 1 (-t)- 1 (t) ]=λ(t)-λ(-t) =∫−tλ′(x)x<∫−t1=2tfort>0. = ^t_-tλ (x)\,dx< ^t_-t1=2t \ t>0. Consequently, (24) holds and dlogh(t)dt|t>0<0 . d h(t)dt |_t>0<0 holds. We conclude that argmaxth(t)=0 _th(t)=0. Next, we show that the half-space splits are optimal among all splits. We continue to assume a large number of measurements and generalize the previous derivation to arbitrary splits: n1n n_1n ≈∫G1ϕ(u)du=ℙ(G1)=p, ≈ _G_1φ(u)\,d^du=P(G_1)=p, n2n n_2n ≈1−p, ≈ 1-p, u¯1 u_1 ≈[u∣u∈G1]=∫G1uϕ(u)du∫G1ϕ(u)du=∫G1uϕ(u)dup, [u u∈ G_1]= _G_1uφ(u)\,d^du _G_1φ(u)\,d^du= _G_1uφ(u)\,d^dup, u¯2 u_2 ≈[u∣u∈G2]=∫G2uϕ(u)du∫G2ϕ(u)du=∫G2uϕ(u)du1−p. [u u∈ G_2]= _G_2uφ(u)\,d^du _G_2φ(u)\,d^du= _G_2uφ(u)\,d^du1-p. By the law of total expectation, p[u∣u∈G1]+(1−p)[u∣u∈G2] pE[u u∈ G_1]+(1-p)E[u u∈ G_2] =∫ℝduϕ(u)du=[u]=0. = _R^duφ(u)\,d^du=E[u]=0. Using the equalities above, the objective function can be written as maxG1n1n2n2‖u¯1−u¯2‖2 _G_1 n_1n_2n^2\| u_1- u_2\|^2 ≈ ≈ maxG1p(1−p)∥[u∣u∈G1]−p−1[u∣u∈G1]∥2 _G_1p(1-p) \|E[u u∈ G_1]- pp-1E[u u∈ G_1] \|^2 = = maxG1p1−p∥[u∣u∈G1]∥2 _G_1 p1-p \|E[u u∈ G_1] \|^2 = = maxpmaxG1:ℙ(G1)=p1−p∥[u∣u∈G1]∥2, _p _G_1:P(G_1)=p p1-p \|E[u u∈ G_1] \|^2, where the last equality decomposes the maximization into two steps and makes the following observation clearer: for a fixed p, we need to maximize ∥[u∣u∈G1]∥ \|E[u u∈ G_1] \| in order to maximize the objective function. Let v be: v=[u∣u∈G1]∥[u∣u∈G1]∥.v= E[u u∈ G_1] \|E[u u∈ G_1] \|. (27) Substituting in v yields: maxG1:ℙ(G1)=p∥[u∣u∈G1]∥ _G_1:P(G_1)=pp \|E[u u∈ G_1] \| =maxG1:ℙ(G1)=pp(v[u∣u∈G1]) = _G_1:P(G_1)=pp (v TE[u u∈ G_1] ) =maxG1:ℙ(G1)=p∫G1vuϕ(u)du = _G_1:P(G_1)=p _G_1v Tuφ(u)\,d^du =s=vumaxG1:ℙ(G1)=p∫G1sϕ(u)du s=v Tu= _G_1:P(G_1)=p _G_1sφ(u)\,d^du =maxG1:ℙ(G1)=p∫s:(s,z)∈G1[∫z:(s,z)∈G1ϕ(z)d−1z]sϕ(s)ds = _G_1:P(G_1)=p _s:(s,z)∈ G_1 [ _z:(s,z)∈ G_1φ(z)\,d^d-1z ]sφ(s)\,ds =maxG1:ℙ(G1)=p∫s:(s,z)∈G1s[∫ℝd−1ϕ(z)d−1z]ϕ(s)ds, = _G_1:P(G_1)=p _s:(s,z)∈ G_1s [ _R^d-1φ(z)\,d^d-1z ]φ(s)\,ds, where the penultimate equality decomposes the integral into the coordinate along v and the remaining d−1d-1 orthogonal coordinates, using the isotropic Gaussian assumption for u and the resulting independence between coordinates. The last equality considers the following maximization process: starting from the largest possible value of s, we include all points in the orthogonal coordinates to assign as much probability mass as possible to each value of s before including points with smaller values of s. The process continues until the total probability mass equals p. Therefore, argmaxG1:ℙ(G1)=p∥[u∣u∈G1]∥ G_1:P(G_1)=p \|E[u u∈ G_1] \| = = argmaxG1:ℙ(G1)=p∫s∣∀z∈ℝd−1,(s,z)∈G1sϕ(s)ds G_1:P(G_1)=p _\s ∀ z ^d-1,\,(s,z)∈ G_1\sφ(s)\,ds (28) = = u:vu=s≥t, \u:v Tu=s≥ t\, (29) where t is selected such that ℙ(G1)=pP(G_1)=p. Note that (28) is essentially a one-dimensional problem, so it is obvious that we need to include the largest possible values of s to maximize the objective, leading to s≥ts≥ t as the solution. It is also worthwhile to mention that v is defined in (27) as a way to rewrite ∥[u∣u∈G1]∥ \|E[u u∈ G_1] \| for the derivation. Because of the isotropic Gaussian assumption, v may point in any direction. In other words, the solution is not unique. Any half-space split satisfying (29) and ℙ(G1)=pP(G_1)=p is optimal.