Online planning in Markov Decision Processes (MDPs) enables agents to make sequential decisions by simulating future trajectories from the current state, making it well-suited for large-scale or dynamic environments. Sample-based methods such as Sparse Sampling and Monte Carlo Tree Search (MCTS) are widely adopted for their ability to approximate optimal actions using a generative model. However, in practical settings, the generative model is often learned from limited data, introducing approximation errors that can degrade performance or lead to unsafe behaviors. To address these challenges, Robust MDPs (RMDPs) offer a principled framework for planning under model uncertainty, yet existing approaches are typically computationally intensive and not suited for real-time use. In this work, we introduce Robust Sparse Sampling (RSS), the first online planning algorithm for RMDPs with finite-sample theoretical performance guarantees. Unlike Sparse Sampling, which estimates the nominal value function, RSS computes a robust value function by leveraging the efficiency and theoretical properties of Sample Average Approximation (SAA), enabling tractable robust policy computation in online settings. RSS is applicable to infinite or continuous state spaces, and its sample and computational complexities are independent of the state space size. We provide theoretical performance guarantees and empirically show that RSS outperforms standard Sparse Sampling in environments with uncertain dynamics.

Decision-making under uncertainty is a critical aspect of many practical autonomous systems due to incomplete information. Partially Observable Markov Decision Processes (POMDPs) offer a mathematically principled framework for formulating decision-making problems under such conditions. However, finding an optimal solution for a POMDP is generally intractable. In recent years, there has been a significant progress of scaling approximate solvers from small to moderately sized problems, using online tree search solvers. Often, such approximate solvers are limited to probabilistic or asymptotic guarantees towards the optimal solution. In this paper, we derive a deterministic relationship for discrete POMDPs between an approximated and the optimal solution. We show that at any time, we can derive bounds that relate between the existing solution and the optimal one. We show that our derivations provide an avenue for a new set of algorithms and can be attached to existing algorithms that have a certain structure to provide them with deterministic guarantees with marginal computational overhead. In return, not only do we certify the solution quality, but we demonstrate that making a decision based on the deterministic guarantee may result in superior performance compared to the original algorithm without the deterministic certification.

Taking into account future risk is essential for an autonomously operating robot to find online not only the best but also a safe action to execute. In this paper, we build upon the recently introduced formulation of probabilistic belief-dependent constraints. In our methodology safety can be materialized with any general belief-dependent operator we call payoff. We present an anytime approach employing the Monte Carlo Tree Search (MCTS) method in continuous domains in terms of states, actions and observations and general-belief dependent reward and payoff operators. Unlike previous approaches, our method ensures safety anytime with respect to the currently expanded search tree without relying on the convergence of the search. We prove convergence in probability with an exponential rate of a version of our algorithms and study proposed techniques via extensive simulations. Even with a tiny number of tree queries, the best action found by our approach is much safer than the baseline. Moreover, our approach constantly yields better than the baseline action in terms of objective function. This is because we revise the values and statistics maintained in the search tree and remove from them the contribution of the pruned actions. We rigorously show that our cleaning routine is necessary. Without it, at the limit of convergence of MCTS, an infinite amount of sampled dangerous actions can be detrimental to the objective function.

Solving partially observable Markov decision processes (POMDPs) with high dimensional and continuous observations, such as camera images, is required for many real life robotics and planning problems. Recent researches suggested machine learned probabilistic models as observation models, but their use is currently too computationally expensive for online deployment. We deal with the question of what would be the implication of using simplified observation models for planning, while retaining formal guarantees on the quality of the solution. Our main contribution is a novel probabilistic bound based on a statistical total variation distance of the simplified model. We show that it bounds the theoretical POMDP value w.r.t. original model, from the empirical planned value with the simplified model, by generalizing recent results of particle-belief MDP concentration bounds. Our calculations can be separated into offline and online parts, and we arrive at formal guarantees without having to access the costly model at all during planning, which is also a novel result. Finally, we demonstrate in simulation how to integrate the bound into the routine of an existing continuous online POMDP solver.

Multi-robot belief space planning (MR-BSP) is essential for reliable and safe autonomy. While planning, each robot maintains a belief over the state of the environment and reasons how the belief would evolve in the future for different candidate actions. Yet, existing MR-BSP works have a common assumption that the beliefs of different robots are consistent at planning time. Such an assumption is often highly unrealistic, as it requires prohibitively extensive and frequent communication capabilities. In practice, each robot may have a different belief about the state of the environment. Crucially, when the beliefs of different robots are inconsistent, state-of-the-art MR-BSP approaches could result in a lack of coordination between the robots, and in general, could yield dangerous, unsafe and suboptimal decisions. In this paper, we tackle this crucial gap. We develop a novel decentralized algorithm that is guaranteed to find a consistent joint action. For a given robot, our algorithm reasons for action preferences about 1) its local information, 2) what it perceives about the reasoning of the other robot, and 3) what it perceives about the reasoning of itself perceived by the other robot. This algorithm finds a consistent joint action whenever these steps yield the same best joint action obtained by reasoning about action preferences; otherwise, it self-triggers communication between the robots. Experimental results show efficacy of our algorithm in comparison with two baseline algorithms.

We introduce an innovative method for incremental nonparametric probabilistic inference in high-dimensional state spaces. Our approach leverages slices from highdimensional surfaces to efficiently approximate posterior distributions of any shape. Unlike many existing graph-based methods, our slices perspective eliminates the need for additional intermediate reconstructions, maintaining a more accurate representation of posterior distributions. Additionally, we propose a novel heuristic to balance between accuracy and efficiency, enabling real-time operation in nonparametric scenarios. In empirical evaluations on synthetic and real-world datasets, our slices approach consistently outperforms other state-of-the-art methods. It demonstrates superior accuracy and achieves a significant reduction in computational complexity, often by an order of magnitude.

Decision making under uncertainty is at the heart of any autonomous system acting with imperfect information. The cost of solving the decision-making problem is exponential in the action and observation spaces, thus rendering it unfeasible for many online systems. This article introduces a novel approach to efficient decision making, by partitioning the high-dimensional observation space. Using the partitioned observation space, we formulate analytical bounds on the expected information-theoretic reward, for general belief distributions. These bounds are then used to plan efficiently while maintaining performance guarantees. We show that the bounds are adaptive and computationally efficient, and that they converge to the original solution. We extend the partitioning paradigm and present a hierarchy of partitioned spaces that allows greater efficiency in planning. We then propose a specific variant of these bounds for Gaussian beliefs and show a theoretical performance improvement of at least a factor of 4. Finally, we compare our novel method to other state-of-the-art algorithms in active simultaneous localization and mapping scenarios, in simulation and in real experiments. In both cases, we show a significant speedup in planning with performance guarantees.

Online decision making under uncertainty in partially observable domains, also known as Belief Space Planning, is a fundamental problem in Robotics and Artificial Intelligence. Due to an abundance of plausible future unravelings, calculating an optimal course of action inflicts an enormous computational burden on the agent. Moreover, in many scenarios, e.g., Information gathering, it is required to introduce a belief-dependent constraint. Prompted by this demand, in this article, we consider a recently introduced probabilistic belief-dependent constrained partially observable Markov decision process (POMDP). We present a technique to adaptively accept or discard a candidate action sequence with respect to a probabilistic belief-dependent constraint, before expanding a complete set of sampled future observations episodes and without any loss in accuracy. Moreover, using our proposed framework, we contribute an adaptive method to find a maximal feasible return (e.g., Information Gain) in terms of Value at Risk and a corresponding action sequence, given a set of candidate action sequences, with substantial acceleration. On top of that, we introduce anadaptive simplificationtechnique for a probabilistically constrained setting. Such an approach provably returns an identical-quality solution while dramatically accelerating the online decision making. Our universal framework applies to any belief-dependent constrained continuous POMDP with parameteric beliefs, as well as nonparameteric beliefs represented by particles. In the context of an information-theoretic constraint, our presented framework stochastically quantifies if a cumulative Information Gain along the planning horizon is sufficiently significant (for e.g., Information Gathering, active simultaneous localization and mapping (SLAM)). As a case study, we apply our method to two challenging problems of high dimensional belief space planning: active SLAM and sensor deployment. Extensive realistic simulations corroborate the superiority of our proposed ideas.

It is a long-standing objective to ease the computation burden incurred by the decision-making problem under partial observability. Identifying the sensitivity to simplification of various components of the original problem has tremendous ramifications. Yet, algorithms for decision-making under uncertainty usually lean on approximations or heuristics without quantifying their effect. Therefore, challenging scenarios could severely impair the performance of such methods. In this paper, we extend the decision-making mechanism to the whole by removing standard approximations and considering all previously suppressed stochastic sources of variability. On top of this extension, we scrutinize the distribution of the return. We begin from a return given a single candidate policy and continue to the pair of returns given a corresponding pair of candidate policies. Furthermore, we present novel stochastic bounds on the return and novel tools, Probabilistic Loss (PLoss) and its online accessible counterpart (PbLoss), to characterize the effect of a simplification.

Autonomous agents operating in real-world scenarios frequently encounter uncertainty and make decisions based on incomplete information. Planning under uncertainty can be mathematically formalized using partially observable Markov decision processes (POMDPs). However, finding an optimal plan for POMDPs can be computationally expensive and is feasible only for small tasks. In recent years, approximate algorithms, such as tree search and sample-based methodologies, have emerged as state-of-the-art POMDP solvers for larger problems. Despite their effectiveness, these algorithms offer only probabilistic and often asymptotic guarantees toward the optimal solution due to their dependence on sampling. To address these limitations, we derive a deterministic relationship between a simplified solution that is easier to obtain and the theoretically optimal one. First, we derive bounds for selecting a subset of the observations to branch from while computing a complete belief at each posterior node. Then, since a complete belief update may be computationally demanding, we extend the bounds to support reduction of both the state and the observation spaces. We demonstrate how our guarantees can be integrated with existing state-of-the-art solvers that sample a subset of states and observations. As a result, the returned solution holds deterministic bounds relative to the optimal policy. Lastly, we substantiate our findings with supporting experimental results.

Active simultaneous localization and mapping (SLAM) is the problem of planning and controlling the motion of a robot to build the most accurate and complete model of the surrounding environment. Since the first foundational work in active perception appeared, more than three decades ago, this field has received increasing attention across different scientific communities. This has brought about many different approaches and formulations, and makes a review of the current trends necessary and extremely valuable for both new and experienced researchers. In this article, we survey the state of the art in active SLAM and take an in-depth look at the open challenges that still require attention to meet the needs of modern applications. After providing a historical perspective, we present a unified problem formulation and review the well-established modular solution scheme, which decouples the problem into three stages that identify, select, and execute potential navigation actions. We then analyze alternative approaches, including belief-space planning and deep reinforcement learning techniques, and review related work on multirobot coordination. This article concludes with a discussion of new research directions, addressing reproducible research, active spatial perception, and practical applications, among other topics.

Unresolved data association in ambiguous and perceptually aliased environments leads to multi-modal hypotheses on both the robot's and the environment state. To avoid catastrophic results, when operating in such ambiguous environments, it is crucial to reason about data association within Belief Space Planning (BSP). However, explicitly considering all possible data associations, the number of hypotheses grows exponentially with the planning horizon and determining the optimal action sequence quickly becomes intractable. Moreover, with hard budget constraints where some non-negligible hypotheses must be pruned, achieving performance guarantees is crucial. In this work we present a computationally efficient novel approach that utilizes only a distilled subset of hypotheses to solve BSP problems while reasoning about data association. Furthermore, to provide performance guarantees, we derive error bounds with respect to the optimal solution. We then demonstrate our approach in an extremely aliased environment, where we manage to significantly reduce computation time without compromising on the quality of the solution.

Reasoning about uncertainty is vital in many real-life autonomous systems. However, current state-of-the-art planning algorithms either cannot reason about uncertainty explicitly, or do so with high computational burden. Here, we focus on making informed decisions efficiently, using reward functions that explicitly deal with uncertainty. We formulate an approximation, namely an abstract observation model, that uses an aggregation scheme to alleviate computational costs. We derive bounds on the expected information-theoretic reward function and, as a consequence, on the value function. We then propose a method to refine aggregation to achieve identical action selection in a fraction of the computational time.

Semantic simultaneous localization and mapping is a subject of increasing interest in robotics and AI that directly influences the autonomous vehicles industry, the army industries, and more. One of the challenges in this field is to obtain object classification jointly with robot trajectory estimation. Considering view-dependent semantic measurements, there is a coupling between different classes, resulting in a combinatorial number of hypotheses. A common solution is to prune hypotheses that have a sufficiently low probability and to retain only a limited number of hypotheses. However, after pruning and renormalization, the updated probability is overconfident with respect to the original probability. This is especially problematic for systems that require high accuracy. If the prior probability of the classes is independent, the original normalization factor can be computed efficiently without pruning hypotheses. To the best of our knowledge, this is the first work to present these results. If the prior probability of the classes is dependent, we propose a lower bound on the normalization factor that ensures cautious results. The bound is calculated incrementally and with similar efficiency as in the independent case. After pruning and updating based on the bound, this belief is shown empirically to be close to the original belief.

In this paper, we consider online planning in par-tially observable domains. Solving the corresponding POMDP problem is a very challenging task, particularly in an online setting. Our key contribution is a novel algorithmic approach, Simplified Information Theoretic Belief Space Planning (SITH-BSP), which aims to speed up POMDP planning considering belief-dependent rewards, without compromising the solution's accuracy. We do so by mathematically relating the simplified el-ements of the problem to the corresponding counterparts of the original problem. Specifically, we focus on belief simplification and use it to formulate bounds on the corresponding original belief-dependent rewards. These bounds in turn are used to perform branch pruning over the belief tree, in the process of extracting the optimal policy from this existing belief tree. We further introduce the notion of adaptive simplification, while re-using calculations between different simplification levels, and exploit it to prune, at each level in the belief tree, all branches but one. Therefore, our approach is guaranteed to find the optimal solution (policy) that corresponds to the given belief tree but with substantial speedup. As a second key contribution, we derive novel analytical bounds for differential entropy, considering a sampling-based belief representation, which we believe are of interest on their own. We validate our approach in simulation using these bounds and where simplification corresponds to reducing the number of samples, exhibiting a significant computational speedup while yielding the optimal solution for the given belief tree.

Planning under uncertainty is a fundamental problem in robotics. Classical approaches rely on a metrical representation of the world and robot's states to infer the next course of action. While these approaches are considered accurate, they are often susceptible to metric errors and tend to be costly regarding memory and time consumption. However, in some cases, relying on qualitative geometric information alone is sufficient. Hence, the issues described above become an unnecessary burden. This work presents a novel qualitative Belief Space Planning (BSP) approach, highly suitable for platforms with low-cost sensors and particularly appealing in sparse environment scenarios. Our algorithm generalizes its predecessors by avoiding any deterministic assumptions. Moreover, it smoothly incorporates spatial information propagation techniques, known as compositions. We demonstrate our algorithm in simulations and the advantage of using compositions in particular.

Autonomous navigation missions require online decision making abilities, in order to choose from a given set of candidate actions an action that will lead to the best outcome. In a partially observable setting, decision making under uncertainty, also known as belief space planning (BSP), involves reasoning about belief evolution considering realizations of future observations. Yet, when candidate actions lead the robot to an unknown environment the decision making mission becomes a very challenging problem since without a map it is hard to foresee future observations. In this paper we develop a data-driven approach for predicting a distribution over an unexplored map, generating future observations, and combining these observations within BSP. We examine our approach and compare it to existing BSP methods in a Gazebo simulation, and demonstrate it often yields improved performance.

Simultaneous localization and mapping (SLAM) is essential in numerous robotics applications such as autonomous navigation. Traditional SLAM approaches infer the metric state of the robot along with a metric map of the environment. While existing algorithms exhibit good results, they are still sensitive to measurement noise, sensors quality, data association and are still computationally expensive. Alternatively, we note that some navigation and mapping missions can be achieved using only qualitative geometric information, an approach known as qualitative spatial reasoning (QSR). In this work we contribute a novel probabilistic qualitative localization and mapping approach, which extends the state of the art by inferring also the qualitative state of the camera poses (localization), as well as incorporating probabilistic connections between views (in time and in space). Our method is in particular appealing in scenarios with a small number of salient landmarks and sparse landmark tracks. We evaluate our approach in simulation and in a real-world dataset, and show its superior performance and low complexity compared to state of the art.

Belief space planning (BSP) is a fundamental problem in robotics. Determining an optimal action quickly grows intractable as it involves calculating the expected accumulated cost (reward), where the expectation accounts for all future measurement realizations. State of the art approaches therefore resort to simplifying assumptions and approximations to reduce computational complexity. Importantly, while in robotics re-planning is essential, these approaches calculate each planning session from scratch. In this work we contribute a novel approach, iX-BSP, that is based on the key insight that calculations in consecutive planning sessions are similar in nature and can be thus re-used. Our approach performs incremental calculation of the expectation by appropriately re-using computations already performed in a precursory planing session while accounting for the information obtained in inference between the two planning sessions. The formulation of our approach considers general distributions and accounts for data association aspects. We evaluate iX-BSP in statistical simulation and show that incremental expectation calculations significantly reduce runtime without impacting performance.

We present an approach for localization and semantic mapping in ambiguous scenarios by incrementally maintaining a hybrid belief over continuous states and discrete classification and data association variables. Unlike existing incremental approaches, we explicitly maintain data association components over time, which allows us to deal with perceptual aliasing. Crucially, we utilize a viewpoint-dependent classifier model over rich classifier outputs and leverage the coupling between poses and semantic measurements both for disambiguating data association and in pose estimation. We demonstrate in simulation that incorporating semantic measurements with a viewpoint-dependent classifier model enhances disambiguation of both data association and localization over usage of only geometric measurements or viewpoint independent models, further contributing to the tractability of the approach in practice, and providing better estimates.

We propose an algorithm for robust visual classification of an object of interest observed from multiple views using a black-box Bayesian classifier which provides a measure of uncertainty, in the presence of significant ambiguity and classifier noise, and of localization error. The fusion of classifier outputs takes into account viewpoint dependency and spatial correlation among observations, as well as pose uncertainty when these observations are taken and a measure of confidence provided by the classifier itself. Our experiments confirm an improvement in robustness over state-of-the-art.

In this paper we introduce a novel concept, topological belief space planning (BSP), that uses topological properties of the underlying factor graph representation of future posterior beliefs to direct the search for an optimal solution. This concept deviates from state-of-the-art BSP approaches and is motivated by recent results which indicated, in the context of graph pruning, that topological properties of factor graphs dominantly determine the estimation accuracy. Topological space is also often less dimensional than the embedded state space. In particular, we show how this novel concept can be used in multi-robot belief space planning in high-dimensional state spaces to overcome drawbacks of state-of-the-art approaches: computational intractability of an exhaustive objective evaluation for all candidate path combinations from different robots and dependence on the initial guess in the announced path approach, which can lead to a local minimum of the objective function. We demonstrate our approach in a synthetic simulation.

Typical inference approaches that work with high-dimensional visual measurements use hand-engineered image features (e.g, SIFT) that require combinatorial data association, or predict only hidden state mean without considering its uncertainty and multi-modality aspects. We develop a novel approach to infer system hidden state from visual observations via CNN features which are outputs of a CNN classifier. To that end, at pre-deployment stage we use neural networks to learn a generative viewpoint-dependent model of CNN features given the robot pose and approximate this model by a spatially-varying Gaussian distribution. Further, at deployment this model is utilized within a Bayesian framework for probabilistic inference, considering a robot localization problem. Our method does not involve data association and provides uncertainty covariance of the final estimation. Moreover, we show empirically that the CNN feature likelihood is unimodal which simplifies the inference task. We test our method in a simulated Unreal Engine environment, where we succeed to retrieve high-level state information from CNN features and produce trajectory estimation with high accuracy. Additionally, we analyze robustness of our approach to different light conditions.

In this paper we introduce a novel approach for efficient decision making under uncertainty and belief space planning, in high dimensional state spaces. While recently developed methods focus on sparsifying the inference process, the sparsification here is done in the context of efficient decision making, with no impact on the state inference. By identifying state variables which are uninvolved in the decision, we generate a sparse version of the state's information matrix, to be used in the examination of candidate actions. This sparse approximation is action-consistent, i.e. has no influence on the action selection. Overall we manage to maintain the same quality of solution, while reducing the computational complexity of the problem. The approach is put to the test in a SLAM simulation, where a significant improvement in runtime is achieved. Nevertheless, the method is generic, and not tied to a specific type of problem.

Inference and decision making under uncertainty are essential in numerous robotics problems. In recent years, the similarities between inference and control triggered much work, from developing unified computational frameworks to pondering about the duality between the two. In spite of the aforementioned efforts, inference and control, as well as inference and belief space planning (BSP) are still treated as two separate processes. In this paper we propose a novel approach that utilizes the similarities between inference and BSP and make the key observation that inference can be efficiently updated using the precursory planning stage, thus paving the way towards a joint inference and BSP paradigm. We develop four different methods that implement our novel approach under simplifying assumptions and validate them in the context of autonomous navigation in unknown environment. Results indicate that not only our methods improve running time by at least two orders of magnitude, compared to iSAM2 paradigm, they also found to be less sensitive to state dimensionality and loop closures.

One key assumption of Belief Space Planning (BSP) is that the data association is known perfectly. In this paper, we relax this assumption in the context of non-myopic planning as well as belief being a Gaussian Mixture Model (GMM). Interestingly, explicit reasoning about the data association within the belief enables our framework to have parsimonious data association, thereby resulting in a scalable solution compared with naïve permutational approaches. Unlike in some of the recent approaches where the number of components in a GMM belief can only be reduced, in our approach this can also go up such as due to perceptual aliasing present in the environment. Furthermore, our approach naturally integrates with inference, providing a unified framework for robust passive and active perception. We demonstrate key aspects of our approach and its comparison with the state of the art on a general abstract domain as well as in a real robot setup.

In this paper we introduce a novel sparsification method for efficient decision making under uncertainty and belief space planning in high dimensional state spaces. By using a sparse version of the state's information matrix, we are able to improve the high computational cost of examination of all candidate actions. We also present an in-depth analysis for the general case of approximated decision making, and use it in order to set bounds over the induced error in potential revenue. The scalability of the method allows balancing between the degree of sparsification and the tolerance for this error, in order to maximize its benefits. The approach differs from recent methods by focusing on improving the decision making process directly, and not as a byproduct of a sparsification of the state inference. Eventually, we demonstrate the superiority of the approach in a SLAM simulation, where we manage to maintain the accuracy of the solution, while demonstrating a significant improvement in run time.

High accuracy navigation in GPS-deprived environments is of prime importance to various robotics applications and has been extensively investigated in the last two decades. Recent approaches have shown that incorporating sensor's calibration states in addition to the 6DOF pose states may cause better performance of the system. However, these approaches typically consider a passive setting, where robot actions are externally defined. On the other hand, belief space planning (BSP) approaches account for different sources of uncertainty, thus identifying actions that improve certain aspects in inference, such as accuracy. Yet, existing BSP approaches typically do not consider sensor calibration, nor a visual-inertial SLAM setup. In this paper we contribute a BSP approach for active sensor calibration of a visual-inertial SLAM setup. For this purpose we incorporate within the belief both robot's pose and sensor calibration states while considering operation in partially unknown and uncertain environment. In particular, we leverage the recently developed concept of IMU pre-integration and develop appropriate factor graph formulation for future beliefs to facilitate computationally efficient inference within BSP. Our approach is valid for general cost functions, and can be used to identify best robot actions from a given set of candidate actions or to calculate locally-optimal actions using direct trajectory optimization techniques. We demonstrate our approach in high-fidelity synthetic simulation and show that incorporate sensors calibration state into the BSP significantly improved estimation accuracy.

We develop a novel approach for decision making under uncertainty in high-dimensional state spaces, considering both active unfocused and focused inference, where in the latter case reducing the uncertainty of only a subset of variables is of interest. State of the art approaches typically first calculate the posterior information (or covariance) matrix, followed by its determinant calculation, and do so separately for each candidate action. In contrast, using the generalized matrix determinant lemma, we avoid calculating these posteriors and determinants of large matrices. Furthermore, as our key contribution we introduce the concept of calculation re-use, performing a onetime computation that depends on state dimensionality and system sparsity, after which evaluating the impact of each candidate action no longer depends on state dimensionality. Such a concept is derived for both active focused and unfocused inference, leading to general, non-myopic and exact approaches that are faster by orders of magnitude compared to the state of the art. We verify our approach experimentally in two scenarios, sensor deployment (focused and unfocused) and measurement selection in visual SLAM, and show its superiority over standard techniques.

In this paper we develop a new approach for decentralized multi-robot belief space planning in high-dimensional state spaces while operating in unknown environments. State of the art approaches often address related problems within a sampling based motion planning paradigm, where robots generate candidate paths and are to choose the best paths according to a given objective function. As exhaustive evaluation of all candidate path combinations from different robots is computationally intractable, a commonly used (sub-optimal) framework is for each robot, at each time epoch, to evaluate its own candidate paths while only considering the best paths announced by other robots. Yet, even this approach can become computationally expensive, especially for high-dimensional state spaces and for numerous candidate paths that need to be evaluated. In particular, upon an update in the announced path from one of the robots, state of the art approaches re-evaluate belief evolution for all candidate paths and do so from scratch. In this work we develop a framework to identify and efficiently update only those paths that are actually impacted as a result of an update in the announced path. Our approach is based on appropriately propagating belief evolution along impacted paths while employing insights from factor graph and incremental smoothing for efficient inference that is required for evaluating the utility of each impacted path. We demonstrate our approach in a synthetic simulation.

In this paper, we present an information-based approach to select a reduced number of landmarks and poses for a robot to localize itself and simultaneously build an accurate map. We develop an information theoretic algorithm to efficiently reduce the number of landmarks and poses in a SLAM estimate without compromising the accuracy of the estimated trajectory. We also propose an incremental version of the reduction algorithm which can be used in SLAM framework resulting in information based reduced landmark SLAM. The results of reduced landmark based SLAM algorithm are shown on Victoria park dataset and a Synthetic dataset and are compared with standard graph SLAM (SAM [6]) algorithm. We demonstrate a reduction of 40-50% in the number of landmarks and around 55% in the number of poses with minimal estimation error as compared to standard SLAM algorithm.

We demonstrate distributed, online, and real-time cooperative localization and mapping between multiple robots operating throughout an unknown environment using indirect measurements. We present a novel Expectation Maximization (EM) based approach to efficiently identify inlier multi-robot loop closures by incorporating robot pose uncertainty, which significantly improves the trajectory accuracy over long-term navigation. An EM and hypothesis based method is used to determine a common reference frame. We detail a 2D laser scan correspondence method to form robust correspondences between laser scans shared amongst robots. The implementation is experimentally validated using teams of aerial vehicles, and analyzed to determine its accuracy, computational efficiency, scalability to many robots, and robustness to varying environments. We demonstrate through multiple experiments that our method can efficiently build maps of large indoor and outdoor environments in a distributed, online, and real-time setting.

In this paper we address the problem of collaborative active state estimation within the framework of multi-robot simultaneous localization and mapping (SLAM). We assume each robot has to autonomously navigate to a pre-specified set of goals in unknown environments and develop an approach that enables the robots to collaborate in order to reduce the uncertainty in their state estimation. We formulate this problem as multi-robot belief space planning, where the belief represents the probability distribution of robot states from the entire group, as well as the mapped environment thus far. Our approach is capable of guiding each robot to reduce its uncertainty by re-observing areas previously observed (only) by other robots. Direct observations between robot states, such as relative-pose measurements, are not required, providing enhanced flexibility for the group as the robots do not have to coordinate rendezvous with each other. Instead, our framework supports indirect constraints between the robots, that are induced by mutual observations of the same area possibly at different time instances, and accounts for these future multi-robot constraints within the planning phase. The proposed approach is evaluated in a simulation study.

Factor graphs are a general estimation framework that has been widely used in computer vision and robotics. In several classes of problems a natural partition arises among variables involved in the estimation. A subset of the variables are actually of interest for the user: we call those target variables. The remaining variables are essential for the formulation of the optimization problem underlying maximum a posteriori (MAP) estimation; however these variables, that we call support variables, are not strictly required as output of the estimation problem. In this paper, we propose a systematic way to abstract support variables, defining optimization problems that are only defined over the set of target variables. This abstraction naturally leads to the definition of smart factors, which correspond to constraints among target variables. We show that this perspective unifies the treatment of heterogeneous problems, ranging from structureless bundle adjustment to robust estimation in SLAM. Moreover, it enables to exploit the underlying structure of the optimization problem and the treatment of degenerate instances, enhancing both computational efficiency and robustness.

This work investigates the problem of planning under uncertainty, with application to mobile robotics. We propose a probabilistic framework in which the robot bases its decisions on the generalized belief, which is a probabilistic description of its own state and of external variables of interest. The approach naturally leads to a dual-layer architecture: an inner estimation layer, which performs inference to predict the outcome of possible decisions, and an outer decisional layer which is in charge of deciding the best action to undertake. The approach does not discretize the state or control space, and allows planning in continuous domain. Moreover, it allows to relax the assumption of maximum likelihood observations: predicted measurements are treated as random variables and are not considered as given. Experimental results show that our planning approach produces smooth trajectories while maintaining uncertainty within reasonable bounds.

This paper presents a novel approach for multirobot pose graph localization and data association without requiring prior knowledge about the initial relative poses of the robots. Without a common reference frame, the robots can only share observations of interesting parts of the environment, and trying to match between observations from different robots will result in many outlier correspondences. Our approach is based on the following key observation: while each multi-robot correspondence can be used in conjunction with the local robot estimated trajectories, to calculate the transformation between the robot reference frames, only the inlier correspondences will be similar to each other. Using this concept, we develop an expectation-maximization (EM) approach to efficiently infer the robot initial relative poses and solve the multi-robot data association problem. Once this transformation between the robot reference frames is estimated with sufficient measure of confidence, we show that a similar EM formulation can be used to solve also the full multi-robot pose graph problem with unknown multi-robot data association. We evaluate the performance of the developed approach both in a statistical synthetic-environment study and in a real-data experiment, demonstrating its robustness to high percentage of outliers.

This paper presents an consistent decentralized data fusion approach for robust multi-robot SLAM in dangerous, unknown environments. The DDF-SAM 2.0 approach extends our previous work by combining local and neighborhood information in a single, consistent augmented local map, without the overly conservative approach to avoiding information double-counting in the previous DDF-SAM algorithm. We introduce the anti-factor as a means to subtract information in graphical SLAM systems, and illustrate its use to both replace information in an incremental solver and to cancel out neighborhood information from shared summarized maps. This paper presents and compares three summarization techniques, with two exact approaches and an approximation. We evaluated the proposed system in a synthetic example and show the augmented local system and the associated summarization technique do not double-count information, while keeping performance tractable.

This paper presents a new computationally-efficient method for vision-aided navigation (VAN) in autonomous robotic applications. While many VAN approaches are capable of processing incoming visual observations, incorporating loop-closure measurements typically requires performing a bundle adjustment (BA) optimization, that involves both all the past navigation states and the observed 3D points. Our approach extends the incremental light bundle adjustment (LBA) method, recently developed for structure from motion [10], to information fusion in robotics navigation and in particular for including loop-closure information. Since in many robotic applications the prime focus is on navigation rather then mapping, and as opposed to traditional BA, we algebraically eliminate the observed 3D points and do not explicitly estimate them. Computational complexity is further improved by applying incremental inference. To maintain highrate performance over time, consecutive IMU measurements are summarized using a recently-developed technique and navigation states are added to the optimization only at camera rate. If required, the observed 3D points can be reconstructed at any time based on the optimized robot's poses. The proposed method is compared to BA both in terms of accuracy and computational complexity in a statistical simulation study.

Presented at the Ninth Conference on 23rd British Machine Vision Conference (BMVC 2012), 3-7 September 2012, Guildford, Surrey, UK.

This paper addresses the problem of distributed cooperative navigation. A new graph-based method is developed for on-demand calculation of the required correlation terms, considering a general multi-robot measurement model. These correlation terms are necessary for the consistent EKF-based data fusion when several statistically-dependent sources of information are used. The measurement model relates between the navigation information transmitted by any number of robots and the actual readings taken by the available onboard sensors. The transmitted information is not necessarily of the current time instant, but may actually belong to some time instant from the past. Experiment results and a theoretical example of the developed method are presented considering a three-view measurement, formulated upon receiving three images of the same scene, captured by different robots at different a priori unknown time instances.