Pose Graph Optimization Research Articles

A Lightweight, Centralized, Collaborative, Truncated Signed Distance Function-Based Dense Simultaneous Localization and Mapping System for Multiple Mobile Vehicles.

Simultaneous Localization And Mapping (SLAM) algorithms play a critical role in autonomous exploration tasks requiring mobile robots to autonomously explore and gather information in unknown or hazardous environments where human access may be difficult or dangerous. However, due to the resource-constrained nature of mobile robots, they are hindered from performing long-term and large-scale tasks. In this paper, we propose an efficient multi-robot dense SLAM system that utilizes a centralized structure to alleviate the computational and memory burdens on the agents (i.e. mobile robots). To enable real-time dense mapping of the agent, we design a lightweight and accurate dense mapping method. On the server, to find correct loop closure inliers, we design a novel loop closure detection method based on both visual and dense geometric information. To correct the drifted poses of the agents, we integrate the dense geometric information along with the trajectory information into a multi-robot pose graph optimization problem. Experiments based on pre-recorded datasets have demonstrated our system's efficiency and accuracy. Real-world online deployment of our system on the mobile vehicles achieved a dense mapping update rate of ∼14 frames per second (fps), a onboard mapping RAM usage of ∼3.4%, and a bandwidth usage of ∼302 KB/s with a Jetson Xavier NX.

IMCB-PGO: Incremental Minimum Cycle Basis Construction and Application to Online Pose Graph Optimization

iMCB-PGO: Incremental Minimum Cycle Basis Construction and Application to Online Pose Graph Optimization

Laser-inertial tightly coupled SLAM system for indoor degraded environments

PurposeTo address the issues of low localization and mapping accuracy, as well as map ghosting and drift, in indoor degraded environments using light detection and ranging-simultaneous localization and mapping (LiDAR SLAM), a real-time localization and mapping system integrating filtering and graph optimization theory is proposed. By incorporating filtering algorithms, the system effectively reduces localization errors and environmental noise. In addition, leveraging graph optimization theory, it optimizes the poses and positions throughout the SLAM process, further enhancing map accuracy and consistency. The purpose of this study resolves common problems such as map ghosting and drift, thereby achieving more precise real-time localization and mapping results.Design/methodology/approachThe system consists of three main components: point cloud data preprocessing, tightly coupled inertial odometry based on filtering and backend pose graph optimization. First, point cloud data preprocessing uses the random sample consensus algorithm to segment the ground and extract ground model parameters, which are then used to construct ground constraint factors in backend optimization. Second, the frontend tightly coupled inertial odometry uses iterative error-state Kalman filtering, where the LiDAR odometry serves as observations and the inertial measurement unit preintegration results as predictions. By constructing a joint function, filtering fusion yields a more accurate LiDAR-inertial odometry. Finally, the backend incorporates graph optimization theory, introducing loop closure factors, ground constraint factors and odometry factors from frame-to-frame matching as constraints. This forms a factor graph that optimizes the map’s poses. The loop closure factor uses an improved scan-text-based loop closure detection algorithm for position recognition, reducing the rate of environmental misidentification.FindingsA SLAM system integrating filtering and graph optimization technique has been proposed, demonstrating improvements of 35.3%, 37.6% and 40.8% in localization and mapping accuracy compared to ALOAM, lightweight and ground optimized lidar odometry and mapping and LiDAR inertial odometry via smoothing and mapping, respectively. The system exhibits enhanced robustness in challenging environments.Originality/valueThis study introduces a frontend laser-inertial odometry tightly coupled filtering method and a backend graph optimization method improved by loop closure detection. This approach demonstrates superior robustness in indoor localization and mapping accuracy.

Graph-Based vs. Error State Kalman Filter-Based Fusion of 5G and Inertial Data for MAV Indoor Pose Estimation

5G New Radio Time of Arrival (ToA) data has the potential to revolutionize indoor localization for micro aerial vehicles (MAVs). However, its performance under varying network setups, especially when combined with IMU data for real-time localization, has not been fully explored so far. In this study, we develop an Error State Kalman Filter (ESKF) and a Pose Graph Optimization (PGO) approach to address this gap. We systematically evaluate the performance of the derived approaches for real-time MAV localization in realistic scenarios with 5G base stations in Line-Of-Sight (LOS), demonstrating the potential of 5G technologies in this domain. In order to experimentally test and compare our localization approaches, we augment the EuRoC MAV benchmark dataset for visual-inertial odometry with simulated yet highly realistic 5G ToA measurements. Our experimental results comprehensively assess the impact of varying network setups, including varying base station numbers and network configurations, on ToA-based MAV localization performance. The findings show promising results for seamless and robust localization using 5G ToA measurements, achieving an accuracy of 15 cm throughout the entire trajectory within a graph-based framework with five 5G base stations, and an accuracy of up to 34 cm in the case of ESKF-based localization. Additionally, we measure the run time of both algorithms and show that they are both fast enough for real-time implementation.

Distributed Pose-Graph Optimization With Multi-Level Partitioning for Multi-Robot SLAM

Distributed Pose-Graph Optimization With Multi-Level Partitioning for Multi-Robot SLAM

Highly robust and accurate multi-sensor fusion localization system for complex and challenging scenarios

Highly robust and accurate localization using on-board sensors in the absence of GNSS is a key issue for long-term autonomous navigation of vehicles. However, the limited sensing capabilities of a single sensor cannot resist the effects of feature degradation and high-speed motion in complex and challenging scenarios, which results in the fragile of existing LiDAR-based and visual-based localization algorithms. To solve these problems, we propose a highly robust and accurate localization system by fusing information from multi-sensors. Firstly, we propose a global localization method that leverages deep learning models for processing visual information to provide an accurate initial position for vehicles, which solves the problem of global localization of vehicles in the absence of GNSS. Then, we propose a highly robust and accurate LiDAR-Vision-IMU localization method based on pose graph optimization, which integrates LiDAR-based poses and visual-inertial-based poses. The proposed method detects sensor degradation in real time to dynamically adjust the fusion weights, which solves the problem of low localization performance of a single sensor. Finally, we conduct experiments in complex and challenging scenarios, such as urban, tunnels, and unstructured mountains. Experimental results show that our method can achieve a global localization error of less than 0.5 m in complex scenes. In unstructured mountainous scenarios of length 21.1 km, the root mean square errors (RMSE) of translation and rotation are 0.67 m and 0.048rad, respectively. Compared to most existing algorithms, our method achieves the most robust and accurate localization performance.

Three‐dimensionalized feature‐based LiDAR‐visual odometry for online mapping of unpaved road surfaces

AbstractAutomated maintenance and motion planning for unpaved roads are research areas of great interest in the field robotics. Constructing such systems necessitates the development of surface maps for unpaved roads. However, the lack of distinctive features on unpaved roads degrades the performance of light detection and ranging (LiDAR)‐based mapping. To address this problem, this paper proposes three‐dimensionalized feature‐based LiDAR‐visual odometry (TFB odometry) for the online mapping of unpaved road surfaces. TFB odometry introduces a novel interpolation concept to directly estimate the three‐dimensional coordinates of the image features using LiDAR. Furthermore, LiDAR intensity‐weighted motion estimation is proposed to effectively mitigate the effects of dust, which significantly impact the performance of LiDAR. Finally, TFB odometry includes pose graph optimization to efficiently fuse global navigation satellite system data and poses estimated from motion estimation. Through field experiments on unpaved roads, TFB odometry demonstrated successful online full mapping and outperformed other simultaneous localization and mapping methods. Additionally, it demonstrated remarkable performance in accurately mapping road surface anomalies, even in dusty regions.

SLAM-RAMU: 3D LiDAR-IMU lifelong SLAM with relocalization and autonomous map updating for accurate and reliable navigation

PurposeAutonomous mobile robots (AMRs) play a crucial role in industrial and service fields. The paper aims to build a LiDAR-based simultaneous localization and mapping (SLAM) system used by AMRs to overcome challenges in dynamic and changing environments.Design/methodology/approachThis research introduces SLAM-RAMU, a lifelong SLAM system that addresses these challenges by providing precise and consistent relocalization and autonomous map updating (RAMU). During the mapping process, local odometry is obtained using iterative error state Kalman filtering, while back-end loop detection and global pose graph optimization are used for accurate trajectory correction. In addition, a fast point cloud segmentation module is incorporated to robustly distinguish between floor, walls and roof in the environment. The segmented point clouds are then used to generate a 2.5D grid map, with particular emphasis on floor detection to filter the prior map and eliminate dynamic artifacts. In the positioning process, an initial pose alignment method is designed, which combines 2D branch-and-bound search with 3D iterative closest point registration. This method ensures high accuracy even in scenes with similar characteristics. Subsequently, scan-to-map registration is performed using the segmented point cloud on the prior map. The system also includes a map updating module that takes into account historical point cloud segmentation results. It selectively incorporates or excludes new point cloud data to ensure consistent reflection of the real environment in the map.FindingsThe performance of the SLAM-RAMU system was evaluated in real-world environments and compared against state-of-the-art (SOTA) methods. The results demonstrate that SLAM-RAMU achieves higher mapping quality and relocalization accuracy and exhibits robustness against dynamic obstacles and environmental changes.Originality/valueCompared to other SOTA methods in simulation and real environments, SLAM-RAMU showed higher mapping quality, faster initial aligning speed and higher repeated localization accuracy.

Robust pose graph optimization against outliers using consistency credibility factor

Robust pose graph optimization against outliers using consistency credibility factor

Majorization Minimization Methods for Distributed Pose Graph Optimization

Majorization Minimization Methods for Distributed Pose Graph Optimization

Time-Differenced Carrier Phase Assisted Visual-Inertial Odometry with Global Pose Graph Optimization

Time-Differenced Carrier Phase Assisted Visual-Inertial Odometry with Global Pose Graph Optimization

Robust Posegraph Optimization Using Proximity Points

This paper proposes a robust posegraph optimization (PGO) method for posegraphs with keypoints. In the conventional PGO formulation, a loop constraint is defined between a pair of nodes, whereas in the proposed method, it is defined between a pair of keypoints. In this manner, robust PGO based on switch variables can be realized in a more fine-grained manner. Loop constraint is defined based on the unique geometric property of proximity point, and implemented as a new edge type of the g2o solver. The proposed method is compared with other robust PGO methods using real world data recorded in Nakanoshima Robot Challenge 2021.

SEMANTIC ENRICHMENT OF 3D POINT CLOUDS USING 2D IMAGE SEGMENTATION

Abstract. 3D point cloud segmentation is computationally intensive due to the lack of inherent structural information and the unstructured nature of the point cloud data, which hinders the identification and connection of neighboring points. Understanding the structure of the point cloud data plays a crucial role in obtaining a meaningful and accurate representation of the underlying 3D environment. In this paper, we propose an algorithm that builds on existing state-of-the-art techniques of 2D image segmentation and point cloud registration to enrich point clouds with semantic information. DeepLab2 with ResNet50 as backbone architecture trained on the COCO dataset is used for indoor scene semantic segmentation into several classes like wall, floor, ceiling, doors, and windows. Semantic information from 2D images is propagated along with other input data, i.e., RGB images, depth images, and sensor information to generate 3D point clouds with semantic information. Iterative Closest Point (ICP) algorithm is used for the pair-wise registration of consecutive point clouds and finally, optimization is applied using the pose graph optimization on the whole set of point clouds to generate the combined point cloud of the whole scene. 3D point cloud of the whole scene contains pseudo-color information which denotes the semantic class to which each point belongs. The proposed methodology use an off-the-shelf 2D semantic segmentation deep learning model to semantically segment 3D point clouds collected using handheld mobile LiDAR sensor. We demonstrate a comparison of the accuracy achieved compared to a manually segmented point cloud on an in-house dataset as well as a 2D3DS benchmark dataset.

Localization and Motion Planning of Industrial Tractor–Trailers Vehicles

This brief presents real-time configuration estimation and motion planning for the industrial tractor–trailers vehicle composed of a full-scale car-like tractor and multiple full trailers. For the real-life vehicle, determining the configuration is challenging. With only on-tractor sensors, we solve this problem by fusing information from system dynamics propagation, geometrical constraints among the articulated units, and matching with the prebuilt environment map. The solution is efficiently achieved by formulating and solving a maximum a posterior (MAP) estimation problem in the pose-graph optimization framework. With the complicated tractor–trailers’ structure, small mismatch between actual and planned trajectories is crucial to inherit obstacle-free guarantee from planning to execution. We consider the dynamics and focus on facilitating the reduction of the mismatch by proposing a controller-based smoothing method to perform online motion planning. The given waypoint path is smoothed by forward propagation using a deliberately designed controller to generate the trajectory. The efficiently computed trajectories are also obstacle-free and dynamically feasible. The controller is also applied in execution to precisely reproduce the planned system evolvement. We demonstrate the performance with the real-life industrial tractor–trailers’ vehicle.

SPSVO: a self-supervised surgical perception stereo visual odometer for endoscopy

AbstractAccurate tracking and reconstruction of surgical scenes is a critical enabling technology toward autonomous robotic surgery. In endoscopic examinations, computer vision has provided assistance in many aspects, such as aiding in diagnosis or scene reconstruction. Estimation of camera motion and scene reconstruction from intra-abdominal images are challenging due to irregular illumination and weak texture of endoscopic images. Current surgical 3D perception algorithms for camera and object pose estimation rely on geometric information (e.g., points, lines, and surfaces) obtained from optical images. Unfortunately, standard hand-crafted local features for pose estimation usually do not perform well in laparoscopic environments. In this paper, a novel self-supervised Surgical Perception Stereo Visual Odometer (SPSVO) framework is proposed to accurately estimate endoscopic pose and better assist surgeons in locating and diagnosing lesions. The proposed SPSVO system combines a self-learning feature extraction method and a self-supervised matching procedure to overcome the adverse effects of irregular illumination in endoscopic images. The framework of the proposed SPSVO includes image pre-processing, feature extraction, stereo matching, feature tracking, keyframe selection, and pose graph optimization. The SPSVO can simultaneously associate the appearance of extracted feature points and textural information for fast and accurate feature tracking. A nonlinear pose graph optimization method is adopted to facilitate the backend process. The effectiveness of the proposed SPSVO framework is demonstrated on a public endoscopic dataset, with the obtained root mean square error of trajectory tracking reaching 0.278 to 0.690 mm. The computation speed of the proposed SPSVO system can reach 71ms per frame.

MuCoGraph: A multi-scale constraint enhanced pose-graph framework for MLS point cloud inconsistency correction

The position consistency of mobile laser scanning (MLS) point clouds is crucial for large-scale applications, and is normally guaranteed by the global navigation satellite system (GNSS) and high-precision inertial measurement unit (IMU) in the data acquisition. However, GNSS-denied environments such as city valleys result in significant position inconsistency for overlapping areas, and it is difficult to automatically locate these inconsistent areas. In this paper, to overcome these problems, we present MuCoGraph, which introduces multi-scale constraints to establish the correct correspondences for revisited areas, and formulates an enhanced pose graph for position inconsistency correction. The georeferenced MLS point cloud is first sliced into segments adaptively, and these segments are then abstracted as graph vertices, which satisfy local geometric consistency and rigid transformation hypotheses. Accurate revisited graph edges are then constructed hierarchically under multi-scale scenery consistency constraints. These revisited edges are initialized based on feature-based correspondence estimation and further unreliable edge pruning. Finally, through combination with virtual co-observations, correspondence-enhanced pose-graph optimization is introduced to globally redistribute the errors and obtain a high-precision point cloud. The proposed method was used to correct the MLS point cloud position inconsistency in three datasets. The average three-dimensional distance of the checkpoints was reduced from 0.362 m, 0.108 m, and 1.027 m to 0.057 m, 0.033 m, and 0.051 m for datasets I, II, and III respectively. In addition, the root-mean-square error of all three datasets was less than 0.04 m after correction. The experiments confirmed that the proposed method can automatically locate and correct the position inconsistency of MLS point clouds, showing good robustness and effectiveness.

ODLC_SAM: a novel LiDAR SLAM system towards open-air environments with loop closure

PurposeThe light detection and ranging sensor has been widely deployed in the area of simultaneous localization and mapping (SLAM) for its remarkable accuracy, but obvious drift phenomenon and large accumulated error are inevitable when using SLAM. The purpose of this study is to alleviate the accumulated error and drift phenomenon in the process of mapping.Design/methodology/approachA novel light detection and ranging SLAM system is introduced based on Normal Distributions Transform and dynamic Scan Context with switch. The pose-graph optimization is used as back-end optimization module. The loop closure detection is only operated in the scenario, while the path satisfies conditions of loop-closed.FindingsThe proposed algorithm exhibits competitiveness compared with current approaches in terms of the accumulated error and drift distance. Further, supplementary to the place recognition process that is usually performed for loop detection, the authors introduce a novel dynamic constraint that takes into account the change in the direction of the robot throughout the total path trajectory between corresponding frames, which contributes to avoiding potential misidentifications and improving the efficiency.Originality/valueThe proposed system is based on Normal Distributions Transform and dynamic Scan Context with switch. The pose-graph optimization is used as back-end optimization module. The loop closure detection is only operated in the scenario, while the path satisfies condition of loop-closed.

Road-Network-Map-Assisted Vehicle Positioning Based on Pose Graph Optimization.

Satellite signals are easily lost in urban areas, which causes difficulty in vehicles being located with high precision. Visual odometry has been increasingly applied in navigation systems to solve this problem. However, visual odometry relies on dead-reckoning technology, where a slight positioning error can accumulate over time, resulting in a catastrophic positioning error. Thus, this paper proposes a road-network-map-assisted vehicle positioning method based on the theory of pose graph optimization. This method takes the dead-reckoning result of visual odometry as the input and introduces constraints from the point-line form road network map to suppress the accumulated error and improve vehicle positioning accuracy. We design an optimization and prediction model, and the original trajectory of visual odometry is optimized to obtain the corrected trajectory by introducing constraints from map correction points. The vehicle positioning result at the next moment is predicted based on the latest output of the visual odometry and corrected trajectory. The experiments carried out on the KITTI and campus datasets demonstrate the superiority of the proposed method, which can provide stable and accurate vehicle position estimation in real-time, and has higher positioning accuracy than similar map-assisted methods.

Versatile Multi-LiDAR Accurate Self-Calibration System Based on Pose Graph Optimization

Versatile Multi-LiDAR Accurate Self-Calibration System Based on Pose Graph Optimization

Covariance Estimation for Pose Graph Optimization in Visual-Inertial Navigation Systems

Pose graph optimization helps reduce drift accumulated in pure odometry of visual simultaneous localization and mapping (SLAM) systems by solving a nonlinear least square problem, including both sequential constraints and loop-closing constraints. However, the covariances of all constraints are set to constant matrices or by manual setting. In this paper, we propose a novel approach to approximate covariances of constraints in pose graph optimization to better represent the true uncertainty of the underlying visual-inertial navigation system (VINS) that fuses inertial measurements and visual observations. Specifically, for sequential constraints, we propose to utilize nonlinear factor recovery to optimally extract covariance matrices from the accumulated visual-inertial odometry (VIO). For loop-closing constraints, we propose a dynamic scale estimation method to approximate the scales of the information matrices. To evaluate the effectiveness and robustness of the proposed method, we conduct extensive experiments on public and self-collected datasets in various environments. Results show that our proposed method achieves higher accuracy compared with naively-formulated pose graph optimization adopted by several state-of-the-art visual-inertial navigation systems.