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 Simultaneous Localization And Mapping (SLAM) problems, high-level landmarks have the potential to build compact and informative maps compared to traditional point-based landmarks. In this work, we focus on the param-eterization of frequently used geometric primitives including points, lines, planes, ellipsoids, cylinders, and cones. We first present a unified representation based on quadrics, an algebraic representation of quadratic surfaces in 3D. Then we propose a decomposed model of quadrics that discloses the symmetry and degeneration properties of a primitive. Based on the decomposition, we develop geometrically meaningful quadrics factors for the graph-SLAM problem. Then in simulation, it is shown that the decomposed formulation has better efficiency and robustness to observation noises than baseline parame-terizations. Finally, in real-world experiments, the proposed back-end framework is demonstrated to be capable of building compact and regularized maps.

Stereo reconstruction models trained on small images do not generalize well to high-resolution data. Training a model on high-resolution image size faces difficulties of data availability and is often infeasible due to limited computing resources. In this work, we present the Occlusion-aware Recurrent binocular Stereo matching (ORStereo), which deals with these issues by only training on available low disparity range stereo images. ORStereo generalizes to unseen high-resolution images with large disparity ranges by formulating the task as residual updates and refinements of an initial prediction. ORStereo is trained on images with disparity ranges limited to 256 pixels, yet it can operate 4K-resolution input with over 1000 disparities using limited GPU memory. We test the model’s capability on both synthetic and real-world high-resolution images. Experimental results demonstrate that ORStereo achieves comparable performance on 4K-resolution images compared to state-of-the-art methods trained on large disparity ranges. Compared to the baseline methods that are only trained on low-resolution images, our method has 60% or less error on 4K-resolution images.

This work presents dense stereo reconstruction using high-resolution images for infrastructure inspections. The state-of-the-art stereo reconstruction methods, both learning and non-learning ones, consume too much computational resource on high-resolution data. Recent learning-based methods achieve top ranks on most benchmarks. However, they suffer from the generalization issue due to lack of task-specific training data. We propose to use a less resource demanding non-learning method, guided by a learning-based model, to handle high-resolution images and achieve accurate stereo reconstruction. The deep-learning model produces an initial disparity prediction with uncertainty for each pixel of the down-sampled stereo image pair. The uncertainty serves as a self-measurement of its generalization ability and the perpixel searching range around the initially predicted disparity. The downstream process performs a modified version of the Semi-Global Block Matching method with the up-sampled perpixel searching range. The proposed deep-learning assisted method is evaluated on the Middlebury dataset and high-resolution stereo images collected by our customized binocular stereo camera. The combination of learning and non-learning methods achieves better performance on 12 out of 15 cases of the Middlebury dataset. In our infrastructure inspection experiments, the average 3D reconstruction error is less than 0.004m.

Although Structure-from-Motion (SfM) as a maturing technique has been widely used in many applications, state-of-the-art SfM algorithms are still not robust enough in certain situations. For example, images for inspection purposes are often taken in close distance to obtain detailed textures, which will result in less overlap between images and thus decrease the accuracy of estimated motion. In this paper, we propose a LiDAR-enhanced SfM pipeline that jointly processes data from a rotating LiDAR and a stereo camera pair to estimate sensor motions. We show that incorporating LiDAR helps to effectively reject falsely matched images and significantly improve the model consistency in large-scale environments. Experiments are conducted in different environments to test the performance of the proposed pipeline and comparison results with the state-of-the-art SfM algorithms are reported.

Light-weight camera localization in existing maps is essential for vision-based navigation. Currently, visual and visual-inertial odometry (VO&VIO) techniques are well-developed for state estimation but with inevitable accumulated drifts and pose jumps upon loop closure. To overcome these problems, we propose an efficient monocular camera localization method in prior LiDAR maps using direct 2D-3D line correspondences. To handle the appearance differences and modality gaps between LiDAR point clouds and images, geometric 3D lines are extracted offline from LiDAR maps while robust 2D lines are extracted online from video sequences. With the pose prediction from VIO, we can efficiently obtain coarse 2D-3D line correspondences. Then the camera poses and 2D-3D correspondences are iteratively optimized by minimizing the projection error of correspondences and rejecting outliers. Experimental results on the EurocMav dataset and our collected dataset demonstrate that the proposed method can efficiently estimate camera poses without accumulated drifts or pose jumps in structured environments.

The application of robots in inspection tasks has been growing quickly thanks to the advancements in autonomous navigation technology, especially the robot localization techniques in GPS-denied environments. Although many methods have been proposed to localize a robot using onboard sensors such as cameras and LiDARs, achieving robust localization in geometrically degenerated environments, e.g. tunnels, remains a challenging problem. In this work, we focus on the robust localization problem in such situations. A novel degeneration characterization model is presented to estimate the localizability at a given location in the prior map. And the localizability of a LiDAR and an Ultra-Wideband (UWB) ranging radio is analyzed. Additionally, a probabilistic sensor fusion method is developed to combine IMU, LiDAR and the UWB. Experiment results show that this method allows for robust localization inside a long straight tunnel.

This paper presents a robust localization approach that fuses measurements from inertial measurement unit (IMU) and a rotating laser scanner. An Error State Kalman Filter (ESKF) is used for sensor fusion and is combined with a Gaussian Particle Filter (GPF) for measurements update. We experimentally demonstrated the robustness of this implementation in various challenging situations such as kidnapped robot situation, laser range reduction and various environment scales and characteristics. Additionally, we propose a new method to evaluate localizability of a given 3D map and show that the computed localizability can precisely predict localization errors, thus helps to find safe routes during flight.

Limbless locomotion, evidenced by both biological and robotic snakes, capitalizes on these systems' redundant degrees of freedom to negotiate complicated environments. While the versatility of locomotion methods provided by a snake-like form is of great advantage, the difficulties in both representing the high dimensional workspace configuration and implementing the desired translations and orientations makes difficult further development of autonomous behaviors for snake robots. Based on a previously defined average body frame and set of motion primitives, this work proposes locomotive reduction, a simplifying methodology which reduces the complexity of controlling a redundant snake robot to that of navigating a differential-drive vehicle. We verify this technique by controlling a 16-DOF snake robot using locomotive reduction combined with a visual tracking system. The simplicity resulting from the proposed locomotive reduction method allows users to apply established autonomous navigation techniques previously developed for differential-drive cars to snake robots. Best of all, locomotive reduction preserves the advantages of a snake robot's ability to perform a variety of locomotion modes when facing complicated mobility challenges.

Successful deployment of a snake robot in search and rescue tasks requires the capability of generating controls which can adapt to unknown environments in real-time. However, available motion generation techniques can be computationally expensive and lack the ability to adapt to the surroundings. This work considers modeling the rolling motion of a snake robot by applying the Bellows model with computation reduction techniques. One benefit of this is that controllers are defined with physically meaningful parameters, which in turn allows for higher level control of the robot. Another benefit is that it allows controllers to be defined by “composing shapes”, which enables developing controllers that can adapt to the surroundings. Using shape composition, we implemented a novel gait, named rolling hump, which forms a contour-fitting hump to negotiate obstacles. The efficacy of a snake robot climbing over obstacles by using the rolling hump is experimentally evaluated. An autonomous control strategy is presented and realized in simulation.