Repositorio Institucional de Documentos

Repositorio de la Universidad de Zaragoza – Zaguan http://zaguan.unizar.es Trabajo Fin de Máster On the Comparison of Uncertainty Criteria for Active SLAM Autor Henry David Carrillo Lindado Director D. José Ángel Castellanos Gómez Escuela de Ingeniería y Arquitectura de Zaragoza (EINA) 2011 On the Comparison of Uncertainty Criteria for Active SLAM Henry David Carrillo Lindado Department of Computer Science and Systems Engineering University of Zaragoza Advisor: D. Jos´e ´ Angel Castellanos G´omez M´aster en Ingenier´ıa de Sistemas e Inform´atica 2010-2011 Programa Oficial de Posgrado en Ingenier´ıa Inform´atica November 2011 Resumen En este reporte se estudia el c´alculo del criterio de optimalidad D, para el caso en que es utilizado como una medida de la incertidumbre de un sistema SLAM. Propiedades del uso de este criterio de medida de la incertidumbre en el contexto de SLAM activo son presentadas, al igual que una comparaci´on contra otros criterios de medida de la incertidumbre tales como la entrop´ıa y el criterio de optimalidad A. En este reporte se muestra que contrario a lo divulgado previamente en la literatura cient´ıfica relacionada, el criterio de optimalidad D es capaz de proporcionar informaci´on ´util acerca de la incertidumbre que tiene un robot que ejecuta un algoritmo de SLAM. Finalmente, a trav´es de varios experimentos con robots reales y simulados, damos soporte a nuestras afirmaciones y mostramos que el uso del criterio de optimalidad D tiene efecto deseables en varias tareas que hacen uso de algoritmos de SLAM como mapeo y navegaci´on activa. Abstract In this report, we consider the computation of the D-optimality criterion as a metric for the uncertainty of a SLAM system. Properties regarding the use of this uncertainty criterion in the active SLAM context are highlighted, and comparisons against the A-optimality criterion and entropy are presented. This report shows that contrary to what has been previously reported in the literature, the D-optimality criterion is indeed capable of giving fruitful information as a metric for the uncertainty of a robot performing SLAM. Finally, through various experiments with simulated and real robots, we support our claims and show that the use of D-opt has desirable effects in various SLAM related tasks such as active mapping and exploration. Contents Contents iii List of Figures v Nomenclature vii 1 Introduction 1 2 Preliminaries 3 2.1 ActiveSLAM .................................. 3 2.1.1 The value function . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 2.2 Theory of Optimal Experiment Design and active SLAM . . . . . . . . . . 5 2.3 Uncertainty and information measures . . . . . . . . . . . . . . . . . . . . 5 2.3.1 Uncertainty measures . . . . . . . . . . . . . . . . . . . . . . . . . . 5 2.3.2 Information measures . . . . . . . . . . . . . . . . . . . . . . . . . . 7 2.3.2.1 Fisher based information measures . . . . . . . . . . . . . 7 2.3.2.2 Shannon based information measures . . . . . . . . . . . . 7 3 Uncertainty criteria for active SLAM 8 4 Experiments 10 4.1 First experiment: On the computation . . . . . . . . . . . . . . . . . . . . 10 4.1.1 Simulated robot in an indoor environment . . . . . . . . . . . . . . 10 4.1.2 Real robot in an indoor environment: DLR dataset . . . . . . . . . 12 4.1.3 Discussion................................ 13 4.2 Second experiment: Active approach . . . . . . . . . . . . . . . . . . . . . 14 4.2.1 One step look-ahead results . . . . . . . . . . . . . . . . . . . . . . 15 4.2.2 Discussion................................ 16 4.3 Thirdexperiment................................ 17 5 Conclusions 18 A19 A.1 Real robot in an ad-hoc indoor environment . . . . . . . . . . . . . . . . . 19 A.2 Real robot in an outdoor environment: Victoria Park dataset . . . . . . . . 20 iii CONTENTS CONTENTS B22 References 45 iv List of Figures 4.1 Resulting stochastic map for the experiment with a simulated robot in an indoor environment. In red is the estimated trajectory of the robot and in blue is the graphical representation of the covariance for each landmark. . . 11 4.2 (a)-(f) Evolution of the A-opt,E-opt,D-opt, determinant, entropy and MI for the experiment with a simulated robot in an indoor environment . . . . 11 4.3 (a) Resulting stochastic map with uncertainty regions for each landmark. (b) Blueprint of the environment with a superimposed sketch of the trajectory 12 4.4 (a)-(f) Evolution of the A-opt,E-opt,D-opt, determinant, entropy and MI for the experiment using the DLR dataset . . . . . . . . . . . . . . . . . . 13 4.5 (a) Ground truth of the landmarks and (b) initial stochastic map of the 30x30testenvironment............................. 15 4.6 Resulting paths from each uncertainty metric: (a) D-opt, (b) A-opt and (c) Entropy. Each colour represents an executed path. The planning area was20x20m. ................................. 15 4.7 Evolution of the MSE ((a)-(c)) and χ2((d)-(f)) ratios related to the map (30x30) after each active step. The ratios are computed for each possible uncertainty metric combination. The average of 10 Monte Carlo runs is depictedforeachratio.............................. 16 4.8 Evolution of the MSE ((a)-(c)) and χ2((d)-(f)) ratios related to the map (20x20) after each active step. The ratios are computed for each possible uncertainty metric combination. The average of 10 Monte Carlo runs is depictedforeachratio.............................. 17 4.9 Resulting trajectories for a 10000 steps active SLAM simulation. (a). Predefined trajectory and landmarks ground truth. (b). A-opt based active SLAM. (c). D-opt based active SLAM. This figure is best viewed in colour. 17 A.1 (a). Pioneer robot used in the experiment. (b) Metric map of the test environment with the position of the markers (Red) and the initial position oftherobot(Blue). .............................. 19 A.2 (a)-(f) Evolution of the A-opt,E-opt,D-opt, determinant, entropy and MI for the experiment with a real robot in an indoor environment. . . . . . . . 20 A.3 Resulting pose/feature graph. In dark blue and green are respectively, the estimated trajectory of the robot and landmarks. In yellow are shown the constraints between the nodes of the graph. . . . . . . . . . . . . . . . . . 21 v LIST OF FIGURES LIST OF FIGURES A.4 (a)-(f) Evolution of the A-opt,E-opt,D-opt, determinant, entropy and MI for the experiment using the Victoria Park dataset. . . . . . . . . . . . . . 21 vi Nomenclature Roman Symbols A-opt A-optimality criterion D-opt D-optimality criterion E-opt E-optimality criterion G-opt G-optimality criterion SLAM Simultaneous Localization and Mapping TOED Theory of Optimal Experiment Design vii 2. PRELIMINARIES 2.3.2 Information measures The uncertainty of an experiment and the information gain with it, share an inversely proportional relationship. The informativeness of an experiment can be measured mainly in two forms, and both of them inform about the compactness of a probability distribution and not about the data itself. 2.3.2.1 Fisher based information measures For a probability distribution P(x), the Fisher information is defined as [25]: J(x) = d2log P(x) dx2(2.10) For the particular case of a Gaussian distribution (i.e. N(µ,Σ)) the Fisher information measure is J(x) = Σ−1(2.11) Finally, it is worth to mention that the Fisher information measure is only defined for continuous distribution. 2.3.2.2 Shannon based information measures The Shannon information measures are based on the concept of entropy defined by Shannon [26]. In brief, the entropy of a random variable with an associated probability distribution P(x) is defined in the continuous case as: H(x) = − ∞ Z −∞ P(x) log P(x) dx(2.12) The entropy is an ever decreasing function; i.e. any new information increase the informativeness of the experiment. This property does not allow a direct comparison of the entropy between two instances of an experiment. To overcome this, another entropy based measure, the mutual information, is defined as: MI(x;y) = H(x)−H(x|y) (2.13) where xand yhave probability distributions P(x) and P(y), respectively. H(x|y) is known as the conditional entropy of ygiven x. Because of the properties of the entropy, the mutual information is bounded between zero and infinity. For the particular case of a multidimensional Gaussian distribution (i.e. Nn(µ,Σ)) the entropy is: H(x) = 1 2log(2πe)n|Σ|(2.14) 7 Chapter 3 Uncertainty criteria for active SLAM In the planning under uncertainty or active SLAM context [27], [10], [11] and [12], have done comparisons between uncertainty criteria, in order to determine if there is a criterion that for that specific task, converges faster to a desired solution. In all the aforementioned papers, the D-opt - defined by them as the determinant of the covariance matrix - has been disregarded as a fruitful metric for mainly two reasons: i) The D-optimality criterion does not allow the checking of task completion as the A-optimality criterion does. ii) The D-optimality criterion can be driven rapidly to zero, so no fruitful information is provided by this criterion. The authors believe that the above two reasons are misconceptions stemming from a misuse of the TOED. For (i), the misuse lies in that the determinant of a matrix l×lis homogeneous of degree l; hence the comparison of the determinant of a matrix l×land a matrix m×m is unfair. Specifically in the case of a SLAM system this is relevant, because the size of the covariance matrix varies over time, so the evolution of an uncertainty criterion based on determinants has to be normalized in order to be compared fairly [14]. Recently, Vidal-Calleja et al [28] intuited this, and proposed a solution that needs to suppose the maximum number of landmarks in the environment and initialize its covariance with a constant number. This solution is effective to fairly compare the determinant as the matrix size does not vary in time, but adds complexity to the computation of the metric and fails if the number of landmarks is greater than the initial assumption. A proper solution as pointed out by [14], is to take the lth root of the determinant of Σ(with size l×l) before making any comparison. This solution rises evidently if the D-opt is derived from the family of uncertainty criteria proposed by Kiefer in [15], φp(ξ) = [l−1trace(Σp(ξ))]1/p (3.1) This family of uncertainty criteria is valid in the range of 0 < p < ∞for a covariance matrix (Σ) of size l×lassociated to a design ξ(e.g. π). Moreover, the case φ1and the boundary cases φ0and φ∞are the already known A, D and E-optimality criteria. 8 3. UNCERTAINTY CRITERIA FOR ACTIVE SLAM Taking the above into account, the normalized D-optimality criterion proposed by Kiefer is, φ0(π) = lim p→0+ φp(π) = [det(Σ(π))]1/l = ( Y k=1,...,l λk)1/l (3.2) The misuse of TOED for the second reason, usually used to disregard the D-opt, lies in the fact that this criterion considers the global variance. Geometrically, this means the volume of a n-dimensional ellipsoid [13]. The latter implies that estimated parameters with low uncertainty will produce very low value of D-opt, hence making its computation prone to round-off errors. Specifically in the SLAM case, as the landmarks get correlated the eigenvalues of Σ become quite small values near to zero. A zero eigenvalue would mean that without doubt the position of a landmark is known, but this does not happen in practice. Examples of the above are presented in chapter 4, where we reported several experiments with simulated and real robots. Regarding the computation of the determinant, it is possible that a small value of an eigenvalue can cause a round-off error in the computation, so the D-opt gets stuck at zero. One way to overcome this issue is to use the logarithmic space to compute the determinant as proposed by Pazman [13]. Thus, the resulting equation to compute the criterion would be, exp(log([det(Σ(π))]1/l)) (3.3) Summarizing, for the particular case of measuring the uncertainty of a SLAM system, the D-opt should be computed using the definition of Kiefer [15] and as presented in (3.3). 9 Chapter 4 Experiments In this chapter, two experiments are presented in order to (i) support the claims about the computation of the D-optimality criterion of a SLAM system (ii) point out some properties of the D-optimality criterion. The first experiment investigates the evolution of different uncertainty metrics in simulated and real robots performing SLAM. The second experiment is related to performing active SLAM using solely the uncertainty as a guiding factor. A third experiment is reported in the appendix B.3 and deals with obtaining the minimum uncertainty path for autonomous navigation. 4.1 First experiment: On the computation Aiming at showing that is feasible to compute the D-opt in a robot performing SLAM, in the following the evolution of the aforementioned uncertainty criterion is computed for simulated and real robots performing SLAM. Due to space limitations only two test scenarios are shown, but other results on the Victoria Park dataset and in an ad-hoc indoor environment using a Pioneer DX-3 robot are presented in the appendix A. For completeness, the A-opt,E-opt, the determinant of the covariance matrix, entropy and mutual information are also computed. In each of the following experiments the aforementioned uncertainty criteria are computed at each step update of the covariance matrix Σassociated to ˆ xRand ˆ xF. 4.1.1 Simulated robot in an indoor environment The simulation environment was created using C++ and the Mobile Robot Programming Toolkit (MRPT) v0.9.4. The data of the covariance matrix were gathered while the robot was performing EKF-SLAM with a predefined trajectory, within a map with static landmarks and using a limited range sensor. Specifically, the robot was moving at 0.3 m per step and travelled along a squareshaped trajectory of 25x25 m. The navigation environment was composed of 2-D point features, located in both sides of the trajectory with a distribution of 1.8 feature/m. The robot was equipped with a range-bearing sensor with a frontal field of view of 360oand a maximum range of 3 m. Synthetic errors, with a Gaussian distribution, were generated for the odometry model of the robot (standard deviations of 0.1oin orientation and 0.2 10 4. EXPERIMENTS 0 5 10 15 20 25 0 5 10 15 20 25 Figure 4.1: Resulting stochastic map for the experiment with a simulated robot in an indoor environment. In red is the estimated trajectory of the robot and in blue is the graphical representation of the covariance for each landmark. 0 50 100 150 200 250 300 350 0 5 10 15 Steps A−criterion @Robot−Landmark (a) 0 50 100 150 200 250 300 350 0 2 4 6 8 10 Steps E−Criterion @Robot+Landmarks (b) 0 50 100 150 200 250 300 350 0 0.2 0.4 0.6 0.8 1x 10−4 Steps D−Criterion by Kiefer @Robot+Landmarks (c) 0 50 100 150 200 250 300 350 0 1 2 3 4x 10−67 Steps Determinant @Robot+Landmarks (d) 0 50 100 150 200 250 300 350 0 50 100 150 200 Steps Entropy @Robot+Landmark (e) 0 50 100 150 200 250 300 350 0 2 4 6 8 10 12 Steps MI @Robot+Landmarks (f) Figure 4.2: (a)-(f) Evolution of the A-opt,E-opt,D-opt, determinant, entropy and MI for the experiment with a simulated robot in an indoor environment m per m in displacement) and the sensor measurements (standard deviations of 0.125o in orientation and 1 cm per m in range), but known data association is assumed. The resulting stochastic map after one loop is shown in Fig. 4.1 Fig. 4.2 shows the evolution of the different criteria as stated above. Each point of the evolution gives an indication of the amount of uncertainty the SLAM system has at that step. As expected, once the robot starts navigating, the uncertainty related to the landmarks and robot’s localization starts increasing. The evolution of the tested criteria behaves similarly at this stage. Around the step 350 a loop closing event occurred, and therefore a decrease in the uncertainty of the system is produced as expected. This drop is sensed by all the metrics 11 4. EXPERIMENTS (a) (b) Figure 4.3: (a) Resulting stochastic map with uncertainty regions for each landmark. (b) Blueprint of the environment with a superimposed sketch of the trajectory but at different magnitudes, A-opt and E-opt had a major reduction, but D-opt had a minor one. The difference in magnitude is due to the opposite definition of the metrics. D-opt in general, takes into account the uncertainty of each element of the system multiplicatively, i.e. every element has an equal chance to contribute to the uncertainty. This definition allows encompassing the global uncertainty in the D-optimality criterion. On the other hand, A-opt gives independent and additive contribution to each element of uncertainty. Giving the possibility of a single component of the system to drive the whole uncertainty. In fact, as can be seen in Fig. 4.2a and Fig. 4.2b,A-opt and E-opt resemble in shape and scale, thus giving a numerical example, although qualitative, of the above, as E-opt represents the value of the single maximum eigenvalue. Moreover, the correlation between A-opt and E-opt is 0.9655, giving a quantity value of its resemblance. Fig. 4.2d shows an example of computing the determinant of the covariance matrix as reported in [27], [10], [11] or [12], as can be seen after few steps -in this case 8- the value of the criterion goes to zero. In contrast, Fig 4.2c shows an example of meaningful values of uncertainty using the logarithmic based computation method presented in (3.3). 4.1.2 Real robot in an indoor environment: DLR dataset In this experiment the DLR dataset [29] is used. This dataset was recorded at the Deutsches Zentrum fur Luft und Raumfahrt (DLR) with a mobile platform. The environment is a typical office indoor environment and covers a region of 60m x 45m. To estimate the trajectory and the map of the environment an EKF based SLAM algorithm coded in python is used. Fig. 4.4 shows the evolution of the different uncertainty criteria associated to the uncertainty of the robot and landmarks for the DLR dataset that has a path length of approximately 505 meters. 12 4. EXPERIMENTS 0 500 1000 1500 2000 2500 3000 0 2 4 6 8 Steps A−Criterion @Robot+Landmarks (a) 0 500 1000 1500 2000 2500 3000 0 0.5 1 1.5 2 2.5 3 E−Criterion @Robot+Landmarks Steps (b) 0 500 1000 1500 2000 2500 3000 0 0.2 0.4 0.6 0.8 1x 10−3 Steps D−Criterion @Robot+Landmarks (c) 0 500 1000 1500 2000 2500 3000 2 4 6 8 10 12x 10−53 Steps Determinant @Robot+Landmarks (d) 0 500 1000 1500 2000 2500 3000 0 200 400 600 800 1000 Entropy @Robot+Landmarks Steps (e) 0 500 1000 1500 2000 2500 3000 0 1 2 3 4 5 6 MI @Robot+Landmarks Steps (f) Figure 4.4: (a)-(f) Evolution of the A-opt,E-opt,D-opt, determinant, entropy and MI for the experiment using the DLR dataset 4.1.3 Discussion The above results give numerical examples about the feasibility of computing the D-opt in the SLAM context in simulated and real data. Also, the results give some insights between the relation among the A-opt and E-opt. Although this relation (shape and magnitude of the plots) is qualitative, a quantitative relation via the correlation of the data can be obtained. The correlation between the A-opt and the E-opt for all the experiments has a mean of 0.9872 ±2.1155 ×10−4. The latter means that exist a strong relation between these two criteria in the SLAM context. Moreover, based on the definition of the E-opt, the uncertainty measured by the A-opt is dominated by a single eigenvalue. In our context, the above implies that a single feature - in the case of a probabilistic feature based SLAM - can drive the complete SLAM uncertainty. The effect of the above property could lead an active SLAM algorithm using an A-opt based metric to get stuck in a local minima. An example of this is shown in the next experiment. For the above experiments, the A-opt and D-opt correlation has a mean of 0.6003 ± 0.0540, which means that exist a correlation but neither is weak or strong. Moreover, it gives an example of the main characteristic of the criteria according to the TOED: The A-opt measures the mean of the uncertainty and the D-opt measures the complete dimension of the uncertainty (e.g. Area in a 2D case). 13 4. EXPERIMENTS 4.2 Second experiment: Active approach In this experiment we perform a comparison between an active SLAM approach driven by the A-opt,D-opt and entropy. The active SLAM approach used follows the algorithm outlined in chapter 2, therefore assumes a priori and probably incomplete, stochastic map of the environment. This map is generated by commanding the robot to follow a predefined trajectory in the environment, while performing EKF-SLAM. Once the predefined trajectory is completed, the robot begins the performance of active SLAM and therefore starts planning autonomously trajectories that achieve an accurate map. Each time the robot is planning which trajectory it has to follow, in order to fulfil the active SLAM objectives, it has to consider every possible path in the navigation environment. In order to make the problem computationally tractable, the possible destinations are constrained to positions near the landmarks already discovered. Planning each time only the next movement is known as greedy approach or one step look-ahead [4]. It is possible to plan several steps ahead that yields, as has been pointed out by [4], in a faster convergence of the active SLAM goals but with an increase in the complexity of the computation. Independent of the one step look-ahead or multi-step look-ahead planning, each time the next movement is chosen as the one that minimizes an uncertainty metric, in this case the value of A-opt or D-opt or entropy related to the SLAM. In this experiment, the paths follow autonomously for the robot are generated via an A* based path planner. Specifically the environment is discretized and the only forbidden areas are the positions of the landmarks. Two test environments were used for this experiment: the first test environment consists of a 30x30 meter obstacle free square area with 104 landmarks distributed around the perimeter of a 25 meter square. The second test environment consists of a 20x20 meter obstacle free square area with 72 landmarks distributed on the perimeter of a 15 meter square. The Mean Squared Error (MSE) between the two initial stochastic maps has a ratio of 9.65, with the first environment having a bigger MSE. The initial position of the robot is (X=1,Y=0) in both environments. The ground truth position of the landmarks and their estimated positions from the EKFSLAM are depicted in Fig. 4.5. The strategy for active SLAM described above can be summarized in the following steps: •Hallucinate paths from the current estimated position of the robot to all the landmarks, except those which are below a radius of X(i.e. 1) meters from the current estimated position. •Measure the uncertainty at the end of each hallucinated path. •Select the path that produced the lowest uncertainty according to the chosen metric. •If the number of path planned is greater than i(i.e. 100), exit. In any other case, execute again. 14 4. EXPERIMENTS 0 5 10 15 20 25 0 5 10 15 20 25 (a) 0 5 10 15 20 25 0 5 10 15 20 25 (b) Figure 4.5: (a) Ground truth of the landmarks and (b) initial stochastic map of the 30x30 test environment 0 5 10 15 −2 0 2 4 6 8 10 12 14 16 18 (a) 0 5 10 15 −2 0 2 4 6 8 10 12 14 16 18 (b) 0 5 10 15 −2 0 2 4 6 8 10 12 14 16 18 (c) Figure 4.6: Resulting paths from each uncertainty metric: (a) D-opt, (b) A-opt and (c) Entropy. Each colour represents an executed path. The planning area was 20 x 20 m. 4.2.1 One step look-ahead results Performing active SLAM with a one-step look-ahead approach leads to completely different trajectories using the A-opt and D-opt. The A-opt plans trajectories with a distinctive local behaviour, while the D-opt plans trajectories more globally, often revisiting previous landmarks. Regarding the entropy, this generates paths similar to the D-opt. An example of the above behaviour is illustrated in Fig. 4.6. There, the active SLAM starts after the robot has executed one loop (i.e. X=1,Y=0) and has an estimation of all the landmarks in the environment. The resulting trajectories for the A-opt,D-opt and the entropy are shown in Fig. 4.6a, Fig. 4.6b and Fig. 4.6c, respectively. Each generated trajectory is identified by a different colour. A video of the incremental construction of each trajectory can be seen in http://webdiis.unizar.es/~hcarri/1.avi. In addition to the above qualitative assessment of the effect derived by using each criterion, we can quantify the effect of using each criterion by measuring the quality of its resulting maps. To measure the quality of the map we use the guidelines proposed in [30] that urge for the use of the MSE an χ2together in the assessment of the maps quality generated by a SLAM algorithm. In order to compare the three criteria, we compute the ratio between them for each quality metric at each update step of the active algorithm. Therefore we have the A- opt/D-opt ratio, the A-opt/entropy ratio and the entropy/D-opt for the MSE and χ2 15 4. EXPERIMENTS 0 20 40 60 80 100 0 1 2 3 4 5 Iteration MSE ratio (a) A-opt/D-opt 0 20 40 60 80 100 0 1 2 3 4 5 Iteration MSE ratio (b) A-opt/Entropy 0 20 40 60 80 100 0 0.5 1 1.5 2 Iteration MSE ratio (c) Entropy/D-opt 0 20 40 60 80 100 0 0.5 1 1.5 2 Iteration χ2 ratio (d) A-opt/D-opt 0 20 40 60 80 100 0 0.5 1 1.5 2 Iteration χ2 ratio (e) A-opt/Entropy 0 20 40 60 80 100 0 0.5 1 1.5 2 Iteration χ2 ratio (f) Entropy/D-opt Figure 4.7: Evolution of the MSE ((a)-(c)) and χ2((d)-(f)) ratios related to the map (30x30) after each active step. The ratios are computed for each possible uncertainty metric combination. The average of 10 Monte Carlo runs is depicted for each ratio. metric. Fig. 4.7 presents the result of 10 Monte Carlo Runs for each ratio related to the MSE and χ2metric of the 20x20 test environment. Respectively, Fig. 4.8 presents the same information for the 30x30 environment. Finally, Fig. 4.9 shows the resulting path for the active SLAM strategy presented in this section using a limit of 10000 steps and a continuous path planner based on an attractor/repulsion technique. This last experiment illustrates another example of the quasi-opposite behaviour of an active SLAM strategy using the A-opt and D-opt. 4.2.2 Discussion An explanation of the difference in the path planning behaviours due to the A-opt or D- opt used relies on the definition of the metric itself. As pointed out in the previous section, D-opt encompasses the global uncertainty therefore revisiting previous landmarks (closing the loop) helps in decreasing the value of the metric. On the other hand, A-opt criterion can be driven by a single eigenvalue, and therefore the uncertainty of the covariance matrix can get stuck in a local minimum. Regarding the quality of the maps, the results show an advantage in the use of D-opt and entropy over the A-opt. Also in this specific experiment the D-opt and entropy share similar results. This similarity does not come as a surprise, because the EKF-SLAM assumed gaussianity as well the noise used in the experiment, therefore the D-opt and the entropy have an explicit relationship through the determinant as can be seen comparing 16 Experimental Comparison of Optimum Criteria for Active SLAM Henry Carrillo and Jos´e A. Castellanos Abstract—In this paper, we consider the computation of the D-optimality criterion as a metric for the uncertainty of a SLAM system. Properties regarding the use of this uncertainty criterion in the active SLAM context are highlighted, and comparisons against the A-optimality criterion are presented. This paper shows that contrary to what have been previously reported, the D-optimality criterion is indeed capable of giving fruitful information as a metric for the active SLAM problem. Moreover, its performance is comparable to A-optimality, but with extra appealing characteristics such as the invariance to change in scale and an intrinsic global trajectory planning. I. INTRODUCTION A model of the operative environment is an essential requirement for an autonomous mobile robot. The construction of this model requires the solution of at least three basic tasks for a mobile robot, namely localization, mapping and trajectory planning. The intersection of the first two tasks, defines a key problem in modern robotics: Simultaneous Localization and Mapping (SLAM). SLAM is the problem of acquiring on-line and sequentially spatial data of an unknown environment in order to construct a map of it, and at the same time, allows the robot to localize itself in this map. SLAM is still a key-open problem regarding mobile robotics and its solution in all senses (practical and theoretical) will be a breakthrough achievement towards autonomous robots [1]. To integrate the trajectory planning into SLAM allows a mobile robot to perform common tasks such as autonomous environment exploration. This approach is known as active SLAM and specifically refers to the problem of how to give a mobile robot the capability of generating on-line trajectories that simultaneously maximize the accuracy of the map and robot’s localization, regarding a SLAM task. The active SLAM paradigm was first proposed and tested in [2]. Since then, different approaches have been done. e.g. [3] and [4] proposed a discrete and greedy planning methodology. Huang et al. in [5] studied and tested the feasibility of multi-step planning. Continuous states planning but with a discretization in actions space is explored in [6]. Recently, continuous planning approach in states and actions has been proposed by [7]. This work has been supported by the MICINN-FEDER project DPI2009- 13710, and research grants BES-2010-033116 and EEBB-2011-44287. H. Carrillo and J. A. Castellanos are with the Departamento de Inform´atica e Ingenier´ıa de Sistemas, Instituto de Investigaci´on en Ingenier´ıa de Arag´on, Universidad de Zaragoza, C/ Mar´ıa de Luna 1, 50018, Zaragoza, Spain. {henry.carrillo, jacaste}@unizar.es To the best of the authors’ knowledge, the different approaches that attempt to produce an active SLAM algorithm, rely on metrics that quantify the improvement of the actions taken by the robot (e.g. movements). This improvement is measured relative to the robot and the map localization accuracy, the area of the map explored or the time that the robot has been navigating. Specifically, the metrics that relate the improvement of the localization accuracy or uncertainty to the movements the robot makes are of high value, because their use allow the reduction of the map’s error, and therefore the probability to accomplish a given task is improved. Until now the preferred criterion to quantify the localization uncertainty has been A-optimality. This criterion captures the mean uncertainty of the covariance matrix of a SLAM system. The choice of this criterion in many active SLAM related works such as [8], [9], [10], [11], [6], [12], and [7], among others, found its foundation in the fact that papers such as [13], [14], and [15] reported that A-optimality criterion applied to the problems of planning under uncertainty outperforms others well known criteria such as D-optimality. In the Theory of Optimal Experiment Design (TOED) [16] [17] [18] [19], it is well known that the use of the D- optimality criterion has more appealing characteristics than the A-optimality or E-optimality criterion. Moreover, J. Kiefer in [20] demonstrated that the A, D and E-optimality criteria are special cases of a general family of optimality criteria and therefore they share some properties, but D-optimality is the only one proportional to the uncertainty ellipse of the estimated parameters, and it is also invariant to reparametrizations and linear transformations [19]. In this paper, it is shown that is indeed possible to obtain a fruitful metric from the D-optimality criterion for the specifically case of the active SLAM problem, and its use as a metric perform comparable to the A-optimality metric popularized by [13], [14] and [15], but with extra appealing characteristics such as the invariance to change in scale and an intrinsic global trajectory planning. The reminder of the paper is structured as follows: section II gives an overview of the active SLAM problem. Section III reviews several optimality criteria and its application to the active SLAM problem. Moreover, this section also shows how to compute correctly those criteria in order to be compared correctly. Section IV reports a comparison of the evolution of different optimality criteria on a simulated and real SLAM scenarios. Section Vshows a comparison between the performances of an active SLAM approach using different optimality criteria. Finally, section VI presents conclusions. II. ACTIVE SLAM The SLAM problem does not establish which trajectories a robot has to follow. Usually, they are chosen randomly or beforehand. It is well known that the trajectories selected and the order they are executed by a robot, are critical, among other things, firstly for a rapidly convergence of the uncertainty of a SLAM algorithm, secondly for increasing the area of the environment explored by the robot, and thirdly to improve the possibility of fulfilling possible tasks. The integration of the trajectory planning task into SLAM has been first proposed in [2] and the term active SLAM referring to the aforementioned integration was coined by [10]. The general idea of active SLAM is summarized in Alg. 1. There, the SLAM approach taken is based on a probabilistic state-space model, where the robot Rand a set of features or landmarks in the environment F={F1,...,Fn}are represented by a stochastic state vector xwith an estimated mean ˆ xand associated covariance matrix Σ. Furthermore, ˆ x=ˆ xR ˆ xF;Σ=ΣRR ΣRF ΣFRΣF F (1) where ˆ xRand ˆ xFare the estimated locations of the robot and the landmarks respectively, ΣRR is the covariance matrix of the estimated robot pose (e.g. x, y, θ) and it has a size of p×pthat is invariant with respect to (wrt) the time, ΣF F represents the covariance matrix of the estimated locations of the discovered landmarks and it has a size of n×nthat varies wrt the time. Finally, ΣRFand ΣFRare matrices that encode the cross-covariance of the robot pose and the landmarks estimations. The covariance matrix Σhas size l×l, where l=p+n, and its value is variable wrt the time. Moreover, it is a positive semi-definitive matrix with eigenvalues {λ1,...,λl}. Algorithm 1 The active SLAM algorithm Require: •A complete or incomplete stochastic map of the environment Mk={ˆ xk,Σk}. •The length iof the horizon of planning. Ensure: •A policy class of trajectories π. 1: Create a set πsof sdifferent policy class with itrajectories each one. The initial trajectory of each policy starts at ˆ xRk. 2: Perform a SLAM algorithm using each policy class and the given map Mk. 3: Compute a value function Jfor each policy class of πs, using the information of each trajectory followed and the final covariance matrix associated to the SLAM algorithm. 4: Select the policy class πopt that optimize J. A. The value function As mentioned above, the integration of trajectory planning or, what is equivalent, applying the active sensing paradigm [21] [13] to the SLAM problem, involves the optimization of a multi-objective performance criterion or value function J. This value function is used to decide which trajectories have to be followed by the robot. A definition of this value function can be as follows: J=X i αiUi+X i βiTi(2) Where the index idefines the length of the planning horizon (i.e. the numbers of consecutive trajectories planned ahead). The first term, Uicharacterizes the expected cost of the uncertainty in the parameters of the system. The second term, Tiincludes other expected costs such as trajectory length, navigation time, and energy consumption, among others. Finally, αand βare weight coefficients for tuning the parameters and are task dependant. The Uiterm can be further specified as a metric of the associated covariance matrix Σ(e.g. the trace). This metric needs to encode the robot and the landmarks estimated locations uncertainty and can be defined as follows: Ui:Σ→R(3) The different ways to compute the above metric and theirs properties in relation to the goals of the active SLAM approach is the target of the following sections of this paper. Moreover, a clarification in the computation of one of them is pointed out in section III. The second term Ti, as done previously, can be further specified and constrained as a metric that represents the cost of performing a free collision trajectory Γby the robot, Ti: Γ →R(4) This metric can be constrained to be a function only of the distance travelled, since its cost is directly related to the power and navigation time of the robot while it performs a task. Finally, summarizing all the above definitions at this point, a statement of the active SLAM problem can be formulated as: the task of choosing a single or multiple step policy class πof robot’s trajectories that optimize a value function J. III. OPTIMUM EXPERIMENTAL DESIGN AND OPTIMUM CRITERIA A. Background In the Theory of Optimal Experiment Design (TOED) [16] [18] [19], a single trial of an experiment is the process of changing the input parameters of a system perturbed with unknown noise, with the purpose of observing the variation in the output parameters. In this context, the particular values of the input parameters are known as a particular design ξ. In the active SLAM context, the aforementioned design is a particular policy class πcommanded to the robot, the unknown noise is the commonly assumed zero mean Gaussian noise, and the variation of the parameters is encoded in the covariance matrix Σ. Based on the TOED, it is possible to know if a design ξ1 is better than a design ξ2[16] [19]. Applying this concept in the active SLAM context, a policy class π1is better in terms of uncertainty than a policy class π2if : Cov(π1)−Cov(π2)∈NND(l)(5) Where Cov(πi)is the covariance matrix of size l×lafter the robot has followed πiand NND(l)stands for the group of non-negative definite matrices of size l×l.NND matrices are also known as positive semi-definite matrices [19]. As this criterion only tells if a policy class is better than other but does not quantify how much, it is advantageous to define a function φthat map a NND covariance matrix of size l×lto a scalar, φ:NND(l)→R(6) This function has to capture the idea of whether or not the uncertainty of a covariance matrix is large or small. Moreover, this function has to be positive homogeneous, isotonic (i.e. order preserving) and concave [19]. In the TOED context, a first criterion fulfilling the above requirements was proposed by K. Smith back in 1918 [22], and it aims at minimizing the maximum variance of any predicted value over the experimental space. This criterion, later named globally optimum or G- optimality by J. Kiefer [23], suffers from a high complexity in its computation because the variance of each parameter of the system has to be tested individually, thus making it impractical to be used in a many-parameters systems [19]. A second criterion named D-optimality (D-opt) was proposed by A. Wald in 1943 [24]. This criterion aims at monitoring directly the quality of the parameter estimation, to do so it is defined as the determinant of the covariance matrix Σ: det(Σ) = Y j=1,...,l λj(7) Where, λjrepresents the eigenvalues of Σand the equality holds because the covariance matrix is symmetric [16]. This criterion is the preferred in the TOED because: 1) It captures well the information in the confidence ellipsoid of the parameters, furthermore exists an inverse proportionality [16] [17], and, 2) This criterion is the only one that is invariant to reparametrization (i.e. change in scale) and linear transformation on the covariance matrix [18]. This last property is appealing because a SLAM algorithm using this criterion does not need to take into account if the parameters of Σare in millimetres, meters, kilometres or inches. A common variation of the D-optimality criterion [17] is to apply the logarithm, in order to exploit the addition and multiplication relationship in that domain, ln(det(Σ)) = ln( Y j=1,...,l λj)(8) Additionally, working in the logarithmic space allows a correction of the round-offerror due to small values multiplication up to certain scale. A third criterion named A-optimality (A-opt) was introduced by H. Chernoff in 1953 [25]. This criterion targets the minimization of the average variance and it is defined as follows, trace(Σ) = X j=1,...,l λj(9) Although this criterion does not have the advantages of the D-optimality, its information is related with the major axis of the confidence ellipsoid of the parameters [16]. Another optimality criterion named E-optimality (E-opt), was introduced by E. Ehrenfeld in 1955 [26] and intends to minimize the maximum eigenvalue of Σ. The main advantage of this criterion is the simplicity of its computation, but it is a rough approximation of the error ellipsoid. The above optimality criteria are compiled and discuss in further detail in [16], [17] or [18]. B. Optimum criteria for active SLAM In the planning under uncertainty or active SLAM context the following articles [27], [13], [14] and [15], have done comparisons between optimum criteria, in order to determine if there is a criterion that for that specific task, converges faster to a desire solution. In all the aforementioned papers, the D- optimality has been disregarded as a fruitful metric for mainly two reasons: 1) The D-optimality criterion does not allow the checking of task completion as the A-optimality criterion does. 2) The D-optimality criterion can be driven rapidly to zero, so no fruitful information is provided by this criterion. The authors believe that the above two reasons are misconceptions lead by a misuse of the TOED. For the first reason, the misuse lies in that the determinant of a matrix l×lis positively homogeneous of degree l, hence the comparison of the determinant of a matrix l×land a matrix m×mis unfair. Specifically in the case of a SLAM system this is relevant, because the size of the covariance matrix varies with time, so the evolution of optimality criteria based in determinants has to be normalized in order to be compared fairly [19]. Recently, Vidal-Calleja et al [28] intuited this, and proposed a solution that needs to suppose a maximum number of landmarks in the environment and also initialize its covariance with a constant number. This solution is effective to compare fairly the determinant as the matrix size does not vary in time, but adds complexity to the computation of the metric and fails if the number of landmarks are superior to the initial assumption. A proper solution as pointed out by [19], is to take the lroot of the determinant of Σ(with size l×l) before making any comparison. This solution rises evidently if the optimality criteria are derived from the family of optimal criteria proposed by J. Kiefer in 1974 [20], φp(ξ) = [l−1trace(Σp(ξ))]1/p (10) This family of optimal criteria is valid in the range of 0< p < ∞for a covariance matrix (Σ) of size l×lassociated to a design ξ(e.g. π). Moreover, the case φ1and the boundary cases φ0and φ∞are the already known A, D and E-optimality criteria. Taking the above into account, the normalized D-optimality criterion proposed by J. Kiefer (DN-opt) is, φ0(π) = lim p→0+ φp(π) = [det(Σ(π))]1/l = ( Y j=1,...,l λj)1/l (11) Furthermore, J. Kiefer in 1959 [23] demonstrated that the G- optimality is equivalent to a D-optimality. Therefore, the later adds another appealing characteristic for the use of the D- optimality criterion, because G-optimality aims at minimizing the maximum variance of all the estimated values. The misuse of TOED for the second reason usually used to disregard the D-optimality, lies in the fact that this criterion considers the global variance. This geometrically means the volume of a n-dimensional ellipsoid [17]. The later implies that estimated parameters with low uncertainty will produce very low value of D-optimality, hence making its computation prone to round-off errors. Specifically in the SLAM case, as the landmarks get correlated the eigenvalues of Σbecome quite small values near to zero. A zero eigenvalue would mean that without doubt the position of a landmark is known, but this practically just not happened. Moreover, it is possible that a small value of an eigenvalue can cause a round-off error in the computation, so the D-optimally criterion gets stuck at zero. One way to overcome this issue is to use the logarithmic space to compute the determinant as proposed by A. Pazman [17]. Thus, the resulting equation to compute the criterion (DN−logopt) would be, exp(ln([det(Σ(π))]1/l)) (12) A common transformation to prevent a matrix to have ill condition eigenvalues, is to shift the eigenvalues in order to have a smaller relative span between them. This practice is very common in numerical linear algebra (e.g. QR algorithm [29]) and it is done by adding algebraically a scaled version of the identity matrix from the ill conditioned matrix. A problem with this approach is that it transforms the D-optimality in a scale version of the A-optimality criterion (see proof in the appendix). For the above reason, working the D-optimality criterion in the logarithmic space seems as the most reasonable solution to avoid round-off errors in computation. 0 5 10 15 20 0 5 10 15 20 GROUND TRUTH, features: 97 Fig. 1. Ground truth of the predefined trajectory and landmarks positions. IV. EVOLUTION OF OPTIMUM CRITERIA IN A SLAM SYSTEM In this section, the evolution of the values of A-opt,E- opt and DN−log-opt optimality criteria in a SLAM system are shown via simulated experiments. The experiments are done firstly to support the claims previously made about the computation of D-optimality and secondly to pointed out some properties of the criteria. A. The simulation setup The simulation environment used for the experiments was MATLAB and the simulations itself consisted in the computation of the A, E and D-optimality criteria at each step update of the covariance matrix Σassociated to ˆ xRand ˆ xF. The data of the covariance matrix were gathered while the robot was performing EKF-SLAM with a predefined trajectory, within a map with static landmarks and using a limited range sensor. Four different predefined trajectories were tested: loop, lawn, snail and random, and all of them share the same results, therefore only the results of the first one (i.e. loop) are shown. Specifically in the reported simulated experiment, the robot travelled along a square-shaped trajectory of 18x18 m, moving 0.325 mper step. The navigation environment was composed of 2-D point features, located in the outer sides of the trajectory with a distribution of 1.8 feature/m and in the inner sides of the trajectory with a random distribution with a density of 6 feature/m2. The robot was equipped with a range-bearing sensor with a maximum range of 2.5 mand a 180ofrontal field of view. Gaussian distributed synthetic errors were generated for both the odometry model of the robot (standard deviations of 0.2 mper min displacement and 0.1oin orientation) and the sensor measurements (standard deviations of 1 cm per min range and 0.125oin orientation), but known data association is assumed. B. The simulation results The square-shaped (i.e. loop) trajectory followed by the robot is shown (in black triangles) along with the 2-D point features (blue circles) in Fig. 1. 0 20 40 60 80 100 120 140 160 180 200 0 2 4 6 8 10 12 14 A−Criterion @Robot+Landmarks Steps (a) 0 20 40 60 80 100 120 140 160 180 200 0 1 2 3 4 5 6 7 8 9E−Criterion @Robot+Landmarks Steps (b) 0 20 40 60 80 100 120 140 160 180 200 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 x 10−4 D−Criterion by Kiefer Log @Robot+Landmarks Steps (c) Fig. 2. Results of the evolution of the different optimum criteria while performing the square-shaped trajectory. (a). A-opt. (b). E-opt. (c). DN−log-opt. 0 50 100 150 200 0 0.5 1 1.5 x 10−40 D−Criterion @Robot+Landmarks Steps (a) 0 20 40 60 80 100 120 140 160 180 200 0 0.5 1 1.5 x 10−4 D−Criterion by Kiefer @Robot+Landmarks Steps (b) Fig. 3. Results of the evolution of different computation forms of the D- optimality criterion while performing the square-shaped trajectory. (a). D-opt. (b). DN-opt. Fig. 2shows the evolution of the A-opt,E-opt and DN−logopt criteria as stated above. Each point of the evolution gives an indication of the amount of uncertainty the SLAM system has at that step. As expected, once the robot starts navigating, the uncertainty regarding the parameters the SLAM algorithm is estimating start increasing. The evolution of the three tested criteria behave similarly at this stage. At step 165 a loop closing event occurred, and therefore a decrease in the uncertainty of the system is produced as expected. This drop is sensed by all the metrics but at different magnitudes, A-opt and E-opt had a major reduction, but DN−log-opt had a minor one. The difference in magnitude is due to the opposite definition of the metrics. D-optimality in general, takes into account the uncertainty of each element of the system multiplicatively, that is every element has equal chance to contribute to the uncertainty. This definition allows encompassing the global uncertainty in the D-optimality criterion. On the other hand, A-optimality criterion gives independent and additive contribution to each element of uncertainty. Giving the possibility of a single component of the system to drive the whole uncertainty. In fact, as can be seen in Fig. 2(a) and Fig. 2(b), A-opt and E-opt resemble in shape and scale, thus giving a numerical example of the above, as E-opt represents the value of the single maximum eigenvalue. Fig. 3shows the evolution of two different ways of computing the D-optimality criteria for the above case. The first, (a) (b) Fig. 4. (a). Pioneer robot used in the test. (b) Metric map of the test environment. is the traditional form proposed by A. Wald in [24] as in (7), and the second is the form proposed by J. Kiefer [20] as in (10). As it is shown, D-opt does not give fruitful information and mainly, as it can be seen in the magnitude, because of round-off errors. DN-opt shows good results in the first steps but gets saturated at step 160. This happens again, due to the round-off error problems in the computation. This problem is overcome by computing the multiplication in the logarithmic space (i.e. DN−log-opt), as it is shown previously in Fig 2(c). C. Experiments with real robots In order to validate completely the simulated results, an further evaluate the proposed computation method. A real test is perform on a Pioneer P3-DX robot. The robot is equipped, besides its standards accessories, with a LMS 200 SICK laser, a Microsoft Kinect camera and a laptop with a Intel Core i7 @ 2.7 GHz and 6 GB of memory. The robot is programmed using ROS in Ubuntu 10.10. A photo of the robot is shown in Fig.4a. In order to isolate the effect of data association, markers of the ARToolkit are used as a distinguishable isolated features. Also to guarantee a correct navigation, a localization module based on AMCL is used when the robot is following a commanded path. The environment is a room of 6 x 4 meters. A metric map of the room is shown in Fig.4b. Figure 5shows the evolution of the different optimum criteria associated to the uncertainty of the robot and five Fig. 5. Results of the evolution of the different optimum criteria while performing a real trajectory. (Up). D-opt. (Down). A-opt. landmarks. The landmarks are distribute over the walls. V. COMPARISON WITHIN AN ACTIVE SLAM APPROACH In this section, the results of a comparison between an active SLAM approach driven by the A-optimality criterion and by the D-optimality (i.e. DN−log-opt) criterion, is presented. The active SLAM approach used, assumes a priori and probably incomplete, stochastic map of the environment. This map is generated by commanding the robot to follow a predefined trajectory in the environment, while performing SLAM. Once the predefined trajectory is completed, the robot begins the performance of active SLAM and therefore starts planning autonomously trajectories that achieve an accurate map. Each time the robot is planning which trajectory it has to follow, in order to fulfil the active SLAM objectives, it has to consider every possible path in the navigation environment. In order to make the problem computationally tractable, the possible destinations are constrained to positions near the landmarks already discovered. Planning each time only the next movement is known as greedy approach or one step look-ahead [5]. It is possible to plan several steps ahead that yields, as has been pointed out by [5], in a faster convergence of the active SLAM goals but with an increase in the complexity of the computation. Independent of the one step look-ahead or multi-step lookahead planning, each time the next movement is chosen as the one that minimizes a metric, in this case the value of A or D-optimality of the SLAM system covariance matrix. A. One step look-ahead simulation Performing active SLAM with a one-step look-ahead approach leads to completely different trajectories using the A and D-optimality criterion. The A-optimality plans trajectories with a distinctive local behaviour, while the D-optimality plans trajectories more globally, revisiting often previous landmarks. An example of the above behaviour is illustrated in Fig. 6. There the active SLAM starts after the robot has executed the predefined trajectory (black triangles) shown in Fig. 6(a), and partially discovers some landmarks (blue circles). The resulting trajectories after 10000 steps of simulations for A and D-optimality are shown respectively in Fig. 6(b) and Fig. 6(c). Each trajectory is identified by a different colour. An explanation of the difference in the trajectory planning behaviours due to the A or D-optimality used derives in the definition of the metric itself. As pointed out in the previous section, D-optimality encompasses the global uncertainty therefore revisiting previous landmarks (closing the loop) helps in decreasing the value of the metric. On the other hand, A-optimality criterion can be driven by a single eigenvalue, and therefore the uncertainty of the covariance matrix can get stuck in a local minimum. B. Two steps look-ahead simulation To perform a multi-step active SLAM approach is a cumbersome task, mainly because the numbers of possible trajectories to be considered before planning a trajectory are extremely high. Exactly, the number of trajectories are equal to the possible permutations obtained from a set with length equal to the quantity of known landmarks and arranging at each time i, which represents the amount of steps ahead. In order to reduce the number of places that have to be tested, a partition of the environment is performed with regions having a size equal to the robot’s sensor visibility range. The valid regions after the partition are those which contain one or more landmarks. An example of this partition is shown in Fig. 7, where the scheme of partition was used in a set of 1000 randomly distributed points, in an area of 1×1meters, which simulate the landmarks. The result of partitioning the environment having a circular shape visibility range of radius 0.1, is shown in Fig. 7(b). Each partitioned region is delimited by a red circle that depicts the boundaries of the sensor and the cross in the centre indicated the position of the sensor, which is where the robot has to stand. This type of partition is very common in sensor deployment problems [30] and reduces greatly the number of places to be tested, but it does not produce the optimal partition result. Firstly, because some partitioned regions are partially overlapped and secondly, because it considers that the sensor is equally accurate within its range. Taking into account the above statement, this kind of partitions are more suitable for any-time algorithms [31] used in real time constrained systems, where a suboptimal answer is better than nothing. Fig. 9shows the evolution of the root mean square error (RMSE) of the mean state vector for the A and D-optimality driven active SLAM approach for one and two step look-ahead planning. The ground map and the initial trajectory of the robot are shown in Fig. 8(a) along with the trajectories planned for the two steps look-ahead approach for each criterion, in Fig. 8(b) and Fig. 8(c) respectively. As expected, for the intrinsic global quality of its trajectory, the active SLAM based on the D-criterion yields a lower RMSE than the A-criterion based active SLAM. Moreover, the two steps case, as previously pointed out by [5], leads to a better result than a greedy approach. It is worth to remark, that −4 −2 0 2 4 6 8 10 12 14 −2 0 2 4 6 8 10 12 PREDEFINED PATH, features: 20 (a) −4 −2 0 2 4 6 8 10 12 14 −2 0 2 4 6 8 10 12 PATH A−opt, features: 20 (b) −4 −2 0 2 4 6 8 10 12 14 −2 0 2 4 6 8 10 12 PATH D−opt, features: 20 (c) Fig. 6. Resulting trajectories for a 10000 steps active SLAM simulation. (a). Predefined trajectory and landmarks ground truth. (b). A-opt based active SLAM. (c). DN−log-opt based active SLAM. 0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1 Simulation of 1000 randomly distributed landmarks (a) 0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1 Partition result (b) Fig. 7. Example of the partition proposed. (a). 1000 randomly distributed landmarks. (b) Result of the partition. because the re-planning is done after a complete trajectory (a sequence of discrete movement), a fair comparison between the metrics should be done at the end of each trajectory. The end points of each trajectory are marked in each figure. VI. CONCLUSIONS AND FUTURE WORK In this paper a clarification on the use and computation of the D-optimality criterion for a covariance matrix with variable size in time, in order to make comparisons of uncertainty evolution, is presented. This paper highlights that the definition for computing the D-optimality criterion provided by A. Wald [24] leads to wrong results because it does not take into account the change in dimensionality of the determinant. Instead of the above definition, the one that produces fruitful results is the proposed by J. Kiefer [20]. Furthermore, a solution for the problem of round-off errors in the computation of the D- optimality criterion is achieved by proposing its computation in the logarithmic space. This paper also demonstrates via simulation and real tests the above claims, and point out appealing characteristics (e.g. encompassing global uncertainty) for the use of D-optimality criterion as a measurement of the uncertainty of a SLAM system. Besides, it is shown that the use of D-optimality criterion, instead of the A-optimality, to drive an active SLAM approach seems more rewarding towards the fulfilling of the active SLAM objectives. Finally, as a way to overcome the complexity of computing active SLAM with a multi-step approach, a partition scheme of the environment based on the range of the robot sensor is used. As a future work, firstly we aim at testing the feasibility of the active SLAM approach used, in a real robot at different environments with time constraints and secondly, to include within the assumptions of the active SLAM, dynamic landmarks and obstacles. ACKNOWLEDGEMENT The authors would like to thank C´esar Cadena, Jos´e Neira and Ruben Martinez-Cantin for an earlier version of the code used. REFERENCES [1] U. Frese, “Interview: Is SLAM Solved?” KI - K¨unstliche Intelligenz, vol. 24, pp. 255–257, 2010. 1 [2] H. J. S. Feder, J. J. Leonard, and C. M. Smith, “Adaptive Mobile Robot Navigation and Mapping,” The International Journal of Robotics Research, vol. 18, no. 7, pp. 650–668, 1999. 1,2 [3] A. Makarenko, S. Williams, F. Bourgault, and H. Durrant-Whyte, “An experiment in integrated exploration,” in IEEE / RSJ Int. Conf. on Intelligent Robots and Systems, vol. 1, 2002, pp. 534–539. 1 [4] C. Stachniss, G. Grisetti, and W. Burgard, “Information Gain-based Exploration Using Rao-Blackwellized Particle Filters,” in Proceedings of Robotics: Science and Systems, Cambridge, USA, June 2005. 1 [5] S. Huang, N. Kwok, G. Dissanayake, Q. Ha, and G. Fang, “Multi-Step Look-Ahead Trajectory Planning in SLAM: Possibility and Necessity,” in IEEE Int. Conf. on Robotics and Automation, Apr 2005, pp. 1091 – 1096. 1,6 [6] T. Kollar and N. Roy, “Trajectory Optimization using Reinforcement Learning for Map Exploration,” The International Journal of Robotics Research, vol. 27, no. 2, pp. 175–196, 2008. 1 [7] R. Martinez-Cantin, N. de Freitas, E. Brochu, J. Castellanos, and A. Doucet, “A Bayesian exploration-exploitation approach for optimal online sensing and planning with a visually guided mobile robot,” Autonomous Robots, vol. 27, pp. 93–103, 2009. 1 [8] C. Leung, S. Huang, N. Kwok, and G. Dissanayake, “Planning under uncertainty using model predictive control for information gathering,” Robotics and Autonomous Systems, vol. 54, no. 11, pp. 898 – 910, 2006. 1 [9] T. Kollar and N. Roy, “Using reinforcement learning to improve exploration trajectories for error minimization,” in IEEE Int. Conf. on Robotics and Automation, May 2006, pp. 3338 –3343. 1 [10] C. Leung, S. Huang, and G. Dissanayake, “Active SLAM using Model Predictive Control and Attractor based Exploration,” in IEEE / RSJ Int. Conf. on Intelligent Robots and Systems, Oct 2006, pp. 5026 –5031. 1, 2 −4 −2 0 2 4 6 8 10 12 −4 −2 0 2 4 6 8 10 12 PREDEFINED PATH, features: 64 (a) −6 −4 −2 0 2 4 6 8 10 12 −4 −2 0 2 4 6 8 10 12 PATH A−opt, features: 64 (b) −6 −4 −2 0 2 4 6 8 10 12 −4 −2 0 2 4 6 8 10 12 PATH D−opt, features: 64 (c) Fig. 8. Resulting trajectories for a two-steps look-ahead active SLAM simulation. (a). Predefined trajectory and landmarks ground truth. (b). A-opt based active SLAM. (c). DN−log -opt based active SLAM. 0 100 200 300 400 500 600 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9x 10−3 Steps RMSE(m) Robot+Landmarks @Active SLAM n=1 A−opt D−opt (a) 0 100 200 300 400 500 600 3 4 5 6 7 8 9 10 x 10−3 Steps RMSE(m) Robot+Landmarks @Active SLAM n=2 A−opt D−opt (b) Fig. 9. RMSE evolution of the mean state vector associated to the robot and map while performing active SLAM. (a). One-step look-ahead. (b). Two-steps look-ahead. [11] T. Vidal-Calleja, A. Davison, J. Andrade-Cetto, and D. Murray, “Active control for single camera SLAM,” in IEEE Int. Conf. on Robotics and Automation, May 2006, pp. 1930 –1936. 1 [12] D. Meger, I. Rekleitis, and G. Dudek, “Heuristic search planning to reduce exploration uncertainty,” in IEEE / RSJ Int. Conf. on Intelligent Robots and Systems, Sep 2008, pp. 3392 –3399. 1 [13] L. Mihaylova, T. Lefebvre, H. Bruyninckx, K. Gadeyne, and J. De Schutter, “A Comparison of Decision Making Criteria and Optimization Methods for Active Robotic Sensing,” in Numerical Methods and Applications, ser. Lecture Notes in Computer Science. Springer Berlin / Heidelberg, 2003, vol. 2542, pp. 316–324. 1,2,3 [14] R. Sim and N. Roy, “Global A-Optimal Robot Exploration in SLAM,” in IEEE Int. Conf. on Robotics and Automation, Apr 2005, pp. 661 – 666. 1,3 [15] T. Lefebvre, H. Bruyninckx, and J. De Schutter, “Task Planning With Active Sensing For Autonomous Compliant Motion,” The International Journal of Robotics Research, vol. 24, no. 1, pp. 61–81, 2005. 1,3 [16] V. Fedorov, Theory of Optimal Experiments (Probability and mathematical statistics). Academic Press Inc, 1972. 1,2,3 [17] A. P´azman, Foundations of Optimum Experimental Design (Mathematics and its Applications). Springer, 1986. 1,3,4 [18] A. C. Atkinson and A. N. Donev, Optimum Experimental Designs (Oxford Statistical Science Series). Oxford University Press, USA, 1992. 1,2,3 [19] F. Pukelsheim, Optimal Design of Experiments (Classics in Applied Mathematics). Society for Industrial and Applied Mathematics, 2006. 1,2,3 [20] J. Kiefer, “General Equivalence Theory for Optimum Designs (Approximate Theory),” The Annals of Statistics, vol. 2, no. 5, pp. pp. 849–879, 1974. 1,4,5,7 [21] R. Bajcsy, “Active perception,” Proceedings of the IEEE, vol. 76, no. 8, pp. 966–1005, 1988. 2 [22] K. Smith, “On the Standard Deviations of Adjusted and Interpolated Values of an Observed Polynomial Function and its Constants and the Guidance They Give Towards a Proper Choice of the Distribution of Observations,” Biometrika, vol. 12, no. 1, pp. 1–85, 1918. 3 [23] J. Kiefer, “Optimum Experimental Designs,” Journal of the Royal Statistical Society. Series B (Methodological), vol. 21, no. 2, pp. 272– 319, 1959. 3,4 [24] A. Wald, “On the Efficient Design of Statistical Investigations,” The Annals of Mathematical Statistics, vol. 14, no. 2, pp. pp. 134–140, 1943. 3,5,7 [25] H. Chernoff, “Locally Optimal Designs for Estimating Parameters,” The Annals of Mathematical Statistics, vol. 24, no. 4, pp. pp. 586–602, 1953. 3 [26] S. Ehrenfeld, “On the Efficiency of Experimental Designs,” The Annals of Mathematical Statistics, vol. 26, no. 2, pp. pp. 247–255, 1955. 3 [27] J. de Geeter, J. de Schutter, H. Bruyninckx, H. van Brussel, and M. Decreton, “Tolerance-weighted L-optimal experiment design for active sensing,” in IEEE / RSJ Int. Conf. on Intelligent Robots and Systems, vol. 3, Oct 1998, pp. 1670–1675. 3 [28] T. Vidal-Calleja, A. Sanfeliu, and J. Andrade-Cetto, “Action Selection for Single-Camera SLAM,” Systems, Man, and Cybernetics, Part B: Cybernetics, IEEE Transactions on, vol. 40, no. 6, pp. 1567 –1581, Dec 2010. 3 [29] D. S. Watkins, “Understanding the QR Algorithm,” SIAM Review, vol. 24, no. 4, pp. 427–440, 1982. 4 [30] M. Schwager, D. Rus, and J. J. Slotine, “Unifying Geometric, Probabilistic, and Potential Field Approaches to Multi-Robot Deployment,” International Journal of Robotics Research, vol. 30, no. 3, pp. 371–383, Mar 2011. 6 [31] S. Zilberstein, “Using Anytime Algorithms in Intelligent Systems,” AI Magazine, vol. 17, no. 3, pp. 73–83, 1996. 6 On the Comparison of Uncertainty Criteria for Active SLAM Henry Carrillo, Ian Reid and Jos´e A. Castellanos Abstract—In this paper, we consider the computation of the D-optimality criterion as a metric for the uncertainty of a SLAM system. Properties regarding the use of this uncertainty criterion in the active SLAM context are highlighted, and comparisons against the A-optimality criterion and entropy are presented. This paper shows that contrary to what has been previously reported, the D-optimality criterion is indeed capable of giving fruitful information as a metric for the uncertainty of a robot performing SLAM. Finally, through various experiments with simulated and real robots, we support our claims and show that the use of D-opt has desirable effects in various SLAM related tasks such as active mapping and exploration. I. INTRODUCTION A model of the operative environment is an essential requirement for an autonomous mobile robot. The construction of this model requires the solution of at least three basic tasks for a mobile robot, namely localization, mapping and trajectory planning. The intersection of the first two tasks defines a key problem in modern robotics: Simultaneous Localization and Mapping (SLAM). SLAM is the problem of acquiring on-line and sequentially spatial data of an unknown environment in order to construct a map of it, and at the same time, allows the robot to localize itself in this map. To integrate the trajectory planning into SLAM allows a mobile robot to perform common tasks such as autonomous environment exploration. This approach is known as active SLAM and specifically refers to the problem of how to give a mobile robot the capability of generating on-line trajectories that simultaneously maximize the accuracy of the map and robot’s localization, regarding a SLAM task. The active SLAM paradigm was first proposed and tested in [1]. Since then, different approaches have been done. e.g. [2] and [3] proposed a discrete and greedy planning methodology. Huang et al. in [4] studied and tested the feasibility of multi-step planning. Continuous states planning but with a discretization in actions space is explored in [5]. Recently, a continuous planning approach in states and actions has been proposed by [6]. To the best of the authors’ knowledge, the different approaches that attempt to produce an active SLAM algorithm, This work has been supported by the MICINN-FEDER project DPI2009- 13710, and research grants BES-2010-033116 and EEBB-2011-44287. H. Carrillo and J. A. Castellanos are with the Departamento de Inform´atica e Ingenier´ıa de Sistemas, Instituto de Investigaci´on en Ingenier´ıa de Arag´on, Universidad de Zaragoza, C/ Mar´ıa de Luna 1, 50018, Zaragoza, Spain. {henry.carrillo, jacaste}@unizar.es I. Reid is with the Department of Engineering Science, University of Oxford, Parks Road, Oxford, United Kingdom. [email protected] rely on criteria or metrics that quantify the improvement of the actions taken by the robot (e.g. movements). This improvement is measured relative to (i) the robot and the map localization accuracy, (ii) the area of the map explored or (iii) the time that the robot has been navigating. Specifically, the metrics that relate the improvement of the localization accuracy or the uncertainty related to the movements the robot makes are of high value, because their uses allow the reduction of the map’s error, and therefore the probability to accomplish a given task is improved. Until now the preferred criterion to quantify the localization uncertainty has been the A-optimality criterion (A-opt). This criterion captures the mean uncertainty of the covariance matrix of a SLAM system. The choice of this criterion in many active SLAM related works such as [7], [8], [5], [9], and [6], among others, had its foundation in the fact that papers such as [10], [11], and [12] reported that (i) the A-opt applied to the problems of planning under uncertainty out performs other well-known criteria such as the D-optimality criterion (D-opt), and (ii) that the D-opt for the active SLAM case does not produce a meaningful metric. However, in the Theory of Optimal Experiment Design (TOED) [13] [14], it is well-known that the use of the D- opt has more appealing characteristics than the A-opt or E- optimality criterion (E-opt). Moreover, Kiefer in [15] demonstrated that the A-opt,D-opt and E-opt are special cases of a general family of uncertainty criteria and therefore they share some properties, but D-opt is the only one proportional to the uncertainty ellipse of the estimated parameters, and it is also invariant to re-parametrizations and linear transformations [14]. In this paper, it is shown that is indeed possible to obtain a fruitful metric from the D-opt for the particular case of a mobile robot performing SLAM. Also, it is shown experimentally that its use as a metric for quantifying the uncertainty of the robot and map in an active SLAM context, performs comparably to the A-opt metric popularized by [10], [11] and [12]. The reminder of the paper is structured as follows: section II gives an overview of the active SLAM problem and its connection to the TOED. Section III shows how to compute D-opt in order to be compared correctly, and to allow its use in an active SLAM or path planning under uncertainty context. Sections IV and Vreport several experiments with simulated and real robots that support our claims. Finally, section VI presents the conclusions. II. ACTIVE SLAM The SLAM problem does not establish which trajectories a robot has to follow. Usually, they are chosen randomly or beforehand. However, it is well-known that the trajectories selected and the order they are executed by a robot, are critical, among other things, firstly for a rapidly convergence of the uncertainty of a SLAM algorithm, secondly for increasing the area of the environment explored by the robot, and thirdly to improve the possibility of fulfilling tasks. The integration of the trajectory planning task into SLAM was first proposed in [1] and the term active SLAM referring to the aforementioned integration was coined by [8]. The general idea of active SLAM can be summarized as follows in algorithm 1: Algorithm 1 The active SLAM algorithm Require: •A complete or incomplete stochastic map of the environment Mk={ˆ xFk,Σk}. •The length iof the horizon of planning. Ensure: •A policy class of trajectories π. 1: Create a set πsof sdifferent policy classes with itrajectories each one. The initial trajectory of each policy starts at ˆ xRk. 2: Perform a SLAM algorithm using each policy class and the given map Mk. 3: Compute a value function Jfor each policy class of πs, using the information of each trajectory followed and the final covariance matrix associated to the SLAM algorithm. 4: Select the policy class πopt that optimizes J. The SLAM approach taken above is based on a probabilistic state-space model, where the robot Rand a set of features or landmarks in the environment F={F1,...,Fn}are represented by a stochastic state vector xwith an estimated mean ˆ xand associated covariance matrix Σ. Furthermore, ˆ x=ˆ xR ˆ xF;Σ=ΣRR ΣRF ΣFRΣF F (1) where ˆ xRand ˆ xFare the estimated locations of the robot and the landmarks respectively, ΣRR is the covariance matrix of the estimated robot pose (e.g. x, y, θ) and it has a size of p× pthat is invariant with respect to the time, ΣF F represents the covariance matrix of the estimated locations of the discovered landmarks and it has a size of n×nthat varies over time. Finally, ΣRFand ΣFRare matrices that encode the crosscovariance of the robot pose and the landmarks estimations. The covariance matrix Σhas size l×l, where l=p+n, and its value is variable with time. Moreover, it is a positive semi-definitive matrix with eigenvalues {λ1,...,λl}. A. The value function As mentioned above, the integration of trajectory planning or, what is equivalent, applying the active sensing paradigm [16] [10] to the SLAM problem, involves the optimization of a multi-objective performance criterion or value function J. This value function is used to decide which trajectories have to be followed by the robot. A definition of this value function can be as follows: J=X i αiUi+X i βiTi(2) Where the index idefines the length of the planning horizon (i.e. the numbers of consecutive trajectories planned ahead). The first term, Uicharacterizes the expected cost of the uncertainty in the parameters of the system. The second term, Tiincludes other expected costs such as trajectory length, navigation time, and energy consumption, among others. Finally, αand βare weight coefficients for tuning the parameters and are task dependant. The Uiterm can be further specified as a metric of the associated covariance matrix Σ(e.g. the determinant, the trace). This metric needs to encode the robot and the landmarks’ estimated locations uncertainty and can be defined as follows: Ui:Σ→R(3) The different ways to compute the above metric and their properties in relation to the goals of the active SLAM approach is the target of the following sections of this paper. Moreover, a clarification in the computation of one of them is pointed out in section III. The second term Ti, as done previously, can be further specified and constrained as a metric that represents the cost of performing a free collision trajectory Γby the robot, Ti: Γ →R(4) This metric can be constrained to be a function only of the distance travelled, since its cost is directly related to the power and navigation time of the robot while it performs a task. Finally, summarizing all the above definitions, the statement of the active SLAM problem can be formulated as: the task of choosing a single or multiple step policy class πof robot’s trajectories that optimize a value function J. B. Theory of Optimal Experiment Design and active SLAM In the Theory of Optimal Experiment Design (TOED) [13] [14], a single trial of an experiment is the process of changing the input parameters of a system perturbed with unknown noise, with the purpose of observing the variation in the output parameters. In this context, the particular values of the input parameters are known as a particular design ξ. In the active SLAM context, the ξdesign is a particular policy class πcommanded to the robot, the unknown noise is the commonly assumed zero mean Gaussian noise and the variation of the parameters is encoded in the covariance matrix Σ.Based on the TOED, it is possible to know if a design ξ1 is better than a design ξ2[13] [14]. Applying this concept in the active SLAM context, a policy class π1is better in terms of uncertainty than a policy class π2if : Cov(π1)−Cov(π2)∈NND(l)(5) Planning Minimum Uncertainty Paths Over Pose/Feature Graphs Constructed Via SLAM Henry Carrillo and Jos´e A. Castellanos Abstract—This paper addresses the problem of path planning considering uncertainty metrics over the belief space. Specifically, we propose a path planning algorithm that uses a novel determinant-based measure of uncertainty to obtain the minimum uncertainty path from a roadmap. Our proposal does not require a priori knowledge of the environment due to the construction of the roadmap via a graph-based SLAM algorithm. We report experimental results of our proposal in two real dataset that show its feasibility to obtain the minimum uncertainty path towards an autonomous navigation framework. I. INTRODUCTION Path planning solely in the Cfree (i.e. only taking into account geometric constrains) does not guaranty the safety of a robot navigating over those paths [1] [2] [3]. The main reason for the above problem stems from the uncertainty generated by the inherent noise in the localization and control systems of a robot working in a real environment due to the imperfect data it gathers. Moreover, a robot navigates an environment in order to fulfil a task, and an initial condition to effectively complete any task is to accurately reach a priori initial position established in the workspace where this task will be performed. e.g. A mobile manipulator aiming at opening a door using visual servoing, needs to position its manipulator within the range of the doorknobs. Again, reaching accurately a position using solely path planned in the Cfree cannot be guaranteed, because the sensor inherently gather data with noise (i.e. due to our inability to model every detail within the environment) that prevent performing control algorithms which follow perfectly a path. In order to overcome the above problem, several works have proposed the integration of the uncertainty in the path planning process: [4] [5] [3], and more naturally [6] has proposed to plan over the so-called belief space. The use of the belief space in the proposal of different path planners [7] [8] [9] had proved experimentally that, taking into account the uncertainty in the planning process leads to an accurate and safe navigation process. All the aforementioned path planners, which use the belief space, rely on metrics or criteria that measure or quantify the uncertainty or its dual the information of certain configuration in the space. Among the most used metrics are [10] [11] [12]: the trace of the covariance matrix, the entropy and the mutual This work has been supported by the MICINN-FEDER project DPI2009- 13710, and research grants BES-2010-033116 and EEBB-2011-44287. H. Carrillo and J. A. Castellanos are with the Departamento de Inform´atica e Ingenier´ıa de Sistemas, Instituto de Investigaci´on en Ingenier´ıa de Arag´on, Universidad de Zaragoza, C/ Mar´ıa de Luna 1, 50018, Zaragoza, Spain. {henry.carrillo, jacaste}@unizar.es information. In this paper, we devise a path planner that relies on a novel determinant-based metric to take into account the effects of uncertainty and that can be seamlessly integrated to recently proposed graph-based SLAM algorithms [13] [14] [15] [16]. The reminder of the paper is structured as follows: Section II presents a brief overview of the uncertainty metrics commonly utilized in path planners that uses the belief space and presents in more detail the novel metric used in our approach. In section III, we define the problem we are dealing with: planning the minimum uncertainty path in a roadmap like structure, also in this section, we discuss some conditions to guarantee that we actually obtain the minimum uncertainty path from a roadmap. Section IV reports our path planner that use a novel determinant based metric to quantify the uncertainty and is designed to seamlessly integrate with a graph-based SLAM algorithm. Section Vpresents two experimental trials of our approach in real datasets and finally section VI gives some conclusions. II. UNCERTAINTY MEASURES Historically, the uncertainty metrics were first proposed in the Theory of Optimal Experiment Design (TOED) [17] [18] context and were named like an alphabet with the suffix optimality attached to them to denote the origin. This metrics or criteria coming from the TOED aim at capturing the idea of whether or not the uncertainty of a covariance matrix, Σ, is large or small. The use of the covariance matrix to quantify the uncertainty has a strong base in the TOED literature [17] [18] [19], moreover it has links with the information theory through the Cram´er-Rao bound [20]. Formally, an uncertainty criterion has to define a function φthat maps a NND covariance matrix of size l×lto a scalar, φ: NND(l)→R(1) Where NND(l) stands for the group of non-negative definite matrices of size l×l. NND matrices are also known as positive semi-definite matrices [18]. The above function has to be positive homogeneous, isotonic (i.e. order preserving), concave [18] and defines the magnitude of the uncertainty. A compendium of functions fulfilling the above requirements can be found in [17] or [18]. Among the most commonly used functions or uncertainty criteria, for a covariance matrix Σwith size l×land eigenvalues λl, we find: •A-optimality criterion (A-opt) [21]: This criterion targets the minimization of the average variance and it is defined as follows, trace(Σ) = X k=1,...,l λk(2) •D-optimality criterion (D-opt) [22]: This criterion aims at capturing the full dimension of the covariance matrix and at first glance it can be defined as det(Σ) = Y k=1,...,l λk(3) •E-optimality criterion (E-opt) [23]: This criterion intends to minimize the maximum eigenvalue of the covariance matrix Σ. The main advantage of this criterion is the simplicity of its computation, but it is a rough approximation of the error ellipsoid. According to the TOED [17] [18] the D-opt gives the most accurate approximation of the uncertainty enclosed in the covariance matrix, but in the active SLAM or planning under uncertainty context [10], [11], [12] and [24], have shown that using the definition in (3) to compute the D-opt does not produce a meaningful metric (i.e. The value gets stuck in zero). In [25] a novel computation form of the uncertainty criterion based on the determinant of the covariance matrix is presented. There the D-opt is computed as follows: exp(l−1X k=1,...,l log(λk)) (4) that stems from the family of uncertainty criteria proposed by Kiefer in [26], φp(ξ) = [l−1trace(Σp(ξ))]1/p (5) This family of uncertainty criteria is valid in the range of 0< p < ∞for a covariance matrix (Σ) of size l×lassociated to a design ξ(e.g. π). Moreover, the case φ1and the boundary cases φ0and φ∞are the already known A, D and E-optimality criteria. Using (4) instead of (3) in order to compute the D-opt in the planning under uncertainty or active SLAM context has the advantage of producing a meaningful uncertainty metric. i.e. it does get stuck in zero and evolves resembling the uncertainty encompassed in the covariance matrix. Furthermore, according to the TOED is the uncertainty criterion that by definition truly captures the complete dimension of the uncertainty, unlike the A-opt and E-opt that are approximations [17] [18] [19]. III. PATH PLANNING IN THE BELIEF SPACE Assuming that the data structure representing the environment (i.e. a map) is a graph-like structure (i.e. a metrictopological representation) we could use the well-known Probabilistic Roadmaps (PRM) algorithm to generate a discrete graph in Cfree (i.e. the set of configuration at which the robot does not intersect any obstacle [27]). Given a start configuration XStart of a robot and a goal configuration XGoal, within the above discrete graph, we desire to find the minimum uncertainty path between them. Planning for the minimum uncertainty path cannot be longer done in the Cfree, because it cannot guarantee the safety we Fig. 1. Example of a graphical representation of a belief roadmap. are aiming at. Therefore, it seems more natural to use another space such as the belief (B) or information (I) space [6] [8] [2]. Autonomous path planning in the belief or information space, as any autonomous path planner, relies heavenly in metrics that quantify the cost of moving from one configuration to another. In the case of the belief space, the most common used metrics are the ones based on the TOED (uncertainty) and in the information theory. In both cases, and unlike the metrics used in the configuration space, the evolution of the uncertainty or information metrics are non-monotonic, and so far there is not an optimistic heuristic that allows the use of the plethora of well-know and effective non-uniform path planner such as: A* [28] or D* [29] [30]. Even the use of more traditional path planning algorithm (e.g. Breadth-first search, depth-first search) could lead to misleading results regarding the planning of the minimum uncertainty path. Take for instance, the graph showed in Fig. 1. There, we have a start and a goal nodes, labelled XStart and XGoal respectively, and a series of intermediate nodes n1,...,n7. Each of these nodes encodes a configuration pose in Cfree and the link between two of them has a fixed cost associated to the uncertainty in the goal position. If we perform an exhaustive search over the graph, the minimum uncertainty path will be {XStart,n1,n3,n5,n7,XGoal}with an associated cost of 7 in the goal node. Using for instance the breadth-first search based solution proposed in [9] will result in the following path: {XStart,n1,n2,XGoal}with an associated cost of 10 in the goal node. This result stems from the stop condition of line 9 in algorithm 1 in [9] that is designed to trigger as soon as the goal node is reached. This behaviour is typical of a greedy algorithm that cannot guarantee the minimum uncertainty path to the goal node [31]. Another example of the above behaviour can be corroborated using the Algorithm 2 in [8]. Using that algorithm, we end up with the following path: {XStart,n1,n2,n4,n6,XGoal}with an associated cost of 8 in the goal node. In this algorithm the greedy behaviour is due to the condition imposed to feed the queue (Line 12 to 15). There is no doubt that imposing a discretization of the environment with the PRM algorithm, prevents any discrete search algorithm to guarantee that it will find the minimum uncertainty path. However, within a discrete graph built upon a Algorithm 1 The minimum uncertainty path planning process in a pose/feature graph map Require: •A pose/feature graph map of the environment. •A initial pose nsand a goal pose ng. Ensure: •The path with the minimum cost from the pose nsto the pose ng. 1: Initialize an empty search queue Qwith the initial position, covariance and features seen by ns:Q ← ns= {µs,Σs,es} 2: Initialize an empty search queue C, that will store the value of the uncertainty associated to traverse from a node na to a node nb, as well as the information of those nodes. 3: while Qis not empty do 4: Pop n← Q 5: if n=ngthen 6: Push {n, n} → C 7: Continue 8: end if 9: for i= Range(Successors(n)) do 10: ComputeCost(n,i) 11: Push i→ Q 12: Push {n, i} → C 13: end for 14: end while 15: return SelectPath(ns,ng,C) tractable computing premise according to the PRM algorithm, is possible to find the minimum uncertainty path. Another issue with the PRM algorithm is its requirement of a priori knowledge of the environment to produce the roadmap. This constraint limits the feasibility of the integration of the path planner in an autonomous robot framework. An approach based on a SLAM algorithm can produce a roadmap of the environment and overcomes the aforementioned issue. Specifically, we can use a graph-based SLAM algorithm such as: [13] [14] [15] [16], that does not need a beforehand knowledge of the environment and can produce a good estimate of the environment enclosed in a pose/feature graph. IV. OUR APPROACH In the following, we present an algorithm capable of planning the minimum uncertainty path using a pose/feature graph map. This approach has been previously applied in a SLAM context by [7] using a cost function based on the trace of the covariance matrix and by [9] using an entropy based cost function, but both algorithms rely on a greedy approach that cannot guarantee the obtention of the minimum uncertainty path. Our proposal, in contrast, uses a cost function based on the D-opt and an exhaustive search. We report results of its implementation in two real dataset. Our algorithm requires a pose/feature graph map, such as the produced by graph based SLAM algorithms (e.g. iSAM [14]), a start (XStart) and goal (XGoal) pose within the graph, Algorithm 2 The path selection process: SelectPath(ns,ng,C) Require: •A queue Cwith the associated cost of traverse from the node nato the node nb. •A initial pose nsand a goal pose ng. Ensure: •The path with the minimum cost from the pose nsto the pose ng. 1: Global C 2: i={Cost ←0, Path ←0, Pose ←ng} 3: return PathR(ns,i) The PathR procedure: proc PathR(ns, i)≡ for j=Range(Parents(i)) j.Cost =i.Cost +cost(i, j) j.Path =i.Path ∪j.P ose if j.Pose =ns return j.Path else L.append(PathR(ns, j)) end end return MinCost(L) end. and ensure the path with minimum uncertainty from XStart to XGoal according to the D-opt (c.f. section II and (4)). Algorithm 1outlines our proposal. It starts by creating two queues, the first will contain unvisited nodes and it is initialized with the information of the start node (Line 1). The second queue will store the cost of traversing between nodes (Line 2). After the initialization, a graph search process (Line 3-14) retrieves the uncertainty associated to traverse each node and stores it in a queue (i.e. C). The uncertainty of going from a node nato a node nbis calculated by the function ComputeCost(na,nb) (Line 10). The procedure to compute this cost is one of the main differences between our approach and the presented in [8] or [9]. Our approach is based on the D-opt of the joint compatibility matrix of the marginal covariance [32] belonging to the target node. For a pose node with k visible landmarks, each one with covariance Σjand the node itself with covariance ΣX, the aforementioned matrix is: ΣXlk=     ΣXX ΣXl1··· ΣXlk Σl1XΣl1l1··· Σl1lk . . .. . ..... . . ΣlkXΣlkl1··· Σlklk      (6) The use of D-opt over other uncertainty metrics such as A-opt or E-opt is supported extensively in the TOED [17] [18], mainly because D-opt is a criterion designed to capture the complete dimension of the uncertainty in a covariance matrix. Regarding the entropy based uncertainty metrics, D- (a) Initial graph map (b) Map with the shortest path highlighted (c) Map with the minimum uncertainty path highlighted Fig. 2. (a) Initial graph map of the DLR dataset. In dark blue is the estimated trajectory of the robot and in brown are depicted the position of the landmarks. (b) In red is highlighted the shortest path from the start to the goal node. (c) In red is highlighted the minimum uncertainty path from the start to goal node. This figure is best viewed in color. 0 50 100 150 200 250 300 350 400 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 # Poses Accumulated cost Minimum uncertainty path Shortest path (a) 0 50 100 150 200 250 300 350 400 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 # Poses Positional error at each node (m) Minimum uncertainty path Shortest path (b) Fig. 3. (a) Evolution of the accumulated cost for the selected path in the DLR dataset. (b) Positional error for each path taking as a ground truth a batch optimized map of the entire DLR dataset. opt behaves similarly under the mild and common assumption of gaussianity as reported in [25], moreover its computation is less complex. Finally, in algorithm 1the minimum uncertainty path is reconstructed by the procedure SelectPath(⋆) that back-track the paths from the start node nsto the goal node ngand then select the minimum cost path. This procedure is outlined in algorithm 2. V. EXPERIMENTAL RESULTS The proposed approach was tested in two real dataset. From the odometry and features constraints given by each dataset, an initial pose/feature graph is constructed through a SLAM algorithm [14], next the graph used for path planning is refined through an adaptation of the procedure presented in [33]. This procedure can be summarized as follows: •If the robot rotates more than θradians or move more than xmeters a new pose node is added jointly with any observed feature. •Each observed feature is matched with previous observations to guarantee data association. •If a new pose node is added in less than ymeters from a previous pose (e.g. when the robot revisits a previously unknown area) a constraint is added between the nodes. In each dataset a start and a goal within the pose/feature graph map was selected and the proposed approach was carried out. The first dataset used, was recorded at the Deutsches Zentrum fur Luft und Raumfahrt (DLR) with a mobile platform [34]. The environment is a typical office indoor environment and covers a region of 60m x 45m. One of the main characteristics of this dataset is that it has hand annotated features data association. In order to obtain the pose/feature graph map of the environment, the iSAM [14] algorithm was used. The resulting pose/feature graph map of the dataset is shown in Fig. 2, as well in the same figure is shown the shortest (Fig.2b) and minimum uncertainty path (Fig. 2c). The resulting graph of the DLR dataset has 3816 nodes and 17042 measurements (i.e. edges) and a normalized χ2error of 0.072. Fig. 3a shows the evolution of the accumulated cost for the two paths. The uncertainty of the shortest path is bigger than the minimum uncertainty path, which implies that if the robot follows the minimum uncertainty path it will be better localized and will increase the chances of getting to the goal node. This implication although, depends on how consistent is the belief of the robot, because the uncertainty is measured solely from the covariance matrix that can be alter if the SLAM algorithm is overconfident. This DLR dataset lacks of ground truth, therefore we cannot measure the true positional error of both paths, but as a first approximation, we adopt a batch optimized map of the entire dataset as a ground truth and compare the Mean Squared Error (MSE) of the position of the robot in the two paths. Fig. 3b shows the plot of the positional error at each node for both paths, the minimum uncertainty path has a better localization in the final goal destination as predicted for the low uncertainty achieved during its path. The second dataset used, is the well-known Victoria park dataset. This dataset was recorded in an outdoors environment and provides among others, data from odometry and a laser mounted over a vehicle that is traversing a natural park populated with trees (that can be used as landmarks). As above, (a) Initial graph map (b) Map with the shortest path highlighted (c) Map with the minimum uncertainty path highlighted Fig. 4. (a) Initial graph map of the Victoria park dataset. In dark blue is the estimated trajectory of the robot and in green are depicted the position of the landmarks. (b) In red is highlighted the shortest path from the start to the goal node. (c) In red is highlighted the minimum uncertainty path from the start to the goal node. This figure is best viewed in color. 0 50 100 150 200 250 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 # Poses Accumulated cost Minimum uncertainty path Shortest path (a) 0 50 100 150 200 250 0 1 2 3 4 5 6 7 # Poses Positional error at each node (m) Shortest path Minimum uncertainty path (b) Fig. 5. (a) Evolution of the accumulated cost for the selected path in the Victoria park dataset. (b) Positional error for each path taking as a ground truth a batch optimized map of the entire Victoria park dataset. the iSAM algorithm by Kaess [14] was used to estimate the trajectory and the map of the environment. The pose/feature graph map of the dataset is shown in Fig. 4, along the shortest (Fig.4b) and the minimum uncertainty path (Fig. 4c). The resulting graph of the Victoria park dataset has 7120 nodes and 10609 measurements (i.e. edges) and a normalized χ2 error of 0.8862. Fig. 5a shows the evolution of the accumulated cost for the two paths. As in the previous dataset, the uncertainty of the shortest path is bigger than the minimum uncertainty path. Although this dataset has some GPS based ground truth, it is sparse and does not cover the area used for experimentation. Therefore, we used the approach of the previous experiment in order to measure the positional error of both paths. Fig. 5shows the plot of the positional error at each node for both paths, there, the minimum uncertainty path has a better localization in the final goal destination as predicted from the low uncertainty achieved during its path. VI. DISCUSSION In this paper, we proposed a path planning algorithm capable of obtaining the minimum uncertainty path according to a determinant-based criterion. In the literature, to the best knowledge of the authors of this article, this is the first use of the determinant-based criterion to quantify the uncertainty of the robot and environment in a path planning under uncertainty context, therefore accurately capturing the complete dimension of the uncertainty according to the TOED [17] [18]. The proposed algorithm produces, via an exhaustive search, the minimum uncertainty path from an initial configuration of the robot until the goal one. We use an exhaustive search because the minimum uncertainty path in the belief space cannot be guarantee in a greedy search procedure such as the proposed in [8] or [9] due to the non-monotonic evolution of the uncertainty. Moreover, the non-monotonic evolution prevent the direct use of well-known non-uniform path planning algorithm such as A* or D*. The proposal of optimistic heuristics of the uncertainty should be the focus of future work in the path planning under uncertainty research if we desire to use the incremental, computational tractable yet complete path planning solutions of [29], [30] and [35], among others. Although the proposed exhaustive search is unsuitable for real time re-planning, it is a suitable off-line procedure to obtain a good starting plan that can be re-planned, if the environment changes, with efficient greedy search approaches. Finally, as pointed out in [9], the use of graph-based SLAM algorithm to create the initial roadmap overcomes the necessity of a priori knowledge of the environment, therefore bringing the solution closer to the reality. Furthermore, liking the uncertainty measure of the path planning to the structured of the SLAM problem - i.e. by using the joint compatibility matrix - is a step forward in the integration of mapping , exploration and planning towards achieving truly autonomous robots. ACKNOWLEDGMENT The authors gratefully acknowledge Jos´e Guivant for making the Victoria Park dataset public, Udo Frese for making the DLR dataset public and Michael Kaess for making public iSAM: Incremental Smoothing and Mapping. REFERENCES [1] J.-P. Laumond, Robot Motion Planning and Control. Berlin: Springer- Verlag, 1998, available online at http://www.laas.fr/∼jpl/book.html. 1 [2] J. Barraquand and P. Ferbach, “Motion Planning with Uncertainty: The Information Space Approach,” in Proceedings IEEE International Conference on Robotics & Automation, 1995, pp. 1341–1348. 1,2 [3] A. Lambert and D. Gruyer, “Safe path planning in an uncertainconfiguration space,” in IEEE Int. Conf. on Robotics and Automation, 2003, pp. 4185–4190. 1 [4] L. Mihaylova, J. D. Schutter, and H. Bruyninckx, “A Multisine Approach for Trajectory Optimization Based on Information Gain,” in Proceedings IEEE International Conference on Robotics & Automation, 2002, pp. 661–666. 1 [5] J. Gonzalez and A. Stentz, “Planning with Uncertainty in Position: An Optimal and Efficient Planner,” in IEEE / RSJ Int. Conf. on Intelligent Robots and Systems, August 2005, pp. 2435 – 2442. 1 [6] S. Prentice and N. Roy, “The belief roadmap: Efficient planning in linear pomdps by factoring the covariance,” in Proceedings of the 13th International Symposium of Robotics Research, Hiroshima, Japan, November 2007. 1,2 [7] R. He, S. Prentice, and N. Roy, “Planning in information space for a quadrotor helicopter in a GPS-denied environment,” in IEEE Int. Conf. on Robotics and Automation, 2008, pp. 1814–1820. 1,3 [8] S. Prentice and N. Roy, “The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance,” The International Journal of Robotics Research, 2009. 1,2,3,5 [9] J. Valencia, R. Andrade Cetto and J. Porta, “Path planning in belief space with Pose SLAM,” in IEEE International Conference on Robotics and Automation, vol. 3, May 2011, pp. 78–83. 1,2,3,5 [10] J. de Geeter, J. de Schutter, H. Bruyninckx, H. van Brussel, and M. Decreton, “Tolerance-weighted L-optimal experiment design for active sensing,” in IEEE / RSJ Int. Conf. on Intelligent Robots and Systems, vol. 3, Oct 1998, pp. 1670–1675. 1,2 [11] L. Mihaylova, T. Lefebvre, H. Bruyninckx, K. Gadeyne, and J. De Schutter, “A Comparison of Decision Making Criteria and Optimization Methods for Active Robotic Sensing,” in Numerical Methods and Applications, ser. Lecture Notes in Computer Science. Springer Berlin / Heidelberg, 2003, vol. 2542, pp. 316–324. 1,2 [12] R. Sim and N. Roy, “Global A-Optimal Robot Exploration in SLAM,” in IEEE Int. Conf. on Robotics and Automation, Apr 2005, pp. 661 – 666. 1,2 [13] E. Olson, J. Leonard, and S. Teller, “Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates,” 2006, pp. 2262–2269. 1,3 [14] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental Smoothing and Mapping,” Robotics, IEEE Transactions on, vol. 24, no. 6, pp. 1365 –1378, Dec 2008. 1,3,4,5 [15] K. Konolige and M. Agrawal, “FrameSLAM: From Bundle Adjustment to Real-Time Visual Mapping,” Robotics, IEEE Transactions on, vol. 24, no. 5, pp. 1066 –1077, oct. 2008. 1,3 [16] C. Mei, G. Sibley, M. Cummins, P. Newman, and I. Reid, “RSLAM: A System for Large-Scale Mapping in Constant-Time Using Stereo,” International Journal of Computer Vision, vol. 94, pp. 198–214, 2011, 10.1007/s11263-010-0361-7. 1,3 [17] A. P´azman, Foundations of Optimum Experimental Design (Mathematics and its Applications). Springer, 1986. 1,2,3,5 [18] F. Pukelsheim, Optimal Design of Experiments (Classics in Applied Mathematics). Society for Industrial and Applied Mathematics, 2006. 1,2,3,5 [19] V. Fedorov, Theory of Optimal Experiments (Probability and mathematical statistics). Academic Press Inc, 1972. 1,2 [20] L. L. Scharf and C. Demeure, Statistical Signal Processing. Detection, Estimation and Time series analysis. Addison–Wesley, 1991. 1 [21] H. Chernoff, “Locally Optimal Designs for Estimating Parameters,” The Annals of Mathematical Statistics, vol. 24, no. 4, pp. pp. 586–602, 1953. 1 [22] A. Wald, “On the Efficient Design of Statistical Investigations,” The Annals of Mathematical Statistics, vol. 14, no. 2, pp. pp. 134–140, 1943. 2 [23] S. Ehrenfeld, “On the Efficiency of Experimental Designs,” The Annals of Mathematical Statistics, vol. 26, no. 2, pp. pp. 247–255, 1955. 2 [24] T. Lefebvre, H. Bruyninckx, and J. De Schutter, “Task Planning With Active Sensing For Autonomous Compliant Motion,” The International Journal of Robotics Research, vol. 24, no. 1, pp. 61–81, 2005. 2 [25] H. Carrillo, I. Reid, and J. A. Castellanos, “On the Comparison of Uncertainty Criteria for Active SLAM,” Sep 2011. [Online]. Avalible: http://webdiis.unizar.es/∼hcarri/A.pdf.2,4 [26] J. Kiefer, “General Equivalence Theory for Optimum Designs (Approximate Theory),” The Annals of Statistics, vol. 2, no. 5, pp. pp. 849–879, 1974. 2 [27] H. Choset, W. Burgard, S. Hutchinson, G. Kantor, L. E. Kavraki, K. Lynch, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementation. MIT Press, June 2005. 2 [28] P. Hart, N. Nilsson, and B. Raphael, “A Formal Basis for the Heuristic Determination of Minimum Cost Paths,” IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, Feb. 1968. 2 [29] A. Stentz, “The Focussed D* Algorithm for Real-Time Replanning,” in Proceedings of the International Joint Conference on Artificial Intelligence, August 1995. 2,5 [30] S. Koenig and M. Likhachev, “Fast replanning for navigation in unknown terrain,” IEEE Transactions on Robotics, vol. 21, no. 3, pp. 354–363, 2005. 2,5 [31] S. Huang, N. Kwok, G. Dissanayake, Q. Ha, and G. Fang, “Multi-Step Look-Ahead Trajectory Planning in SLAM: Possibility and Necessity,” in IEEE Int. Conf. on Robotics and Automation, Apr 2005, pp. 1091 – 1096. 2 [32] M. Kaess and F. Dellaert, “Covariance Recovery From a Square Root Information Matrix for Data Association,” Robotics and Autonomous Systems, vol. 57, no. 12, pp. 1198 – 1210, 2009. 3 [33] R. G. Grisetti, R. Kuemmerle, C. Stachniss, and W. Burgard, “A Tutorial on Graph-Based SLAM,” Intelligent Transportation Systems Magazine, IEEE, vol. 2, no. 4, pp. 31 –43, winter 2010. 4 [34] J. Kurlbaum and U. Frese, “DLR Spatial Cognition Data Set,” December 2008. [Online]. Avalible: http://www.informatik.uni-bremen.de/agebv/en/DlrSpatialCognitionDataSet. 4 [35] J. van den Berg, D. Ferguson, and J. Kuffner, “Anytime Path Planning and Replanning in Dynamic Environments,” in IEEE Int. Conf. on Robotics and Automation, May 2006, pp. 2366 – 2371. 5 References [1] H. J. S. Feder, J. J. Leonard, and C. M. Smith, “Adaptive Mobile Robot Navigation and Mapping,” The International Journal of Robotics Research, vol. 18, no. 7, pp. 650–668, 1999. 1,3 [2] A. Makarenko, S. Williams, F. Bourgault, and H. Durrant-Whyte, “An experiment in integrated exploration,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), vol. 1, 2002, pp. 534–539. 1 [3] C. Stachniss, G. Grisetti, and W. Burgard, “Information Gain-based Exploration Using Rao-Blackwellized Particle Filters,” in Proceedings of Robotics: Science and Systems, Cambridge, USA, June 2005. 1 [4] S. Huang, N. Kwok, G. Dissanayake, Q. Ha, and G. Fang, “Multi-Step Look-Ahead Trajectory Planning in SLAM: Possibility and Necessity,” in IEEE International Conference on Robotics and Automation (ICRA), Apr 2005, pp. 1091 – 1096. 1,14 [5] T. Kollar and N. Roy, “Trajectory Optimization using Reinforcement Learning for Map Exploration,” The International Journal of Robotics Research, vol. 27, no. 2, pp. 175–196, 2008. 1 [6] R. Martinez-Cantin, N. de Freitas, E. Brochu, J. Castellanos, and A. Doucet, “A Bayesian exploration-exploitation approach for optimal online sensing and planning with a visually guided mobile robot,” Autonomous Robots, vol. 27, pp. 93–103, 2009. 1 [7] T. Kollar and N. Roy, “Using reinforcement learning to improve exploration trajectories for error minimization,” in IEEE International Conference on Robotics and Automation (ICRA), May 2006, pp. 3338 –3343. 1 [8] C. Leung, S. Huang, and G. Dissanayake, “Active SLAM using Model Predictive Control and Attractor based Exploration,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct 2006, pp. 5026 –5031. 1,3 [9] D. Meger, I. Rekleitis, and G. Dudek, “Heuristic search planning to reduce exploration uncertainty,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Sep 2008, pp. 3392 –3399. 1 45 REFERENCES REFERENCES [10] L. Mihaylova, T. Lefebvre, H. Bruyninckx, K. Gadeyne, and J. De Schutter, “A Comparison of Decision Making Criteria and Optimization Methods for Active Robotic Sensing,” in Numerical Methods and Applications, ser. Lecture Notes in Computer Science. Springer Berlin / Heidelberg, 2003, vol. 2542, pp. 316–324. 1,2,4,8,12, 18 [11] R. Sim and N. Roy, “Global A-Optimal Robot Exploration in SLAM,” in IEEE International Conference on Robotics and Automation (ICRA), Apr 2005, pp. 661 – 666. 1,2,8,12,18 [12] T. Lefebvre, H. Bruyninckx, and J. De Schutter, “Task Planning With Active Sensing For Autonomous Compliant Motion,” The International Journal of Robotics Research, vol. 24, no. 1, pp. 61–81, 2005. 1,2,8,12,18 [13] A. P´azman, Foundations of Optimum Experimental Design (Mathematics and its Applications). Springer, 1986. 2,5,6,9 [14] F. Pukelsheim, Optimal Design of Experiments (Classics in Applied Mathematics). Society for Industrial and Applied Mathematics, 2006. 2,5,6,8 [15] J. Kiefer, “General Equivalence Theory for Optimum Designs (Approximate Theory),” The Annals of Statistics, vol. 2, no. 5, pp. pp. 849–879, 1974. 2,8,9,18 [16] R. Bajcsy, “Active perception,” Proceedings of the IEEE, vol. 76, no. 8, pp. 966–1005, 1988. 4 [17] V. Fedorov, Theory of Optimal Experiments (Probability and mathematical statistics). Academic Press Inc, 1972. 5,6 [18] L. L. Scharf and C. Demeure, Statistical Signal Processing. Detection, Estimation and Time series analysis. Addison–Wesley, 1991. 5 [19] K. Smith, “On the Standard Deviations of Adjusted and Interpolated Values of an Observed Polynomial Function and its Constants and the Guidance They Give Towards a Proper Choice of the Distribution of Observations,” Biometrika, vol. 12, no. 1, pp. 1–85, 1918. 5 [20] J. Kiefer, “Optimum Experimental Designs,” Journal of the Royal Statistical Society. Series B (Methodological), vol. 21, no. 2, pp. 272–319, 1959. 6 [21] A. Wald, “On the Efficient Design of Statistical Investigations,” The Annals of Mathematical Statistics, vol. 14, no. 2, pp. pp. 134–140, 1943. 6 [22] A. C. Atkinson and A. N. Donev, Optimum Experimental Designs (Oxford Statistical Science Series). Oxford University Press, USA, 1992. 6 [23] H. Chernoff, “Locally Optimal Designs for Estimating Parameters,” The Annals of Mathematical Statistics, vol. 24, no. 4, pp. pp. 586–602, 1953. 6 46 REFERENCES REFERENCES [24] S. Ehrenfeld, “On the Efficiency of Experimental Designs,” The Annals of Mathematical Statistics, vol. 26, no. 2, pp. pp. 247–255, 1955. 6 [25] T. M. Cover and J. A. Thomas, Elements of information theory. New York, NY, USA: Wiley-Interscience, 1991. 7 [26] C. E. Shannon, “A mathematical theory of communication,” The Bell system technical journal, vol. 27, pp. 379–423, Jul 1948. 7 [27] J. de Geeter, J. de Schutter, H. Bruyninckx, H. van Brussel, and M. Decreton, “Tolerance-weighted L-optimal experiment design for active sensing,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), vol. 3, Oct 1998, pp. 1670–1675. 8,12,18 [28] T. Vidal-Calleja, A. Sanfeliu, and J. Andrade-Cetto, “Action Selection for Single- Camera SLAM,” Systems, Man, and Cybernetics, Part B: Cybernetics, IEEE Transactions on, vol. 40, no. 6, pp. 1567 –1581, Dec 2010. 8 [29] J. Kurlbaum and U. Frese, “DLR Spatial Cognition Data Set,” no. [Online]. Avalible: http://www.informatik.uni-bremen.de/agebv/en/DlrSpatialCognitionDataSet., December 2008. 12 [30] E. Olson and M. Kaess, “Evaluating the performance of map optimization algorithms,” in RSS Workshop on Good Experimental Methodology in Robotics, June 2009. 15 [31] M. Kaess, A. Ranganathan, and F. Dellaert, “isam: Incremental smoothing and mapping,” Robotics, IEEE Transactions on, vol. 24, no. 6, pp. 1365 –1378, Dec 2008. 20 47