In this paper, we introduce a novel estimator for vision-aided inertial navigation systems (VINS), the Preconditioned Cholesky-based Square Root Information Filter (PC-SRIF). When solving linear systems, employing Cholesky decomposition offers superior efficiency but can compromise numerical stability. Due to this, existing VINS utilizing (Square Root) Information Filters often opt for QR decomposition on platforms where single precision is preferred, avoiding the numerical challenges associated with Cholesky decomposition. While these issues are often attributed to the ill-conditioned information matrix in VINS, our analysis reveals that this is not an inherent property of VINS but rather a consequence of specific parameterizations. We identify several factors that contribute to an ill-conditioned information matrix and propose a preconditioning technique to mitigate these conditioning issues. Building on this analysis, we present PC-SRIF, which exhibits remarkable stability in performing Cholesky decomposition in single precision when solving linear systems in VINS. Consequently, PC-SRIF achieves superior theoretical efficiency compared to alternative estimators. To validate the efficiency advantages and numerical stability of PC-SRIF based VINS, we have conducted well controlled experiments, which provide empirical evidence in support of our theoretical findings. Remarkably, in our VINS implementation, PC-SRIF’s runtime is 41% faster than QR-based SRIF.

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 present a vision-aided inertial navigation system (VINS) for localizing wheeled robots. In particular, we prove that VINS has additional unobservable directions, such as the scale, when deployed on a ground vehicle that is constrained to move along straight lines or circular arcs. To address this limitation, we extend VINS to incorporate low-frequency wheel-encoder data, and show that the scale becomes observable. Furthermore, and in order to improve the localization accuracy, we introduce the manifold-(m)VINS that exploits the fact that the vehicle moves on an approximately planar surface. In our experiments, we first show the performance degradation of VINS due to special motions, and then demonstrate that by utilizing the additional sources of information, our system achieves significantly higher positioning accuracy, while operating in real-time on a commercial-grade mobile device.

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 address the problem of extrinsically calibrating an inertial measurement unit (IMU) with respect to an RGBD sensor. In particular, we study the observability of the nonlinear IMU-RGBD calibration system and prove that the calibration parameters are observable given observations to a single point feature. Moreover, we show that the system has four unobservable directions corresponding to the global translation and rotations about the gravity vector. Based on the results of the observability analysis, we design a consistency-improved, observability constrained (OC) extended Kalman filter (EKF)-based estimator for calibrating the sensor pair while at the same time tracking its pose and creating a 3D map of the environment. Finally, we validate the key findings of the observability analysis and assess the performance of the OC-EKF estimator both in simulation and experimentally.

In this paper, we present an elegant solution to the 2D LIDAR-camera extrinsic calibration problem. Specifically, we develop a simple method for establishing correspondences between a line-scan (2D) LIDAR and a camera using a small calibration target that only contains a straight line. Moreover, we formulate the nonlinear least-squares problem for finding the unknown 6 degree-of-freedom (dof) transformation between the two sensors, and solve it analytically to determine its global minimum. Additionally, we examine the conditions under which the unknown transformation becomes unobservable, which can be used for avoiding ill-conditioned configurations. Finally, we present extensive simulation and experimental results for assessing the performance of the proposed algorithm as compared to alternative analytical approaches.

In this paper, we present a linear-complexity 3D inertial navigation algorithm using both point and plane features observed from an RGBD camera. In particular, we study the system's observability properties, and prove that: (i) When observing a single plane feature of known direction, the IMU gyroscope bias is observable. (ii) By observing a single point feature, as well as a single plane of known direction but not perpendicular to gravity, all degrees of freedom of the IMU-RGBD navigation system become observable, up to global translations. Next, based on the results of the observability analysis, we design a consistency-improved, observability-constrained (OC) extended Kalman filter (EKF)-based estimator for the IMU-RGBD camera navigation system. Finally, we experimentally validate the superiority of our proposed algorithm compared to alternative methods in urban scenes.

In this paper, we present an observability analysis of a vision-aided inertial navigation system (VINS) in which the camera is downward looking and observes a single point feature on the ground. In our analysis, the full INS parameter vector (including position, velocity, rotation, and inertial sensor biases) as well as the 3D position of the observed point feature are considered as state variables. In particular, we prove that the system has only three unobservable directions corresponding to global translations along the x and y axes, and rotations around the gravity vector. Hence, compared to general VINS, an advantage of using only ground features is that the vertical translation becomes observable. The findings of the theoretical analysis are validated through real-world experiments.

In order to fuse camera and odometer measurements, we first need to estimate their relative transformation through the so-called odometer-camera extrinsic calibration. In this paper, we present a two-step analytical least-squares solution for the extrinsic odometer-camera calibration that (i) is not iterative and finds the least-squares optimal solution without any initialization, and (ii) does not require any special hardware or the presence of known landmarks in the scene. Specifically, in the first step, we estimate a subset of the 3D relative rotation parameters by analytically minimizing a least-squares cost function. We then back-substitute these estimates in the measurement constraints, and determine the rest of the 3D transformation parameters by analytically minimizing a second least-squares cost function. Simulation and experimental results are presented that validate the efficiency and accuracy of the proposed algorithm.