In this paper, we address the problem of cooperative mapping (CM) using datasets collected by multiple users at different times, when the transformation between the users' starting poses is unknown. Specifically, we formulate CM as a constrained optimization problem, in which each user's independently estimated trajectory and map are merged together by imposing geometric constraints between commonly observed point and line features. Additionally, we provide an algorithm for efficiently solving the CM problem, by taking advantage of its structure. The proposed solution is proven to be batch-least-squares (BLS) optimal over all users' datasets, while it is less memory demanding and lends itself to parallel implementations. In particular, our solution is shown to be faster than the standard BLS solution, when the overlap between the users' data is small. Furthermore, our algorithm is resource-aware as it is able to consistently trade accuracy for lower processing cost, by retaining only an informative subset of the common-feature constraints. Experimental results based on visual and inertial measurements collected from multiple users within large buildings are used to assess the performance of the proposed CM algorithm.

In this paper, we seek to provide consistent, real-time 3D localization capabilities to mobile devices navigating within previously mapped areas. To this end, we introduce the Cholesky-Schmidt-Kalman filter (C-SKF), which explicitly considers the uncertainty of the prior map, by employing the sparse Cholesky factor of the map's Hessian, instead of its dense covariance-as is the case for the Schmidt-Kalman filter. By doing so, the C-SKF has memory requirements typically linear in the size of the map, as opposed to quadratic for storing the map's covariance. Moreover, and in order to bound the processing needs of the C-SKF (between linear and quadratic in the size of the map), we introduce two relaxations of the C-SKF algorithm: (i) The sC-SKF, which operates on the Cholesky factors of independent sub-maps resulting from dividing the map into overlapping segments. (ii) We formulate an efficient method for sparsifying the Cholesky factor by selecting and processing a subset of loop-closure measurements based on their temporal distribution. Lastly, we assess the processing and memory requirements of the proposed algorithms, and compare their positioning accuracy against other inconsistent map-based localization approaches that employ measurement-noise-covariance inflation to compensate for the map's uncertainty.

In this paper, a sliding-window two-camera vision-aided inertial navigation system (VINS) is presented in the square-root inverse domain. The performance of the system is assessed for the cases where feature matches across the two-camera images are processed with or without any stereo constraints (i.e., stereo vs. binocular). To support the comparison results, a theoretical analysis on the information gain when transitioning from binocular to stereo is also presented. Additionally, the advantage of using a two-camera (both stereo and binocular) system over a monocular VINS is assessed. Furthermore, the impact on the achieved accuracy of different image-processing frontends and estimator design choices is quantified. Finally, a thorough evaluation of the algorithm's processing requirements, which runs in real-time on a mobile processor, as well as its achieved accuracy as compared to alternative approaches is provided, for various scenes and motion profiles.

In this paper, we address the problem of cooperative mapping (CM) using datasets collected by multiple users at different times, when the transformation between the users' starting poses is unknown. Specifically, we formulate CM as a constrained optimization problem, where each user's independently estimated trajectory and map are combined in a single map by imposing geometric constraints between commonly-observed point and line features. Furthermore, our formulation allows for modularity since new/old maps (or parts of them) can be easily added/removed with no impact on the remaining ones. Additionally, the proposed CM algorithm lends itself, for the most part, to parallel implementations, hence gaining in speed. Experimental results based on visual and inertial measurements collected from four users within two large buildings are used to assess the performance of the proposed CM algorithm.

In this paper, we present C-KLAM, a Maximum A Posteriori (MAP) estimator-based keyframe approach for SLAM. Instead of discarding information from non-keyframes for reducing the computational complexity, the proposed C-KLAM presents a novel, elegant, and computationally-efficient technique for incorporating most of this information in a consistent manner, resulting in improved estimation accuracy. To achieve this, C-KLAM projects both proprioceptive and exteroceptive information from the non-keyframes to the keyframes, using marginalization, while maintaining the sparse structure of the associated information matrix, resulting in fast and efficient solutions. The performance of C-KLAM has been tested in experiments, using visual and inertial measurements, to demonstrate that it achieves performance comparable to that of the computationally-intensive batch MAP-based 3D SLAM, that uses all available measurement information.

We consider the problem of joint-state estimation for mobile wireless sensor networks (WSN) using noisy analog observations from spatially-distributed sensors. Due to communication bandwidth constraints, sensors can transmit only quantized observations. As opposed to existing estimators that process either only quantized or only analog observations, we develop a Maximum A Posteriori-based hybrid estimation framework that enables each sensor to utilize its own local analog observations as well as quantized observations received from other sensors to improve estimation accuracy.

In this paper, we present a Covariance Intersection (CI)-based algorithm for reducing the processing and communication complexity of multi-robot Cooperative Localization (CL). Specifically, for a team of N robots, our proposed approximate CI-based CL approach has processing and communication complexity only linear, O(N), in the number of robots. Moreover, and in contrast to alternative approximate methods, our approach is provably consistent, can handle asynchronous communication, and does not place any restriction on the robots' motion. We test the performance of our proposed approach in both simulations and experimentally, and show that it outperforms the existing linear-complexity split CI-based CL method.

This paper presents hybrid Minimum Mean Squared Error-based estimators for wireless sensor networks with time-varying communication-bandwidth constraints, focusing on the particular application of multi-robot Cooperative Localization. When sensor nodes (e.g., robots) communicate only a quantized version of their analog measurements to the team, our proposed hybrid filters enable robots to process all available information, i.e., local analog measurements (recorded by its own sensors) as well as remote quantized measurements (collected and communicated by other sensors). Moreover, these filters are resource-aware and can utilize additional bandwidth, whenever available, to maximize estimation accuracy. Specifically, in this paper, we present two filters, the Hybrid Batch-Quantized Kalman filter (H-BQKF) and the Hybrid Iteratively-Quantized Kalman filter (H-IQKF), that can process local analog measurements along with remote measurements quantized to any number of bits. We test our proposed filters in simulations and experimentally, and demonstrate that they achieve performance comparable to the standard Kalman filter.

In this paper, we consider the problem of multi-centralized Cooperative Localization (CL) under severe communication constraints, i.e., when each robot can communicate only a single bit per real-valued (analog) measurement. Existing approaches, such as those based on the Sign-of-Innovation Kalman filter (SOI-KF) and its variants, require each robot to process quantized versions of both its local (i.e., recorded by its own sensors) and remote (i.e., collected by other robots) measurements. This results in suboptimal performance since each robot has to discard information that is available in its own analog measurements. To address this limitation, we introduce a novel hybrid estimation scheme that enables each robot to process both quantized (from remote sensors) and analog (from its own sensors) measurements. Specifically, we first present the hybrid (H)-SOI-KF, a direct extension of the SOI-KF, for processing both types of measurements. Secondly, we introduce the modified (M)H-SOI-KF, that uses an asymmetric encoding/decoding scheme to incorporate additional information during quantization (based on the hybrid estimates locally available to each robot), resulting in substantial accuracy improvement. Lastly, we present extensive simulations which demonstrate that both hybrid estimators not only outperform the SOI-KF, but also achieve accuracy comparable to that of the standard (analog) centralized Kalman filter.

This paper presents a generalized framework for inter-robot information-transfer schemes in Multi-Centralized Cooperative Localization (MC-CL) under asynchronous communication, i.e., when the communication graph associated with the mobile robot network is time-varying and intermittently disconnected. Specifically, two information-transfer schemes, which differ based on their communication bandwidth requirements per link, are discussed. Even under asynchronous communication constraints, these schemes enable robots to compute pose estimates identical to those generated using the centralized CL framework, albeit delayed. For each of these schemes, necessary and sufficient conditions for the communication-graph connectivity, that enable each robot to generate the centralized estimates, are developed. Moreover, detailed description of these schemes, along with their communication-complexity analysis and analytical results for the expected time delay in obtaining these estimates, are presented. Lastly, simulation results are used to validate the performance (the trade-off between communication link bandwidth and accuracy/delay) of these information-transfer schemes.

This paper presents a distributed Maximum A Posteriori (MAP) estimator for multi-robot Cooperative Localization (CL). As opposed to centralized MAP-based CL, the proposed algorithm reduces the memory and processing requirements by distributing data and computations amongst the robots. Specifically, a distributed data-allocation scheme is presented that enables robots to simultaneously process and update their local data. Additionally, a distributed Conjugate Gradient algorithm is employed that reduces the cost of computing the MAP estimates, while utilizing all available resources in the team and increasing robustness to single-point failures. Finally, a computationally efficient distributed marginalization of past robot poses is introduced for limiting the size of the optimization problem. The communication and computational complexity of the proposed algorithm is described in detail, while extensive simulation studies are presented for validating the performance of the distributed MAP estimator and comparing its accuracy to that of existing approaches.

In this paper, we present an extended Kalman filter (EKF)-based estimator for simultaneous localization and mapping (SLAM) with processing requirements that are linear in the number of features in the map. The proposed algorithm is based on three key ideas. Firstly, by introducing the global-map postponement method, approximations necessary for ensuring linear computational complexity are delayed over many time steps. Then by employing the power method, only the most informative of the Kalman vectors, generated during the postponement phase, are retained for updating the covariance matrix. This in effect minimizes the information loss during each approximation epoch. Finally, linear-complexity, rank-2 updates, which minimize the trace of the covariance matrix, are applied to increase the speed of convergence of the estimator. In addition to being consistent, the resulting estimator has processing requirements that can be adjusted to the availability of computational resources. Simulation results are presented that demonstrate the accuracy of the proposed algorithm (Power-SLAM) when compared to the quadratic computational cost standard EKF-based SLAM, and two linear- complexity competing alternatives.