Graph Optimization Algorithm Research Articles

An Adaptive Fast Incremental Smoothing Approach to INS/GPS/VO Factor Graph Inference

In response to asynchronous and delayed sensors within multi-sensor integrated navigation systems, the computational complexity of joint optimization navigation solutions persistently rises. This paper introduces an adaptive fast integrated navigation algorithm for INS/GPS/VO based on factor graph. The factor graph model for INS/GPS/VO is developed subsequent to individual modeling of the Inertial Navigation System (INS), Global Positioning System (GPS), and Visual Odometer (VO) using the factor graph model approach. Additionally, an Adaptive Fast Incremental Smoothing (AFIS) factor graph optimization algorithm is proposed. The simulation results demonstrate that the factor-graph-based integrated navigation algorithm consistently yields high-precision navigation outcomes even amidst dynamic changes in sensor validity and the presence of asynchronous and delayed sensor measurements. Notably, the AFIS factor graph optimization algorithm significantly enhances real-time performance compared to traditional Incremental Smoothing (IF) algorithms, while maintaining comparable real-time accuracy.

Resilient Factor Graph-Based GNSS/IMU/Vision/Odo Integrated Navigation Scheme Enhanced by Noise Approximate Gaussian Estimation in Challenging Environments

The signal blockage and multipath effects of the Global Navigation Satellite System (GNSS) caused by urban canyon scenarios have brought great technical challenges to the positioning and navigation of autonomous vehicles. In this paper, an improved factor graph optimization algorithm enhanced by a resilient noise model is proposed. The measurement noise is resilient and adjusted based on an approximate Gaussian distribution-based estimation. In estimating and adjusting the noise parameters of the measurement model, the error covariance matrix of the multi-sensor fusion positioning system is dynamically optimized to improve the system accuracy. Firstly, according to the approximate Gaussian statistical property of the GNSS/odometer velocity residual sequence, the measured data are divided into an approximate Gaussian fitting region and an approximate Gaussian convergence region. Secondly, the interval is divided according to the measured data, and the corresponding variational Bayesian network and Gaussian mixture model are used to estimate the innovation online. Further, the noise covariance matrix of the adaptive factor graph-based model is dynamically optimized using the estimated noise parameters. Finally, based on low-cost inertial navigation equipment, GNSS, odometer, and vision, the algorithm is implemented and verified using a simulation platform and real-vehicle road test. The experimental results show that in a complex urban road environment, compared with the traditional factor graph fusion localization algorithm, the maximum improvement in accuracy of the proposed algorithm can reach 65.63%, 39.52%, and 42.95% for heading, position, and velocity, respectively.

LiDAR-based SLAM pose estimation via GNSS graph optimization algorithm

LiDAR simultaneous localization and mapping (SLAM) is widely used in positioning and navigation. By illuminating a series of light spots on the surface of an object, orientation and pose information is obtained. However, improving the accuracy of the pose optimization algorithm without affecting the position information is difficult. Therefore, this study combines the graph optimization algorithm and the Global Navigation Satellite System (GNSS) to optimize the coordinates of the target object in the LiDAR SLAM pose. A GNSS pose estimation algorithm is proposed to show the relationship between positioning algorithms with and without GNSS pose optimization by analyzing the deviation of the distance, level, and height laser point cloud coordinates. The results show that with GNSS pose optimization, the deviations in distance, level, and height are 99% smaller than those without GNSS pose optimization. Furthermore, we demonstrate the effectiveness of the proposed graph optimization algorithm and GNSS optimization of LiDAR SLAM. Finally, this study highlights the directions for the application of wireless communication technology in the field of LiDAR SLAM.

Local Path Planner for Mobile Robot Considering Future Positions of Obstacles

Local path planning is a necessary ability for mobile robot navigation, but existing planners are not sufficiently effective at dynamic obstacle avoidance. In this article, an improved timed elastic band (TEB) planner based on the requirements of mobile robot navigation in dynamic environments is proposed. The dynamic obstacle velocities and TEB poses are fully integrated through two-dimensional (2D) lidar and multi-obstacle tracking. First, background point filtering and clustering are performed on the lidar points to obtain obstacle clusters. Then, we calculate the data association matrix of the obstacle clusters of the current and previous frame so that the clusters can be matched. Thirdly, a Kalman filter is adopted to track clusters and obtain the optimal estimates of their velocities. Finally, the TEB poses and obstacle velocities are associated: we predict the obstacle position corresponding to the TEB pose through the detected obstacle velocity and add this constraint to the corresponding TEB pose vertex. Then, a pose sequence considering the future positions of obstacles is obtained through a graph optimization algorithm. Compared with the original TEB, our method reduces the total running time by 22.87%, reduces the running distance by 19.23%, and increases the success rate by 21.05%. Simulations and experiments indicate that the improved TEB enables robots to efficiently avoid dynamic obstacles and reach the goal as quickly as possible.

Distribution of resources beyond 5G networks with heterogeneous parallel processing and graph optimization algorithms

In this paper, a design model for resource allocation is formulated beyond 5G networks for effective data allocations in each network nodes. In all networks, data is transmitted only after allocating all resources, and an unrestrained approach is established because the examination of resources is not carried out in the usual manner. However, if data transmission needs to occur, some essential resources can be added to the network. Moreover, these resources can be shared using a parallel optimization approach, as outlined in the projected model. Further the designed model is tested and verified with four case studies by using resource allocator toolbox with parallax where the resources for power and end users are limited within the ranges of 1.4% and 6%. Furthermore, in the other two case studies, which involve coefficient determination and blockage factors, the outcomes of the proposed approach fall within the marginal error constraint of approximately 31% and 87%, respectively.

LIDAR-INERTIAL NAVIGATION BASED ON MAP AIDED DISTANCE CONSTRAINT AND FACTOR GRAPH OPTIMIZATION

Abstract. The simultaneous localization and mapping (SLAM) is one of the well-developed positioning technology that provides high accuracy and reliability positioning for automatic vehicles and robotics applications. Integrating Light Detection and Ranging (LiDAR) with an Inertial Measurement Unit (IMU) has emerged as a promising technique for achieving stable navigation results in dense urban environments, outperforming vision-based or pure Inertial Navigation System (INS) solutions. However, conventional LiDAR-Inertial SLAM systems often suffer from limited perception of surrounding geometric information, resulting in unexpected and accumulating errors. In this paper, we proposed a LiDAR-Inertial SLAM scheme that utilizes a prior structural information map which is generated from opensource OpenStreetMap (OSM). In contrast to conventional solutions of OSM-aided SLAM approaches, our method extracts the vectorized models of road and building and synthetically generates dense point maps for LiDAR registration. Specifically, a structural map processing module extracts the road models and building models from OSM and generates a structure information map (SIM) with dense point clouds. Secondly, a map aided distance (MD) constraint is calculated by registering selected keyframes and the prior SIM. Finally, a factor graph optimization (FGO) algorithm is involved to integrate the relative transformation obtained from LiDAR odometry, IMU pre-integration, and the map aided distance constraints. To evaluate the proposed LiDAR-based positioning accuracy, experimental evaluation is implemented in an opensource dataset collected in the urban canyon environments. Experimental results demonstrates that with the help of the proposed MD constraint, the LiDAR-based navigation solution can achieve accurate positioning, with a root mean square deviation (RSME) of 4.7 m.

Real-Time 3D Reconstruction of UAV Acquisition System for the Urban Pipe Based on RTAB-Map

In urban underground projects, such as urban drainage systems, the real-time acquisition and generation of 3D models of pipes can provide an important foundation for pipe safety inspection and maintenance. The simultaneous localization and mapping (SLAM) technique, compared to the traditional structure from motion (SfM) reconstruction technique, offers high real-time performance and improves the efficiency of 3D object reconstruction. Underground pipes are situated in complex environments with unattended individuals and often lack natural lighting. To address this, this paper presents a real-time and cost-effective 3D perception and reconstruction system that utilizes an unmanned aerial vehicle (UAV) equipped with Intel RealSense D435 depth cameras and an artificial light-supplementation device. This system carries out real-time 3D reconstruction of underground pipes using the RTAB-Map (real-time appearance-based mapping) method. RTAB-Map is a graph-based visual SLAM method that combines closed-loop detection and graph optimization algorithms. The unique memory management mechanism of RTAB-Map enables synchronous mapping for multiple sessions during UAV flight. Experimental results demonstrate that the proposed system, based on RTAB-Map, exhibits the robustness, textures, and feasibility for 3D reconstruction of underground pipes.

A LiDAR-IMU-GNSS fused mapping method for large-scale and high-speed scenarios

In the field of autonomous navigation and surveying, constructing large-scale and globally consistent environmental maps in real-time is a critical problem. In this case, the algorithm faces many challenges, such as high-speed motion, GNSS-denied tunnels and open fields with few features. To solve the aforementioned problems, this paper proposes a mapping method that fuses LiDAR, IMU and GNSS information simultaneously. The proposed method mainly includes two parts: LiDAR-IMU joint optimization and LIO-GNSS joint optimization. In the LiDAR-IMU joint optimization, we realize a tightly-coupled LiDAR inertial odometry (LIO) and propose a new degradation processing mechanism that enables more robust mapping in degraded environments, such as tunnels and open fields. Furthermore, we propose a new constraint method based on ground points to solve the drift problem in the vertical direction. This method maintains a local map based on ground grids, providing more effective constraints for the vertical direction. In the LIO-GNSS joint optimization, we use the factor graph optimization(FGO) algorithm based on sliding window to realize the fusion of LIO and GNSS data, so as to achieve real-time, no drift and global consistency mapping. We conducted experiments in many complex environments with challenges such as high-speed motion, LiDAR degradation, GNSS-denied scenarios and so on. While most open-source algorithms struggled to produce satisfactory mapping results or even failed, our algorithm proved to be robust and achieved the best mapping results.

A graph optimization approach to range-based relative location

With the development of multi-robot cluster system, the research of multi-node relative pose estimation is in the ascendant. However, for the environment without GNSS or fixed anchors, the problem of relative pose estimation is still challenging. In this paper, a general algorithm and system framework based on graph optimization algorithm is proposed to fuse inter- and intra-robot measurements, as well as mobility models with sliding windows for relative attitude estimation. In order to verify the feasibility and universality of the algorithm, the algorithm has used to design a relative positioning system based on UWB positioning with a “single-node multi-tags” system structure, which can simultaneously obtain reliable relative positions and directions. The design of multi-source fusion can further improve the accuracy and robustness of relative positioning. Experiment results shows that in the harsh environment without GNSS and fixed anchor points, the relative pose estimation accuracy of this system can reach centi-meter level, which may be the best potential choice for relative positioning of multi robot cluster in complex environment.

P‐2.7: Slam Fusion Location Mapping Based on Lidar

Aiming at the low accuracy of GNSS/INS integrated navigation caused by the outdoor road environment without GNSS signal or poor GNSS signal, this thesis proposes to combine slam technology with on-board laser scanning system, which can still maintain good robustness in the weak GNSS area. By coupling slam and IMU through the factor graph optimization algorithm, it can improve the mapping results affected by the high speed in the on-board laser scanning system. this thesis uses the system calibration to improve the lio-sam algorithm.Based on the factor graph optimization algorithm, the multi line laser is added to the vehicle lidar scanning system to solve the long-distance and high-precision positioning and attitude determination in the case of weak GNSS signal. The feasibility and effectiveness are verified by the specific application in the field.

Tac2Structure: Object Surface Reconstruction Only Through Multi Times Touch

Inspired by humans' ability to perceive the surface texture of unfamiliar objects without relying on vision, the sense of touch can play a crucial role in robots exploring the environment, particularly in scenes where vision is difficult to apply, or occlusion is inevitable. Existing tactile surface reconstruction methods rely on external sensors or have strong prior assumptions, making the operation complex and limiting their application scenarios. This letter presents a framework for low-drift surface reconstruction through multiple tactile measurements, Tac2Structure. Compared with existing algorithms, the proposed method uses only a new vision-based tactile sensor without relying on external devices. Aiming at the difficulty that reconstruction accuracy is easily affected by the pressure at contact, we propose a correction algorithm to adapt it. The proposed method also reduces the accumulative errors that occur easily during global object surface reconstruction. Multi-frame tactile measurements can accurately reconstruct object surfaces by jointly using the point cloud registration algorithm, loop-closure detection algorithm based on deep learning, and pose graph optimization algorithm. Experiments verify that Tac2Structure can achieve millimeter-level accuracy in reconstructing the surface of objects, providing accurate tactile information for the robot to perceive the surrounding environment.

LiDAR-Based Sensor Fusion SLAM and Localization for Autonomous Driving Vehicles in Complex Scenarios

LiDAR-based simultaneous localization and mapping (SLAM) and online localization methods are widely used in autonomous driving, and are key parts of intelligent vehicles. However, current SLAM algorithms have limitations in map drift and localization algorithms based on a single sensor have poor adaptability to complex scenarios. A SLAM and online localization method based on multi-sensor fusion is proposed and integrated into a general framework in this paper. In the mapping process, constraints consisting of normal distributions transform (NDT) registration, loop closure detection and real time kinematic (RTK) global navigation satellite system (GNSS) position for the front-end and the pose graph optimization algorithm for the back-end, which are applied to achieve an optimized map without drift. In the localization process, the error state Kalman filter (ESKF) fuses LiDAR-based localization position and vehicle states to realize more robust and precise localization. The open-source KITTI dataset and field tests are used to test the proposed method. The method effectiveness shown in the test results achieves 5-10 cm mapping accuracy and 20-30 cm localization accuracy, and it realizes online autonomous driving in complex scenarios.

Graph optimization algorithm using symmetry and host bias for low-latency indirect network

It is known that an indirect network with a small host-to-host Average Shortest Path Length (h-ASPL) improves overall system performance in a parallel computer system. As a means to discuss such indirect networks in graph theory, the Order/Radix Problem (ORP) has been proposed. ORP involves finding a graph with a minimum h-ASPL that satisfies a given number of hosts and radix. A graph in ORP represents an indirect network and has two types of vertices: host and switch. We propose an optimization algorithm to generate graphs with a sufficiently small h-ASPL. The primary features of the proposed algorithm are the symmetry of the graph and the bias of the hosts adjacent to each switch. These features reduce the computational time to calculate the h-ASPL and improve the search performance of the algorithm. The performance of the proposed algorithm is evaluated using problems presented by Graph Golf, an international ORP competition. Our results show that the proposed algorithm can generate graphs with a smaller h-ASPL than the existing algorithm. To evaluate the performance of the graphs generated by the proposed algorithm, we use the parallel simulation framework SimGrid and the parallel benchmark collection NAS Parallel Benchmarks. Our results also show that the graphs generated by the proposed algorithm have higher performance than those generated by the existing algorithm.

An Aerial and Ground Multi-Agent Cooperative Location Framework in GNSS-Challenged Environments

In order to realize the cooperative localization of multi-unmanned platforms in the GNSS-denied environment, this paper proposes a collaborative SLAM (simultaneous localization and mapping, SLAM) framework based on image feature point matching. Without GNSS, a single unmanned platform UGV and UAV (unmanned ground vehicle, UGV; unmanned aerial vehicle, UAV) equipped with vision and IMU (inertial measurement unit, IMU) sensors can exchange information through data communication to jointly build a three-dimensional visual point map, and determine the relative position of each other through visual-based position re- identification and PnP (Perspective-n-Points, PnP) methods. When any agent can receive reliable GNSS signals, GNSS positioning information will greatly improve the positioning accuracy without changing the positioning algorithm framework. In order to achieve this function, we designed a set of two-stage position estimation algorithms. In the first stage, we used the modified ORB-SLAM3 algorithm for position estimation by fusing visual and IMU information. In the second stage, we integrated GNSS positioning and cooperative positioning information using the factor graph optimization (FGO) algorithm. Our framework consists of an UGV as the central server node and three UAVs carried by the UGV, that will collaborate on space exploration missions. Finally, we simulated the influence of different visibility and lighting conditions on the framework function on the virtual simulation experiment platform built based on ROS (robot operating system, ROS) and Unity3D. The accuracy of the cooperative localization algorithm and the single platform localization algorithm was evaluated. In the two cases of GNSS-denied and GNSS-challenged, the error of co-location reduced by 15.5% and 19.7%, respectively, compared with single-platform independent positioning.

Fast Tree Detection and Counting on UAVs for Sequential Aerial Images with Generating Orthophoto Mosaicing

Individual tree counting (ITC) is a popular topic in the remote sensing application field. The number and planting density of trees are significant for estimating the yield and for futher planing, etc. Although existing studies have already achieved great performance on tree detection with satellite imagery, the quality is often negatively affected by clouds and heavy fog, which limits the application of high-frequency inventory. Nowadays, with ultra high spatial resolution and convenient usage, Unmanned Aerial Vehicles (UAVs) have become promising tools for obtaining statistics from plantations. However, for large scale areas, a UAV cannot capture the whole region of interest in one photo session. In this paper, a real-time orthophoto mosaicing-based tree counting framework is proposed to detect trees using sequential aerial images, which is very effective for fast detection of large areas. Firstly, to guarantee the speed and accuracy, a multi-planar assumption constrained graph optimization algorithm is proposed to estimate the camera pose and generate orthophoto mosaicing simultaneously. Secondly, to avoid time-consuming box or mask annotations, a point supervised method is designed for tree counting task, which greatly speeds up the entire workflow. We demonstrate the effectiveness of our method by performing extensive experiments on oil-palm and acacia trees. To avoid the delay between data acquisition and processing, the proposed framework algorithm is embedded into the UAV for completing tree counting tasks, which also reduces the quantity of data transmission from the UAV system to the ground station. We evaluate the proposed pipeline using sequential UAV images captured in Indonesia. The proposed pipeline achieves an F1-score of 98.2% for acacia tree detection and 96.3% for oil-palm tree detection with online orthophoto mosaicing generation.

UAV Localization Algorithm Based on Factor Graph Optimization in Complex Scenes.

With the increasingly widespread application of UAV intelligence, the need for autonomous navigation and positioning is becoming more and more important. To solve the problem that UAV cannot perform localization in complex scenes, a new multi-source fusion framework factor graph optimization algorithm is used for UAV localization state estimation in this paper, which is based on IMU/GNSS/VO multi-source sensors. Based on the factor graph model and the iSAM incremental inference algorithm, a multi-source fusion model of IMU/GNSS/VO is established, including the IMU pre-integration factor, IMU bias factor, GNSS factor, and VO factor. Mathematical simulations and validations on the EuRoC dataset show that, when the selected sliding window size is 30, the factor graph optimization (FGO) algorithm can not only meet the requirements of real time and accuracy at the same time, but it also achieves a plug-and-play function in the event of local sensor failures. Finally, compared with the traditional federated Kalman algorithm and the adaptive federated Kalman algorithm, the positioning accuracy of the FGO algorithm in this paper is improved by 1.5–2-fold, and can effectively improve autonomous navigation system robustness and flexibility in complex scenarios. Moreover, the multi-source fusion framework in this paper is a general algorithm framework that can satisfy other scenarios and other types of sensor combinations.

A factor graph optimization mapping based on normaldistributions transform

This paper aims to achieve highly accurate mapping results and real time pose estimation of autonomous vehicle by using the normal distribution transform (NDT) algoritm. A factor graph optimization algorithm (FGO-NDT) is proposed to address the poor real-time performance and pose drift errors of the NDT algorithm. Smooth point cloud data are obtained by multisensor calibration and data preprocessing. NDT registration is then used for lidar odometry and feature matching. The global navigation satellite system (GNSS) data and loop detection results are added to the factor graph framework as the pose constraint factors to optimize the pose trajectory and eliminate the pose drift error generated during mapping. In addition, a sliding window method is used for map registration to extract a local map to shorten the map loading time. Thus, the real-time performance of creating point cloud maps of large scenes is significantly improved. Several experiments are conducted in different environments to verify the accuracy and performance of the FGO-NDT. The experimental results demonstrate that the proposed method can eliminate the pose estimation error caused by drift, improve the local structure, and reduce and root mean square error.

Graph Optimization Algorithm Using Symmetry and Host Bias for Low-Latency Indirect Network

Graph Optimization Algorithm Using Symmetry and Host Bias for Low-Latency Indirect Network

Map Construction Based on LiDAR Vision Inertial Multi-Sensor Fusion

In order to make up for the shortcomings of independent sensors and provide more reliable estimation, a multi-sensor fusion framework for simultaneous localization and mapping is proposed in this paper. Firstly, the light detection and ranging (LiDAR) point cloud is screened in the front-end processing to eliminate abnormal points and improve the positioning and mapping accuracy. Secondly, for the problem of false detection when the LiDAR is surrounded by repeated structures, the intensity value of the laser point cloud is used as the screening condition to screen out robust visual features with high distance confidence, for the purpose of softening. Then, the initial factor, registration factor, inertial measurement units (IMU) factor and loop factor are inserted into the factor graph. A factor graph optimization algorithm based on a Bayesian tree is used for incremental optimization estimation to realize the data fusion. The algorithm was tested in campus and real road environments. The experimental results show that the proposed algorithm can realize state estimation and map construction with high accuracy and strong robustness.

Graph optimization algorithm for low-latency interconnection networks

In various industrial products including parallel computing systems, it is expected that the performance of the whole system will be improved by applying a network topology with smaller diameter and average shortest path length (ASPL). Such network topologies can be defined as the order/degree problem in graph theory by modeling a network topology as an undirected graph. Although previous research indicates that the random graph has both small diameter and ASPL due to the small-world effect, there is still room for improvement. In this paper, we propose an algorithm for the order/degree problem that optimizes random graphs. The feature of the proposed algorithm is that by giving symmetry to the graph, the solution search performance is improved and the calculation time for obtaining the diameter and APSL is greatly reduced. The proposed algorithm is evaluated using various graphs, including a huge graph with one million vertices presented by Graph Golf, an international competition for the order/degree problem. The results show that the proposed algorithm can generate a network topology with sufficiently small diameter and APSL. In addition, we also simulate the latency, performance of the parallel benchmarks, and bisection on the generated network topologies. The results show that the generated network topology performs better than the random topology and a conventional k-ary n-cube topology.