{'fraction_non_alphanumeric': 0.0346718903036239, 'fraction_numerical': 0.008063989552726086, 'mean_word_length': 4.4214159292035395, 'pattern_counts': {'":': 0, '<': 2, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 7, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Estimation with Fast Landmark Selection in Robot Visual Navigation

We consider the visual feature selection to improve the estimation quality required for the accurate navigation of a robot. We build upon a key property that asserts: contributions of trackable features (landmarks) appear linearly in the information matrix of the corresponding estimation problem. We utilize standard models for motion and vision system using a camera to formulate the feature selection problem over moving finite time horizons. A scalable randomized sampling algorithm is proposed to select more informative features (and ignore the rest) to achieve a superior position estimation quality. We provide probabilistic performance guarantees for our method. The time-complexity of our feature selection algorithm is linear in the number of candidate features, which is practically plausible and outperforms existing greedy methods that scale quadratically with the number of candidates features. Our numerical simulations confirm that not only the execution time of our proposed method is comparably less than that of the greedy method, but also the resulting estimation quality is very close to the greedy method.

Abstract-We consider the visual feature selection to improve the estimation quality required for the accurate navigation of a robot. We build upon a key property that asserts: contributions of trackable features (landmarks) appear linearly in the information matrix of the corresponding estimation problem. We utilize standard models for motion and vision system using a camera to formulate the feature selection problem over moving finite time horizons. A scalable randomized sampling algorithm is proposed to select more informative features (and ignore the rest) to achieve a superior position estimation quality. We provide probabilistic performance guarantees for our method. The time-complexity of our feature selection algorithm is linear in the number of candidate features, which is practically plausible and outperforms existing greedy methods that scale quadratically with the number of candidates features. Our numerical simulations confirm that not only the execution time of our proposed method is comparably less than that of the greedy method, but also the resulting estimation quality is very close to the greedy method.

I. INTRODUCTION
Safe and robust navigation is one of the fundamental problems in robotics. The recent technological advances in the computing devices have opened up new opportunities and made several breakthroughs possible in this research area [1]. However, the research efforts to devise scalable algorithms for planning and perception during robot navigation is still immature. Applications of mobile robots will, at least for a few years, suffer from onboard computational and power limitations. Even if these physical or computational boundaries are pushed further away, the demand for agility and higher levels of autonomy always mandates us to execute onboard procedures in shorter periods of time.
One of the essential subproblems during robot navigation is to solve the localization, mapping, and visual odometry at an acceptable level of accuracy while spending a minimal amount of computational resources [2]. To achieve this goal, many researchers have investigated visual feature selection problem [3]- [10]. The underlying idea is that depending on the current state of the robot and planned motion in the near future (i.e., the task), tracking certain features across a time horizon can be more informative than tracking other features. In other words, certain visual features may deserve more attention compared to the rest. In this regard, [11] uses a greedy method to select a subset of pre-identified visual landmarks which facilitate the pose estimation of the robot. In [7], the authors combine solving the simultaneous localization and mapping (SLAM) using the unscented Kalman filtering with reinforcement learning. Their approach generates policies that govern the feature selection. In [10], a two-stage methodology for measurement planning is discussed. The first stage is the selection of the subset of landmarks for observation, which is followed by the design of observation times for each feature. In [5], the authors consider the problem of task-aware design of a subset of features such that an uncertainty metric is minimized. In [12], the authors consider a visual-inertial navigation problem. They analyze the problem of feature-selection for superior performance, where the design variable is the features that will be tracked during a fixed time-horizon. They use convex relaxations as well as the greedy method of selection and bring performance guarantees for the quality of estimation using the selected features.
In this paper, we aim at solving the problem of feature selection out of a given set of candidates in order to achieve superior estimation and anticipation during the navigation. The navigation setup consists of a robot that moves based on generated position estimates. The robot is assumed to use an onboard camera to (passively) track selected features in a fixed time horizon to improve the quality of the estimation. The underlying vision system model and the resulting feature selection problem are similar to those in [12], while instead of using the greedy method and convex relaxations, we propose a random sampling algorithm for feature selection. In our approach, each available feature is assigned a sampling probability, which is used for finding a set of features via sampling. We demonstrate that the proposed method provides us with theoretical guarantees on the quality of the estimation. It turns out that time-complexity of our randomized sampling algorithm scales linearly with the number of available features, while time complexity of the greedy method of [12] scales quadratically for the exact same problem. Numerical simulations confirm that the estimation quality using features provided by our randomized sampling is very close to the quality of estimation provided by the greedy method, while the required time to run our randomized sampling algorithm is significantly less than the greedy method.
The rest of this paper is organized as follows. In the remaining of this section, we introduce some notations that are used throughout the paper. In Section II, we portray the big picture of the research problem that is being addressed in this paper. In Section III, we discuss the details of the motion and vision models. Then, we define three estimation measures that quantify the quality of estimation based on the feature selection problem. In Section IV, we propose an algorithm for feature selection and conduct performance and time-complexity analysis for our approach. In Section V, a numerical case is explained, wherein we compare the performance of the proposed feature selection method against the totally random choice of features and the greedy method. Finally, the final remarks are discussed in Section VI. The proofs of the theoretical results are given in the appendix. Notations and Preliminaries: The set of nonnegative integer and real numbers are denoted by Z + and R + , respectively. The vectors and matrices are denoted by lower-case and uppercase letters, respectively (e.g. x and X). The identity matrix of size n is denoted by I n . The set of positive definite matrices of size n is denoted by S n ++ . The partial ordering on the cone of positive-semidefinite matrices is denoted by , , ≺, and operators. The block-diagonal matrix with diagonal elements X 1 , . . . , X N is denoted by diag(X 1 , . . . , X N ). For a set S, |S| denotes its cardinality. For a map g, ∇ x g denotes the corresponding partial derivative. A Gaussian random variable with mean vector µ and covariance matrix Σ is denoted by N (µ, Σ). X ⊗ Y denotes the Kronecker product of matrices X and Y . The special orthogonal group in 3 dimensions is denoted by SO(3).

II. PROBLEM STATEMENT
We formulate the feature selection problem for visual navigation of robots. The spatial location of a robot at time t ∈ Z + is denoted by x t ∈ R 3 . For a given positive integer T , the vector of future states over the discrete time horizon [t, t + T ] = t, t + 1, . . . , t + T is represented by Since robot motion creates uncertainty, having access to the statistics of x t,T will help us measure quality of our prediction of robot whereabouts over the time horizon [12]. As it is shown in Subsection III-A, one can obtain mean vector µ t,T ∈ R 3(T +1) and covariance matrixΣ t,T ∈ S 3(T +1) ++ of x t,T under popular Gaussianity assumption. These quantities can be equivalently transformed into more relevant forms for the feature selection problem, namely, information vector and matrix, which are given by [2] b t,T =μ T t,TΣ There is a one-to-one correspondence between mean vector and covariance matrix and their counterparts information vector and information matrix. A striking property of the latter representation is that the contribution of each feature (or landmark) to the information vector and matrix is fused linearly [13]. Having the prior estimation parameters (1)-(2), as it is shown in Subsection III-B, the quality of estimation for x t,T can be improved by fusing information of newly observed visual features using an onboard camera. The updated information matrix and vector are in which b f t,T and H f t,T are contributions of feature f to the overall information matrices of the estimation problem. The set of all identifiable features (landmarks) at time t, which can be triangulated using multiple frames over the time horizon [t, t+T ], is denoted by Θ t . Suppose that |Θ t | = N t is assumed to be large.
Tracking a large number of features (landmarks) for accurate navigation usually requires substantial onboard computational power [4]. As a result, a desirable navigation objective is to select and track a small subset of features that are more informative, while providing an acceptable estimation quality. Suppose that robot is only capable of tracking at most q, which is comparably less than N t , features during the horizon.
Then, the feature selection problem can be formulated as where ρ : S

3(T +1) ++
→ R is a monotone decreasing map that measures the estimation quality.
The optimization problem (5)-(6) is combinatorial and usually NP-hard. The research problem is to propose a scalable algorithm that provides solutions for (5)-(6) with performance guarantees.

III. MODELS FOR ROBOT MOTION AND VISION SYSTEM
In the following, we discuss details of models for the motion of the robot and the onboard vision system.

A. Statistics of Robot Motion
The goal is to calculate information vector and matrix of x t,T when dynamics of robot evolves over time horizon [t, t + T ]. To achieve this, we utilize a model that is inspired by the dynamic model analyzed in [13]. Suppose that dynamics of the robot's position is governed by for all τ ∈ [t + 1, t + T ], where g : R 3 × R 3 → R 3 is a (possibly nonlinear) known map, where u τ in the control command at time τ , and δ τ ∼ N (0, Λ τ ) is a temporally independent random process that captures the aggregate effect of all uncertainties induced by the robot motion. It is assumed that a feedback control law with the following structure is given that ensures the robot with dynamics (7) tracks a reference path, at least in the absence of uncertainties, with some desired accuracy. The command u ref τ may have to be pre-filtered to enhance the tracking performance. Since robot's position x τ −1 is a random variable and its true value is unknown, we use its mean valueμ τ −1 as a faithful estimate of its value in (8) to obtainū In presence of uncertainties, the trajectory of the closed-loop system (7)-(9) will fluctuate around the reference path and the tracking quality will depend on the quality of estimationμ τ −1 .
The control mechanism (9) is merely using the initial statistics of position of the robot. In the next subsection, we show that incorporating new information obtained from observing features (landmarks) will help us improve the estimation quality, which in turn will improve the path tracking quality.
Suppose that the current, i.e., before accounting for the dynamics of the robot, pose estimates for x t is described by mean vector µ t and covariance matrix Σ t . For the time step starting at τ = t in the horizon, let us setμ t = µ t and Σ t = Σ t . For the next steps, we define the composed map Then, upon linearizing the dynamics of the system at working in which vector∆ τ and A τ are given bȳ for all τ ∈ [t + 1, t + T ].
Lemma 1: By settingμ t = µ t andΣ t = Σ t , the mean and covariance of x t,T is given bȳ We can substitute (14) and (15) into (1) and (2) to calculate information vectorb t,T and matrixH t,T . In the next subsection, it is shown that these vectors and matrices will be updated upon receipt of certain information about the observed features over the time horizon [t, t + T ].

B. Camera Model for Feature Tracking
We employ the observation model proposed by in [12] for an onboard camera. For every τ ∈ [t, t + T ], let us denote orientation of the robot by rotation matrix R τ ∈ SO (3), orientation of the camera with respect to the robot by rotation matrix R c ∈ SO(3), translation of the camera with respect to the robot pose by x c ∈ R 3 , the unit vector corresponding to pixel measurement of feature f ∈ Θ t at time τ by u f τ,T ∈ R 3 , and the position vector of the feature by y f ∈ R 3 . We recall that the corresponding skew-symmetric matrix induced by As it is discussed in [12], one may reasonably assume that the observation vector in the image and its counterpart in real world are collinear (i.e., parallel). However, due to existence of noise in the process, one may consider a disrupted version of this assumption by considering the following noisy observation model where η f τ,T ∼ N (0, σ 2 I 3 ). The observation model (16) can be rewritten as The camera takes one frame at every time instant over time horizon [t, t + T ]. With knowledge of planned motion (i.e., location and orientation) for robot over the time horizon, suppose that robot is capable of running forward simulations to determine a feature will be visible in n f frames out of all T + 1 frames over the time horizon. By considering relation (17) for such visible features, one can stack all these equations and write them in more compact form for some appropriate matrices F f t,T and E f t,T . A given apriori information matrixH t,T , which is obtained from (14), can be updated by fusing information of a visible feature {f } according to the following rule [12] where the linearly added information matrix is given by time t time t+2 Fig. 1: The schematic of the motion and vision model adopted in this paper at three consecutive snapshots. The location of the robot is denoted by letter R, which moves and changes its orientation across three frames. The features are denoted by letter f . As a result of this movement, the visible features (those in between the dashed lines) in each frame will vary. Set Θt will consist of all features that can be triangulated during this horizon.
Lemma 2: The information vector of x t,T upon tracking feature f ∈ Θ t is updated according to where matrix B f t,T is given by Since contributions of different features are independent of each other, for a selected subset of features Φ t ⊂ Θ t , one can verify that the updates to the information matrix and vector upon the choice of these features are given by The corresponding mean vector and covariance matrix for the estimation problem can be calculated through In Fig. 1, we illustrate the essence of the motion and vision model described in this section. In order to specify the set of available features Θ t for tracking, the robot needs to determine which features can be triangulated and which ones will result in invertible information matrices (E f t,T ) T E f (cf. [12]).

C. Estimation Measures
Given a subset of trackable features Φ t , we can quantify the quality of resulting estimation in various meaningful ways.
(i) Variance of the Error: Given the covariance matrix, the variance of error equals the sum of the variances of all scalar components of vector x t,T . This measure can be characterized as (ii) Differential Entropy of the Estimation Error: It is known that the differential entropy of a multivariate Gaussian random variable with covariance Σ is This measure quantifies the uncertainty volume of the estimation error, which is given by (iii) Spectral Variance: Let us consider the eigen-space of the largest eigenvalue of the covariance matrix of the estimator. This is the subspace across which the estimation is less accurate than the rest of the directions. Thus, we can use the following estimation measure All these measures are monotonically decreasing. They are also spectral functions, i.e., they only depend on the eigenvalues of the information or covariance matrices. Therefore, having lower and upper bounds for the covariance matrix can be potentially useful to obtain similar bounds for these estimation measures. Measures (ii) and (iii) have been also discussed in [12].

IV. FEATURE SELECTION VIA RANDOMIZED SAMPLING
We propose a scalable algorithm that provides feasible solutions for (5)-(6) with provable performance bounds.

A. Leverage Scores and Induced Probabilities
Each available feature f ∈ Θ t is assigned some nonnegative numbers, which are so-called leverage scores, that are closely related to the notion of effective resistances in graph sparsification problem [14]. The maximal information matrix of x t,T , over the cone of positive-definite matrices, corresponds to the case where all features are employed in the estimation process. This matrix is given by Therefore, for any Φ t ⊂ Θ t , it holds that Hence, for every monotone decreasing map ρ : S Using the maximal matrix, we definē for every f ∈ Θ t with |Θ t | = N t . Definition 2: For a given set of features Θ t , the leverage scores are nonnegative numbers that are defined by for every feature f ∈ Θ t .
One can associate a probability mass function denoted by π : Θ t → [0, 1] to elements of Θ t by setting where n = 3(T + 1) is the dimension ofH t,T . The resulting function is a well-defined probability mass function as we have

B. Sampling Algorithm
We describe details of our algorithm to select a subset of the most informative features over each moving time horizon. This algorithm is inspired by the graph sparsification using effective resistances [14], [15], where the goal is to construct a sparse weighted graph based on a given weighted graph by ensuring that the Laplacians of the two graphs stay spectrally close to each other. A variation of this method appears in [16] for the case of rank-one selected matrices when the constant termH t,T is zero.
The steps of our method are given in Algorithm 1. First, we iteratively and independently sample a feature from Θ t with replacement for q iterations. This sampling takes place according to probability mass function π, which is defined by (30). The sampled feature f is added to Φ t provided that it has not been sampled before. At the end of the procedure, set of selected features Φ t will have at most q elements.
Remark 1: This randomized feature selection algorithm can be applied independently of the model of robot motion which was described in Section III-A. For instance, our featureselection approach can replace the feature selection routines in the inertial-visual navigation setup described in [12] that are based on the greedy method and convex relaxations.

Algorithm 1 Randomized Feature Selection
input: initial information matrixH t,T set of available features Θ t , number of samples q output: selected features Φ t , information matrix H t,T initialize: Φ t = ∅, H t,T =H t,T for k = 1 to q do sample a feature from Θ t using distribution π → f select the corresponding matrix then add f to Φ t update the information matrix:

C. Performance Guarantee
It turns out that Algorithm 1 provides us with an information matrix that is a constant-factor approximation to the maximal information matrix H t,T (Θ t ) given by (26).
Theorem 1: For a given parameter ∈ (0, 1), suppose that Algorithm 1 is executed with a fixed q = O(n log n/ 2 ) < N t . Then, the resulting information matrix, see (21), based on the resulting set of features Φ t , satisfies with probability at least 1/4 for a numberχ. The proof of this theorem and definition ofχ is rather involved and inspired by [14]. The spectral bound (31) can be used to obtain performance bounds for the estimation measures.
Theorem 2: Under the settings of Theorem 1, the estimation quality losses compared to the case where all features in Θ t are used satisfy with probability at least 1/4.

D. Implementation of Algorithm
The nature of the performance guarantees provided in Theorem 1 and 2 motivates us to run the algorithm with multiple random seeds, i.e., by conducting Monte-Carlo simulations. To this end, we choose a design that corresponds to the minimal value of the estimation measure of interest. We inspect that there are (at least) two steps during the feature selection process that are amenable to parallel implementation: (i) evaluation of sampling probabilities π f for different features can be done in parallel, and (ii) independent executions of Algorithm 1 for the purpose of finding different designs can be conducted in parallel.

E. Time-Complexity Analysis
To find the sampling probabilities, we need O(N t T 3 ) operations, where N t = |F t | is the number of available features at time t. One execution of Algorithm 1 requires O(qT 2 ) operations. Evaluation of any of these estimation measures requires O(T 3 ) operations. Therefore, if we run p independent samples of this algorithm, we will need O(pqT 2 + pT 3 ) operations. Hence, the overall feature selection will require O(N t T 3 + pqT 2 + pT 3 ) operations.
For comparison purposes, we also analyze the time complexity of feature selection using the greedy method of [12]. For this method, iteratively, we should examine all candidates and find the feature whose addition will enhance the estimation quality more than the remaining features. This method requires O(qN t T 3 ) operations. In the worst-case, q = O(N t ). Thus, in the worst case, its time complexity is O(N 2 t T 3 ), i.e., quadratic in the number of available features. This suggests that the random sampling using the leverage scores can potentially be faster than the greedy method (see next section for a numerical example).

V. TEST CASE
We bring the details of a numerical experiment, which is conducted to demonstrate the effectiveness of the featureselection approach via Algorithm 1.

A. Model and Environment Description
We consider a robot that is translating and rotating. Let us denote its position vector by x T τ = [p τ , y τ , z τ ] 1 . We suppose that the high-level dynamics of robot follow where u T τ := [u p τ , u y τ , u z τ ] is the input command signal and δ T τ := [δ p τ , δ y τ , δ z τ ] is the random process describing the uncertainty propagation due to the motion of the robot (compare to (7)). We consider the robot that is planning to move in the following reference path which looks like a deformed 3-D number 8 (see Fig. 3). To set the control inputs, we set Moreover, we suppose that the Euler angles describing the absolute orientation of the camera at time τ are given by where the sequence of rotations is z-y-p. The visible landmarks in the environments consists of 1752 points in the space, which is constructed by by putting a circular array of randomly sampled points in a 3-D model of a room 2 . Two views from a snapshot of the environment have been illustrated in Fig. 2 and Fig. 3. We set parameters R = 7500, ω = 0.08, ω r = 0.0064, σ = 0.1 and Λ t = diag (4,4,16) and initial covariance to be Σ 0 = I.

B. Different Approaches for Feature Selection
We consider the navigation setup for 400 time horizons each of length T = 20. The overall simulation consists of doing almost 107 full turns around the 3-D path of interest. For each horizon, after finding the eligible features to track (i.e., features with full rank information matrix (E f t,T ) T E f t,T ), we select at most half of the features. For each horizon, we do this task via three different methods: (i) randomized sampling by leverage scores: we run Algorithm 1 for p = 50 independent experiments and choose the set Φ t which induces the minimal value of the estimation measure. We denote the CPU time spent on this task by τ t .
(ii) randomized sampling using uniform probabilities: we run Algorithm 1 for p = 50 independent experiments, except that instead of evaluating the probabilities, we assume an equal probability for each feature to be sampled. Similar to the previous case, we choose the design that induces minimal value of the estimation measure. We denote this design by Φ u t , where u stands for uniform probabilities. Similarly, we denote the corresponding CPU time for this task by τ u t . (ii) greedy method: the features are added one-by-one, where at each iteration the feature which enhances the estimation quality the most is selected [12]. This method produces a single design for the feature selection denoted by Φ g t , where g stands for the greedy method.
In this example, we consider the estimation measure ρ v as the monotone function governing the feature selection.

C. Metrics for Comparison of Methods
Many researchers have observed that the greedy method over-performs other approaches in several similar combinatorial problems [12], [17]. In general, the brute-force method in these settings is computationally infeasible 3 , we select the greedy method as the base approach. Moreover, to have a clear understanding of the error in the position, we define the root mean squared error (RMSE) as we define for t ∈ {0, T, 2T, . . . }. We define similar error indices for the uniform random (method (ii)) and the greedy method (method (iii)) as well and denote them by θ u t,T and θ g t,T , respectively. To compare the relative difference of these values, we use Similarly, we define φ u t,T , which compares the value of RMSE resulting from the totally random choice of feature with greedy selection. Finally, to compare the CPU times, we look at which represent the ratio of the CPU time spent in methods (i) to the one spent by the greedy method. Similarly, we use κ u t to compare the time spent by method (ii) with method (iii).

D. Numerical Results
In Fig 4, we show the resulting values of the estimation measure versus time, which demonstrate that the estimation measure resulting from Algorithm 1 is almost identical to the estimation measure resulting from the greedy method. This is not the case for the totally random choice of features, in which larger spikes can be observed.
In Fig. 5, we illustrate the values of the RMS error versus times for these methods. The errors corresponding to the feature selection using Algorithm 1 is very close to those values for the greedy method, while the totally random method may result in larger errors. This shows that in this example, not only the choice of features is a non-trivial computational task, but also our algorithm functions with quality and reliability that is very close to these factors in the case of the greedy method. The metric φ t,T is also illustrated in Fig. 6, which quantifies these deviations.
Finally, we compare the CPU times spent on each method. In Fig. 7, we demonstrate the experimental CDF's of the parameters κ t and κ u t , which show that the randomized methods are considerably faster than the greedy method in most cases. For instance, these data suggest that Algorithm 1 has been more than 20 times faster than the greedy method in about 85% of the assigned tasks, while in most cases it has been least 10 times faster. Note that the time includes running the We propose a randomized algorithm for visual feature selection over a fixed-length moving time horizon. The idea is to associate a sampling probability to each candidate feature, randomly sample a subset of features according to these probabilities for a number of independent experiments, and select the outcome with the best estimation quality. We would like to discuss a few remarks.
If the estimation measure enjoys submodularity, then the greedy method provides a performance guarantee compared to the optimal solution [12], [18]. However, it is known that certain measures, e.g., ρ v , are not submodular [19]. Moreover, in our work, we offer a different type of performance guarantee. Theorem 2 compares the estimation quality to the case that we leverage all features for tracking. Nevertheless, our extensive numerical simulations assert that the resulting estimation quality from our algorithm and that of the greedy method are often close to each (see Fig. 4 for example). Further research is required to uncover the practical and theoretical differences of this randomized algorithm and the greedy method.
Based on the time-complexity analysis provided in Section IV-E, if the number of selected features is small, the computational requirements of our randomized sampling method will be comparable to greedy methods. However, in the worst case, the greedy-method scales quadratically with the number of candidate features (i.e., scaling with N 2 t = |Θ t | 2 ). This justifies the significant speed-up in the feature-selection that was observed in our numerical simulations (see Fig. 7). The low time-complexity of our method opens up new opportunities for real-time implementation of this algorithm and utilizing it for agile robot navigation.

APPENDIX
Appendix A: Proof of Lemma 1 After one episode of motion in the horizon, one can verify that the updates to covariance and the mean are given bȳ respectively. Because these update laws also work for any time instant in the horizon, we can use the similar updatē for every instant τ ∈ {t + 1, . . . , t + T }. To fully identify covariance matrixΣ t,T , we can show that for all τ 1 , τ 2 ∈ {t, t + 1, . . . , t + T }, with τ 1 < τ 2 , The proof of this relationship can be conducted by induction. Then, we observe that the covariance matrix has the structurē where the diagonal and off-diagonal matrix terms are supposed to be evaluated by iterative application of (44) and (46), respectively. For the mean, the update is simply achieved by stacking the updated mean variables in (43). This completes the proof.