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 study estimator inconsistency in vision-aided inertial navigation systems (VINS) from the standpoint of system's observability. We postulate that a leading cause of inconsistency is the gain of spurious information along unobservable directions, which results in smaller uncertainties, larger estimation errors, and divergence. We develop an observability constrained VINS (OC-VINS), which explicitly enforces the unobservable directions of the system, hence preventing spurious information gain and reducing inconsistency. This framework is applicable to several variants of the VINS problem such as visual simultaneous localization and mapping (V-SLAM), as well as visual-inertial odometry using the multi-state constraint Kalman filter (MSC-KF). Our analysis, along with the proposed method to reduce inconsistency, are extensively validated with simulation trials and real-world experimentation.

In this paper, we study the problem of Cooperative Localization (CL) for two robots, each equipped with an Inertial Measurement Unit (IMU) and a camera. We present an algorithm that enables the robots to exploit common features, observed over a sliding-window time horizon, in order to improve the localization accuracy of both vehicles. In contrast to existing CL methods, which require robot-to-robot distance and/or bearing measurements to resolve the robots' relative position and orientation (pose), our approach recovers the relative pose through indirect information from the commonly observed features. Moreover, we analyze the system observability properties to determine how many degrees of freedom (d.o.f.) of the relative transformation can be computed under different measurement scenarios. Lastly, we present simulation results to evaluate the performance of the proposed method.

In this work, we present a Direct Least-Squares (DLS) method for computing all solutions of the perspective-n-point camera pose determination (PnP) problem in the general case (n ≥ 3). Specifically, based on the camera measurement equations, we formulate a nonlinear least-squares cost function whose optimality conditions constitute a system of three third-order polynomials. Subsequently, we employ the multiplication matrix to determine all the roots of the system analytically, and hence all minima of the LS, without requiring iterations or an initial guess of the parameters. A key advantage of our method is scalability, since the order of the polynomial system that we solve is independent of the number of points. We compare the performance of our algorithm with the leading PnP approaches, both in simulation and experimentally, and demonstrate that DLS consistently achieves accuracy close to the Maximum-Likelihood Estimator (MLE).

Advances in lidar technology, in particular 360-degree lidar sensors, create new opportunities to augment and improve traditional surveillance systems. This paper describes an initial challenge to use a single stationary 360-degree lidar sensor to detect and track people moving throughout a scene in real-time. The depicted approach focuses on overcoming three primary challenges inherent in any lidar tracker: classification and matching errors between multiple human targets, segmentation errors between humans and fixed objects in the scene, and segmentation errors between targets that are very close together.

This paper presents a novel 3D indoor Laser-aided Inertial Navigation System (L-INS) for the visually impaired. An Extended Kalman Filter (EKF) fuses information from an Inertial Measurement Unit (IMU) and a 2D laser scanner, to concurrently estimate the six degree-of-freedom (d.o.f.) position and orientation (pose) of the person and a 3D map of the environment. The IMU measurements are integrated to obtain pose estimates, which are subsequently corrected using line-to-plane correspondences between linear segments in the laser-scan data and orthogonal structural planes of the building. Exploiting the orthogonal building planes ensures fast and efficient initialization and estimation of the map features while providing human-interpretable layout of the environment. The L-INS is experimentally validated by a person traversing a multistory building, and the results demonstrate the reliability and accuracy of the proposed method for indoor localization and mapping.

This paper presents a strategy for descending-stair detection, approach, and traversal using inertial sensing and a monocular camera mounted on an autonomous tracked vehicle. At the core of our algorithm are vision modules that exploit texture energy, optical flow, and scene geometry (lines) in order to robustly detect descending stairwells during both far- and near-approaches. As the robot navigates down the stairs, it estimates its three-degrees-of-freedom (d.o.f.) attitude by fusing rotational velocity measurements from an on-board tri-axial gyroscope with line observations of the stair edges detected by its camera. We employ a centering controller, derived based on a linearized dynamical model of our system, in order to steer the robot along safe trajectories. A real-time implementation of the described algorithm was developed for an iRobot Packbot, and results from real-world experiments are presented.

This paper presents an indoor localization system for the visually impaired. The basis of our system is an Extended Kalman Filter (EKF) for six degree-of-freedom (d.o.f.) position and orientation (pose) estimation. The sensing platform consists of an Inertial Measurement Unit (IMU) and a 2D laser scanner. The IMU measurements are integrated to obtain pose estimates which are subsequently corrected using line-to-plane correspondences between linear segments in the laser-scan data and known 3D structural planes of the building. Furthermore, we utilize Lie derivatives to show that the system is observable when at least three planes are detected by the laser scanner. Experimental results are presented that demonstrate the reliability of the proposed method for accurate and realtime indoor localization.

This paper presents a method for estimating the six-degrees-of-freedom transformation between a camera and the body of the robot on which it is rigidly attached. The robot maneuvers in front of a planar mirror, allowing the camera to observe fiducial features on the robot from several vantage points. Exploiting these measurements, we form a maximum-likelihood estimate of the camera-to-body transformation, without assuming prior knowledge of the robot motion or of the mirror configuration. Additionally, we estimate the mirror configuration with respect to the camera for each image. We validate the accuracy and correctness of our method with simulations and real-world experiments.

This paper presents an indoor human localization system for the visually impaired. A prototype portable device has been implemented, consisting of a pedometer and a standard white cane, on which a laser range finder and a 3-axis gyroscope have been mounted. A novel pose estimation algorithm has been developed for robustly estimating the heading and position of a person navigating in a known building. The basis of our estimation scheme is a two-layered extended Kalman filter (EKF) for attitude and position estimation. The first layer maintains an attitude estimate of the white cane, which is subsequently provided to the second layer where a position estimate of the user is generated. Experimental results are presented that demonstrate the reliability of the proposed method for accurate, real-time human localization.