Pose Graph Optimization Research Articles

Variational Inference With Parameter Learning Applied to Vehicle Trajectory Estimation

We present parameter learning in a Gaussian variational inference setting using only noisy measurements (i.e., no groundtruth). This is demonstrated in the context of vehicle trajectory estimation, although the method we propose is general. The paper extends the Exactly Sparse Gaussian Variational Inference (ESGVI) framework, which has previously been used for large-scale nonlinear batch state estimation. Our contribution is to additionally learn parameters of our system models (which may be difficult to choose in practice) within the ESGVI framework. In this paper, we learn the covariances for the motion and sensor models used within vehicle trajectory estimation. Specifically, we learn the parameters of a white-noise-on-acceleration motion model and the parameters of an Inverse-Wishart prior over measurement covariances for our sensor model. We demonstrate our technique using a 36~km dataset consisting of a car using lidar to localize against a high-definition map; we learn the parameters on a training section of the data and then show that we achieve high-quality state estimates on a test section, even in the presence of outliers. Lastly, we show that our framework can be used to solve pose graph optimization even with many false loop closures.

Cluster-based Penalty Scaling for Robust Pose Graph Optimization

Robust pose graph optimization is essential for reliable pose estimation in Simultaneous Localization and Mapping (SLAM) system. Due to the nature of loop closures, even one spurious measurement could trick the SLAM estimator and severely distort the mapping results. Existing methods to avoid this problem mostly focus on ensuring local measurement consistency by evaluating measurements independently, often requiring parameters that are difficult to tune. This letter proposes a cluster-based penalty scaling (CPS) method to ensure both the local and global consistency by first evaluating the edge quality locally, and then integrating this information into the optimization formulation.

Visual SLAM With Drift-Free Rotation Estimation in Manhattan World

This letter presents an efficient and accurate simultaneous localization and mapping (SLAM) system in man-made environments. The Manhattan world assumption is imposed, with which the global orientation is obtained. The drift-free rotational motion estimation is derived from the structural regularities using line features. In particular, a two-stage vanishing points (VPs) estimation method is developed, which consists of a short-term tracking module to track the clustered line features and a long-term searching module to generate abundant sets of VPs candidates and retrieve the optimal one. A least square problem is constructed and solved to provide refined VPs with the clusters of structural line features every frame. We make full use of the absolute orientation estimation to benefit the whole SLAM process. In particular, we utilize the absolute orientation estimation to increase the localization accuracy in the front end, and formulate a linear batch camera pose refinement problem with the known rotations to improve the real time performance in the back end. Experiments on both synthesized and real-world scenes reveal results with high-precision in the real time camera pose estimation process and high-speed in pose graph optimization process compared with the existing state-of-the-art methods.

Robot Localization in Water Pipes Using Acoustic Signals and Pose Graph Optimization.

One of the most fundamental tasks for robots inspecting water distribution pipes is localization, which allows for autonomous navigation, for faults to be communicated, and for interventions to be instigated. Pose-graph optimization using spatially varying information is used to enable localization within a feature-sparse length of pipe. We present a novel method for improving estimation of a robot’s trajectory using the measured acoustic field, which is applicable to other measurements such as magnetic field sensing. Experimental results show that the use of acoustic information in pose-graph optimization reduces errors by 39% compared to the use of typical pose-graph optimization using landmark features only. High location accuracy is essential to efficiently and effectively target investment to maximise the use of our aging pipe infrastructure.

Encoder-Camera-Ground Penetrating Radar Sensor Fusion: Bimodal Calibration and Subsurface Mapping

In this article, we report system and algorithmic developments for a sensing suite comprising a camera and a ground penetrating radar (GPR) with a wheel encoder designed for both surface and subsurface infrastructure inspection, which is a multimodal mapping task. To fuse different sensor modalities properly, we solve a novel GPR-camera calibration problem and a synchronization-challenged sensor fusion problem. First, we design a calibration rig, model the GPR imaging process, introduce a mirror to obtain the joint coverage between the camera and the GPR, and employ the maximum-likelihood estimator to estimate the relative pose between the camera and the GPR with error analysis. Second, we propose a data collection scheme using the customized artificial landmarks to synchronize camera images (temporally evenly spaced) and GPR/encoder data (spatially evenly spaced). We also employ pose graph optimization with location discrepancy as penalty functions to perform data fusion for 3-D reconstruction. We have tested our system in physical experiments. The results show that our system successfully fuses encoder-camera-GPR sensory data and accomplishes a metric 3-D reconstruction. Moreover, our sensor fusion approach reduces the end-to-end distance error from 6.4 to 0.7 cm in a real bridge inspection experiment if comparing to the counterpart that only uses encoder measurements.

Collision-free Autonomous Navigation of A Small UAV Using Low-cost Sensors in GPS-denied Environments

This paper proposes a novel complete navigation system for autonomous flight of small unmanned aerial vehicles (UAVs) in GPS-denied environments. The hardware platform used to test the proposed algorithm is a small, custom-built UAV platform equipped with an onboard computer, RGB-D camera, 2D light detection and ranging (LiDAR), and altimeter. The error-state Kalman filter (ESKF) based on the dynamic model for low-cost IMU-driven systems is proposed, and visual odometry from the RGB-D camera and height measurement from the altimeter are fed into the measurement update process of the ESKF. The pose output of the ESKF is then integrated into the open-source simultaneous location and mapping (SLAM) algorithm for pose-graph optimization and loop closing. In addition, the computationally efficient collision-free path planning algorithm is proposed and verified through simulations. The software modules run onboard in real time with limited onboard computational capability. The indoor flight experiment demonstrates that the proposed system for small UAVs with low-cost devices can navigate without collision in fully autonomous missions while establishing accurate surrounding maps.

Fast Georeferenced Aerial Image Stitching With Absolute Rotation Averaging and Planar- Restricted Pose Graph

Accurate digital orthophoto map generation from high-resolution aerial images is important in various applications. Compared with the existing commercial software and the current state-of-the-art mosaicing systems, a novel fast georeferenced orthophoto mosaicing framework is proposed in this study. The framework can adapt to the challenging requirements of high-accuracy orthoimage generations with relatively fast speed, even if the overlap rate is low. We provide appearance and spatial correlation-constrained fast low-overlap neighbor candidate query and matching. On the basis of GPS information, we introduce an absolute position and rotation-averaging strategy for global pose initialization, which is essential for the high convergence and efficiency of nonconvex pose optimization of every image. We also propose a planar-restricted global pose graph optimization method. The optimization is extremely efficient and robust considering that point clouds are parameterized to planes. Finally, we apply a matching graph-based exposure compensation and region reduction algorithm for large-scale and high-resolution image fusion with high efficiency and novel precision. Experimental results demonstrate that our method can achieve the state-of-the-art performance while maintaining high precision and robustness.

Asynchronous and Parallel Distributed Pose Graph Optimization

We present Asynchronous Stochastic Parallel Pose Graph Optimization (ASAPP), the first asynchronous algorithm for distributed pose graph optimization (PGO) in multi-robot simultaneous localization and mapping. By enabling robots to optimize their local trajectory estimates without synchronization, ASAPP offers resiliency against communication delays and alleviates the need to wait for stragglers in the network. Furthermore, ASAPP can be applied on the rank-restricted relaxations of PGO, a crucial class of non-convex Riemannian optimization problems that underlies recent breakthroughs on globally optimal PGO. Under bounded delay, we establish the global first-order convergence of ASAPP using a sufficiently small stepsize. The derived stepsize depends on the worst-case delay and inherent problem sparsity, and furthermore matches known result for synchronous algorithms when there is no delay. Numerical evaluations on simulated and real-world datasets demonstrate favorable performance compared to state-of-the-art synchronous approach, and show ASAPP's resilience against a wide range of delays in practice.

CKF-Based Visual Inertial Odometry for Long-Term Trajectory Operations

The estimation error accumulation in the conventional visual inertial odometry (VIO) generally forbids accurate long-term operations. Some advanced techniques such as global pose graph optimization and loop closure demand relatively high computation and processing time to execute the optimization procedure for the entire trajectory and may not be feasible to be implemented in a low-cost robotic platform. In an attempt to allow the VIO to operate for a longer duration without either using or generating a map, this paper develops iterated cubature Kalman filter for VIO application that performs multiple corrections on a single measurement to optimize the current filter state and covariance during the measurement update. The optimization process is terminated using the maximum likelihood estimate based criteria. For comparison, this paper also develops a second solution to integrate VIO estimation with ranging measurements. The wireless communications between the vehicle and multiple beacons produce the ranging measurements and help to bound the accumulative errors. Experiments utilize publicly available dataset for validation, and a rigorous comparison between the two solutions is presented to determine the application scenario of each solution.

Incremental 3-D pose graph optimization for SLAM algorithm without marginalization

Pose graph optimization algorithm is a classic nonconvex problem which is widely used in simultaneous localization and mapping algorithm. First, we investigate previous contributions and evaluate their performances using KITTI, Technische Universität München (TUM), and New College data sets. In practical scenario, pose graph optimization starts optimizing when loop closure happens. An estimated robot pose meets more than one loop closures; Schur complement is the common method to obtain sequential pose graph results. We put forward a new algorithm without managing complex Bayes factor graph and obtain more accurate pose graph result than state-of-art algorithms. In the proposed method, we transform the problem of estimating absolute poses to the problem of estimating relative poses. We name this incremental pose graph optimization algorithm as G-pose graph optimization algorithm. Another advantage of G-pose graph optimization algorithm is robust to outliers. We add loop closure metric to deal with outlier data. Previous experiments of pose graph optimization algorithm use simulated data, which do not conform to real world, to evaluate performances. We use KITTI, TUM, and New College data sets, which are obtained by real sensor in this study. Experimental results demonstrate that our proposed incremental pose graph algorithm model is stable and accurate in real-world scenario.

Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection

Semidefinite Programming (SDP) and Sums-of-Squares (SOS) relaxations have led to certifiably optimal non-minimal solvers for several robotics and computer vision problems. However, most non-minimal solvers rely on least-squares formulations, and, as a result, are brittle against outliers. While a standard approach to regain robustness against outliers is to use robust cost functions, the latter typically introduce other non-convexities, preventing the use of existing non-minimal solvers. In this paper, we enable the simultaneous use of non-minimal solvers and robust estimation by providing a general-purpose approach for robust global estimation, which can be applied to any problem where a non-minimal solver is available for the outlier-free case. To this end, we leverage the Black-Rangarajan duality between robust estimation and outlier processes (which has been traditionally applied to early vision problems), and show that graduated non-convexity (GNC) can be used in conjunction with non-minimal solvers to compute robust solutions, without requiring an initial guess. Although GNC's global optimality cannot be guaranteed, we demonstrate the empirical robustness of the resulting robust non-minimal solvers in applications, including point cloud and mesh registration, pose graph optimization, and image-based object pose estimation (also called shape alignment). Our solvers are robust to 70-80% of outliers, outperform RANSAC, are more accurate than specialized local solvers, and faster than specialized global solvers. We also propose the first certifiably optimal non-minimal solver for shape alignment using SOS relaxation.

Multimodal localization: Stereo over LiDAR map

AbstractIn this paper, we present a real‐time high‐precision visual localization system for an autonomous vehicle which employs only low‐cost stereo cameras to localize the vehicle with a priori map built using a more expensive 3D LiDAR sensor. To this end, we construct two different visual maps: a sparse feature visual map for visual odometry (VO) based motion tracking, and a semidense visual map for registration with the prior LiDAR map. To register two point clouds sourced from different modalities (i.e., cameras and LiDAR), we leverage probabilistic weighted normal distributions transformation (ProW‐NDT), by particularly taking into account the uncertainty of source point clouds. The registration results are then fused via pose graph optimization to correct the VO drift. Moreover, surfels extracted from the prior LiDAR map are used to refine the sparse 3D visual features that will further improve VO‐based motion estimation. The proposed system has been tested extensively in both simulated and real‐world experiments, showing that robust, high‐precision, real‐time localization can be achieved.

LCPF: A Particle Filter Lidar SLAM System With Loop Detection and Correction

A globally consistent map is the basis of indoor robot localization and navigation. However, map built by Rao-Blackwellized Particle Filter (RBPF) doesn’t have high global consistency which is not suitable for long-term application in large scene. To address the problem, we present an improved RBPF Lidar SLAM system with loop detection and correction named LCPF. The efficiency and accuracy of loop detection depend on the segmentation of submaps. Instead of dividing the submap at fixed number of laser scan like existing method, Dynamic Submap Segmentation is proposed in LCPF. This segmentation algorithm decreases the error inside the submap by splitting the submap where there is high scan match error and later rectifies the error by an improved pose graph optimization between submaps. In order to segment the submap at appropriate point, when to create a new submap is determined by both the accumulation of scan match error and the particle distribution. Furthermore, LCPF uses branch and bound algorithm as basic detector for loop detection and multiple criteria to judge the reliability of a loop. In the criteria, a novel parameter called usable ratio was proposed to measure the useful information that a laser scan containing. Finally, comparisons to existing 2D-Lidar mapping algorithm are performed with a series of open dataset simulations and real robot experiments to demonstrate the effectiveness of LCPF.

Learning Depth for Scene Reconstruction Using an Encoder-Decoder Model

Depth estimation has received considerable attention and is often applied to visual simultaneous localization and mapping (SLAM) for scene reconstruction. At least to our knowledge, sufficiently reliable depth always fails to be provided for monocular depth estimation-based SLAM because new image features are rarely re-exploited effectively, local features are easily lost, and relative depth relationships among depth pixels are readily ignored in previous depth estimation methods. Based on inaccurate monocular depth estimation, SLAM still faces scale ambiguity problems. To accurately achieve scene reconstruction based on monocular depth estimation, this paper makes three contributions. (1) We design a depth estimation model (DEM), consisting of a precise encoder to re-exploit new features and a decoder to learn local features effectively. (2) We propose a loss function using the depth relationship of pixels to guide the training of DEM. (3) We design a modular SLAM system containing DEM, feature detection, descriptor computation, feature matching, pose prediction, keyframe extraction, loop closure detection, and pose-graph optimization for pixel-level scene reconstruction. Extensive experiments demonstrate that the DEM and DEM-based SLAM are effective. (1) Our DEM predicts more reliable depth than the state of the arts when inputs are RGB images, sparse depth, or the fusion of both on public datasets. (2) The DEM-based SLAM system achieves comparable accuracy as compared with well-known modular SLAM systems.

Chordal Based Error Function for 3-D Pose-Graph Optimization

Pose-graph optimization (PGO) is a well-known problem in the robotics community. Optimizing a graph means finding the configuration of the nodes that best satisfies the edges. This is generally achieved using iterative approaches that refine a current solution until convergence. Nowadays, iterative least-squares (ILS) algorithms such as Gauss–Newton (GN) or Levenberg–Marquardt (LM) are dominant. Common to all these implementations is the influence of the error function used to measure the difference between prediction and observation. The smoother the error function is, the better the convergence properties of the system become, resulting in an increased convergence basin and more stable behavior. In this letter, we propose an alternative error function based on a variant of the Frobenious norm between transformation matrices. The proposed approach leads to a larger convergence basin and to numerical properties in the Jacobian computation that can potentially speedup the system. In contrast with some existing approximations, our formulation allows isotropic and anistropic noise covariances to be modeled. To validate our conjectures, we present an extensive comparative analysis between our approach and one of the most used error functions that computes the distance in the unit-quaternion space.

Parking Line Based SLAM Approach Using AVM/LiDAR Sensor Fusion for Rapid and Accurate Loop Closing and Parking Space Detection.

Parking is a challenging task for autonomous vehicles and requires a centimeter level precision of distance measurement for safe parking at a destination to avoid collisions with nearby vehicles. In order to avoid collisions with parked vehicles while parking, real-time localization performance should be maintained even when loop closing occurs. This study proposes a simultaneous localization and mapping (SLAM) method, using around view monitor (AVM)/light detection and ranging (LiDAR) sensor fusion, that provides rapid loop closing performance. We extract the parking line features by utilizing the sensor fusion data for sparse feature-based pose graph optimization that boosts the loop closing speed. Hence, the proposed method can perform the loop closing within a few milliseconds to compensate for the accumulative errors even in a large-scale outdoor environment, which is much faster than other LiDAR-based SLAM algorithms. Therefore, it easily satisfies real-time localization performance. Furthermore, thanks to the parking line features, the proposed method can detect a parking space by utilizing the accumulated parking lines in the map. The experiment was performed in three outdoor parking lots to validate the localization performance and parking space detection performance. All of the proposed methods can be operated in real-time in a single-CPU environment.

Image-Guided Registration of Unordered Terrestrial Laser Scanning Point Clouds for Urban Scenes

This paper presents an image-guided end-to-end registration approach for globally consistent 3-D registration of unordered terrestrial laser scanning (TLS) point clouds. The proposed method can handle arbitrary point clouds with reasonable pairwise overlap without knowledge about their initial position and orientation, without requiring artificial targets, and without needing to record the order of the scanning. One of the novel contributions of the proposed approach lies in the optimization of a scanning network. We retrieve the similarities of all scans based on a vocabulary tree using both the geometrically rectified panorama images and the corresponding 3-D point clouds. The approach also highlights the integral optimization in both the coarse and fine registration. A pose graph is introduced to realize global optimization at the end of the coarse step without primitives. After that, the results act as the inputs to start the pairwise fine registration, which is then followed by the minimum loop expansion (MLE) refinement. Comprehensive experiments demonstrated network optimization rates of over 60% using the image-guided strategy. Using the pose-graph optimization method, successful registration rates (SRRs) increased to 100% for all tested cases. The MLE not only accelerates the speed of the convergence but also improves registration accuracy, which reached 0.1 m and 0.1° in the translation and rotation angles, respectively.

Degeneracy-Aware Imaging Sonar Simultaneous Localization and Mapping

High-frequency imaging sonar sensors have recently been applied to aid underwater vehicle localization, by providing frame-to-frame odometry measurements or loop closures over large timescales. Previous methods have often assumed a planar environment, thereby restricting the use of such algorithms mostly to seafloor mapping. We propose an algorithm to generate pose-to-pose constraints for pairs of sonar images, which may also be applied to larger sets of images, that makes no assumptions about the environmental geometry. The algorithm is sensitive to the inherent degeneracies of the imaging sonar sensor model, and may be tuned to tradeoff between providing more constraints on the sensor motion and not overfitting to noise in the measurements. For real-time localization, we fuse the resulting pair-wise sonar pose constraints with vehicle odometry in a pose graph optimization framework. We rigorously evaluate the proposed method and demonstrate improvement in accuracy over previously proposed formulations both in simulation and real-world experiments.

Direct Relative Edge Optimization, A Robust Alternative for Pose Graph Optimization

Pose graph optimization is a common problem in robotics and associated fields. Most commonly, pose graph optimization is performed by finding the set of pose estimates which are the most likely for a given set of measurements. In some situations, arbitrarily large errors in pose graph initialization are unavoidable and can cause these pose-based methods to diverge or fail especially in cases where global inputs become available after some time after initialization. This letter details the parameterization of the classic pose graph problem in a relative context, optimizing directly over relative edge constraints between vertices in the pose graph and not on the poses themselves. Unlike previous literature on relative optimization, this letter details relative optimization over an entire pose graph, instead of a subset of edges, resulting in greater robustness to arbitrarily large errors than the classic pose-based or prior relative edge-based methods. Several small-scale simulation comparison studies, along with single and multi-agent hardware experiments, are presented. Results point to relative edge optimization as a strong candidate for solving real-world pose graph optimization problems that contain large heading propagation or initialization errors.

General Hand–Eye Calibration Based on Reprojection Error Minimization

This letter describes a novel hand–eye calibration technique based on reprojection error minimization. In contrast to traditional hand–eye calibration methods, the proposed method directly takes images of the calibration pattern and does not require to explicitly estimate the camera pose for each input image. The proposed method is implemented as a pose graph optimization problem, so that it can solve the estimation problem efficiently and robustly, and it can be easily extended for different projection models. It can deal with different camera models (e.g, X-ray cameras with a source-detector projection model) by changing the projection model. Through simulations, we validated that the proposed method shows a good estimation accuracy, and it can be applied to hand–eye calibration with a source-detector camera model. The experimental results with real robots show that the proposed method is applicable to real environments, and it improves the quality of a task that requires accurate hand–eye estimation, such as three-dimensional reconstruction.