-
MegaParticles: Range-based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter
Authors:
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This paper presents a 6-DoF range-based Monte Carlo localization method with a GPU-accelerated Stein particle filter. To update a massive amount of particles, we propose a Gauss-Newton-based Stein variational gradient descent (SVGD) with iterative neighbor particle search. This method uses SVGD to collectively update particle states with gradient and neighborhood information, which provides effici…
▽ More
This paper presents a 6-DoF range-based Monte Carlo localization method with a GPU-accelerated Stein particle filter. To update a massive amount of particles, we propose a Gauss-Newton-based Stein variational gradient descent (SVGD) with iterative neighbor particle search. This method uses SVGD to collectively update particle states with gradient and neighborhood information, which provides efficient particle sampling. For an efficient neighbor particle search, it uses locality sensitive hashing and iteratively updates the neighbor list of each particle over time. The neighbor list is then used to propagate the posterior probabilities of particles over the neighbor particle graph. The proposed method is capable of evaluating one million particles in real-time on a single GPU and enables robust pose initialization and re-localization without an initial pose estimate. In experiments, the proposed method showed an extreme robustness to complete sensor occlusion (i.e., kidnap**), and enabled pinpoint sensor localization without any prior information.
△ Less
Submitted 25 April, 2024;
originally announced April 2024.
-
Tightly-Coupled LiDAR-IMU-Wheel Odometry with Online Calibration of a Kinematic Model for Skid-Steering Robots
Authors:
Taku Okawara,
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno,
Kentaro Uno,
Kazuya Yoshida
Abstract:
Tunnels and long corridors are challenging environments for mobile robots because a LiDAR point cloud should degenerate in these environments. To tackle point cloud degeneration, this study presents a tightly-coupled LiDAR-IMU-wheel odometry algorithm with an online calibration for skid-steering robots. We propose a full linear wheel odometry factor, which not only serves as a motion constraint bu…
▽ More
Tunnels and long corridors are challenging environments for mobile robots because a LiDAR point cloud should degenerate in these environments. To tackle point cloud degeneration, this study presents a tightly-coupled LiDAR-IMU-wheel odometry algorithm with an online calibration for skid-steering robots. We propose a full linear wheel odometry factor, which not only serves as a motion constraint but also performs the online calibration of kinematic models for skid-steering robots. Despite the dynamically changing kinematic model (e.g., wheel radii changes caused by tire pressures) and terrain conditions, our method can address the model error via online calibration. Moreover, our method enables an accurate localization in cases of degenerated environments, such as long and straight corridors, by calibration while the LiDAR-IMU fusion sufficiently operates. Furthermore, we estimate the uncertainty (i.e., covariance matrix) of the wheel odometry online for creating a reasonable constraint. The proposed method is validated through three experiments. The first indoor experiment shows that the proposed method is robust in severe degeneracy cases (long corridors) and changes in the wheel radii. The second outdoor experiment demonstrates that our method accurately estimates the sensor trajectory despite being in rough outdoor terrain owing to online uncertainty estimation of wheel odometry. The third experiment shows the proposed online calibration enables robust odometry estimation in changing terrains.
△ Less
Submitted 3 April, 2024;
originally announced April 2024.
-
Tightly Coupled Range Inertial Localization on a 3D Prior Map Based on Sliding Window Factor Graph Optimization
Authors:
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This paper presents a range inertial localization algorithm for a 3D prior map. The proposed algorithm tightly couples scan-to-scan and scan-to-map point cloud registration factors along with IMU factors on a sliding window factor graph. The tight coupling of the scan-to-scan and scan-to-map registration factors enables a smooth fusion of sensor ego-motion estimation and map-based trajectory corre…
▽ More
This paper presents a range inertial localization algorithm for a 3D prior map. The proposed algorithm tightly couples scan-to-scan and scan-to-map point cloud registration factors along with IMU factors on a sliding window factor graph. The tight coupling of the scan-to-scan and scan-to-map registration factors enables a smooth fusion of sensor ego-motion estimation and map-based trajectory correction that results in robust tracking of the sensor pose under severe point cloud degeneration and defective regions in a map. We also propose an initial sensor state estimation algorithm that robustly estimates the gravity direction and IMU state and helps perform global localization in 3- or 4-DoF for system initialization without prior position information. Experimental results show that the proposed method outperforms existing state-of-the-art methods in extremely severe situations where the point cloud data becomes degenerate, there are momentary sensor interruptions, or the sensor moves along the map boundary or into unmapped regions.
△ Less
Submitted 8 February, 2024;
originally announced February 2024.
-
3D-BBS: Global Localization for 3D Point Cloud Scan Matching Using Branch-and-Bound Algorithm
Authors:
Koki Aoki,
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno,
Junichi Meguro
Abstract:
This paper presents an accurate and fast 3D global localization method, 3D-BBS, that extends the existing branch-and-bound (BnB)-based 2D scan matching (BBS) algorithm. To reduce memory consumption, we utilize a sparse hash table for storing hierarchical 3D voxel maps. To improve the processing cost of BBS in 3D space, we propose an efficient roto-translational space branching. Furthermore, we dev…
▽ More
This paper presents an accurate and fast 3D global localization method, 3D-BBS, that extends the existing branch-and-bound (BnB)-based 2D scan matching (BBS) algorithm. To reduce memory consumption, we utilize a sparse hash table for storing hierarchical 3D voxel maps. To improve the processing cost of BBS in 3D space, we propose an efficient roto-translational space branching. Furthermore, we devise a batched BnB algorithm to fully leverage GPU parallel processing. Through experiments in simulated and real environments, we demonstrated that the 3D-BBS enabled accurate global localization with only a 3D LiDAR scan roughly aligned in the gravity direction and a 3D pre-built map. This method required only 878 msec on average to perform global localization and outperformed state-of-the-art global registration methods in terms of accuracy and processing speed.
△ Less
Submitted 5 March, 2024; v1 submitted 15 October, 2023;
originally announced October 2023.
-
Exact Point Cloud Downsampling for Fast and Accurate Global Trajectory Optimization
Authors:
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This paper presents a point cloud downsampling algorithm for fast and accurate trajectory optimization based on global registration error minimization. The proposed algorithm selects a weighted subset of residuals of the input point cloud such that the subset yields exactly the same quadratic point cloud registration error function as that of the original point cloud at the evaluation point. This…
▽ More
This paper presents a point cloud downsampling algorithm for fast and accurate trajectory optimization based on global registration error minimization. The proposed algorithm selects a weighted subset of residuals of the input point cloud such that the subset yields exactly the same quadratic point cloud registration error function as that of the original point cloud at the evaluation point. This method accurately approximates the original registration error function with only a small subset of input points (29 residuals at a minimum). Experimental results using the KITTI dataset demonstrate that the proposed algorithm significantly reduces processing time (by 87\%) and memory consumption (by 99\%) for global registration error minimization while retaining accuracy.
△ Less
Submitted 24 December, 2023; v1 submitted 6 July, 2023;
originally announced July 2023.
-
Single-Shot Global Localization via Graph-Theoretic Correspondence Matching
Authors:
Shigemichi Matsuzaki,
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This paper describes a method of global localization based on graph-theoretic association of instances between a query and the prior map. The proposed framework employs correspondence matching based on the maximum clique problem (MCP). The framework is potentially applicable to other map and/or query modalities thanks to the graph-based abstraction of the problem, while many of existing global loc…
▽ More
This paper describes a method of global localization based on graph-theoretic association of instances between a query and the prior map. The proposed framework employs correspondence matching based on the maximum clique problem (MCP). The framework is potentially applicable to other map and/or query modalities thanks to the graph-based abstraction of the problem, while many of existing global localization methods rely on a query and the dataset in the same modality. We implement it with a semantically labeled 3D point cloud map, and a semantic segmentation image as a query. Leveraging the graph-theoretic framework, the proposed method realizes global localization exploiting only the map and the query. The method shows promising results on multiple large-scale simulated maps of urban scenes.
△ Less
Submitted 6 June, 2023;
originally announced June 2023.
-
General, Single-shot, Target-less, and Automatic LiDAR-Camera Extrinsic Calibration Toolbox
Authors:
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This paper presents an open source LiDAR-camera calibration toolbox that is general to LiDAR and camera projection models, requires only one pairing of LiDAR and camera data without a calibration target, and is fully automatic. For automatic initial guess estimation, we employ the SuperGlue image matching pipeline to find 2D-3D correspondences between LiDAR and camera data and estimate the LiDAR-c…
▽ More
This paper presents an open source LiDAR-camera calibration toolbox that is general to LiDAR and camera projection models, requires only one pairing of LiDAR and camera data without a calibration target, and is fully automatic. For automatic initial guess estimation, we employ the SuperGlue image matching pipeline to find 2D-3D correspondences between LiDAR and camera data and estimate the LiDAR-camera transformation via RANSAC. Given the initial guess, we refine the transformation estimate with direct LiDAR-camera registration based on the normalized information distance, a mutual information-based cross-modal distance metric. For a handy calibration process, we also present several assistance capabilities (e.g., dynamic LiDAR data integration and user interface for making 2D-3D correspondence manually). The experimental results show that the proposed toolbox enables calibration of any combination of spinning and non-repetitive scan LiDARs and pinhole and omnidirectional cameras, and shows better calibration accuracy and robustness than those of the state-of-the-art edge-alignment-based calibration method.
△ Less
Submitted 10 February, 2023;
originally announced February 2023.
-
Generalized LOAM: LiDAR Odometry Estimation with Trainable Local Geometric Features
Authors:
Kohei Honda,
Kenji Koide,
Masashi Yokozuka,
Shuji Oishi,
Atsuhiko Banno
Abstract:
This paper presents a LiDAR odometry estimation framework called Generalized LOAM. Our proposed method is generalized in that it can seamlessly fuse various local geometric shapes around points to improve the position estimation accuracy compared to the conventional LiDAR odometry and map** (LOAM) method. To utilize continuous geometric features for LiDAR odometry estimation, we incorporate tiny…
▽ More
This paper presents a LiDAR odometry estimation framework called Generalized LOAM. Our proposed method is generalized in that it can seamlessly fuse various local geometric shapes around points to improve the position estimation accuracy compared to the conventional LiDAR odometry and map** (LOAM) method. To utilize continuous geometric features for LiDAR odometry estimation, we incorporate tiny neural networks into a generalized iterative closest point (GICP) algorithm. These neural networks improve the data association metric and the matching cost function using local geometric features. Experiments with the KITTI benchmark demonstrate that our proposed method reduces relative trajectory errors compared to the other LiDAR odometry estimation methods.
△ Less
Submitted 29 October, 2022;
originally announced October 2022.
-
Scalable Fiducial Tag Localization on a 3D Prior Map via Graph-Theoretic Global Tag-Map Registration
Authors:
Kenji Koide,
Shuji Oishi,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This paper presents an accurate and scalable method for fiducial tag localization on a 3D prior environmental map. The proposed method comprises three steps: 1) visual odometry-based landmark SLAM for estimating the relative poses between fiducial tags, 2) geometrical matching-based global tag-map registration via maximum clique finding, and 3) tag pose refinement based on direct camera-map alignm…
▽ More
This paper presents an accurate and scalable method for fiducial tag localization on a 3D prior environmental map. The proposed method comprises three steps: 1) visual odometry-based landmark SLAM for estimating the relative poses between fiducial tags, 2) geometrical matching-based global tag-map registration via maximum clique finding, and 3) tag pose refinement based on direct camera-map alignment with normalized information distance. Through simulation-based evaluations, the proposed method achieved a 98 \% global tag-map registration success rate and an average tag pose estimation accuracy of a few centimeters. Experimental results in a real environment demonstrated that it enables to localize over 110 fiducial tags placed in an environment in 25 minutes for data recording and post-processing.
△ Less
Submitted 25 July, 2022;
originally announced July 2022.
-
Globally Consistent and Tightly Coupled 3D LiDAR Inertial Map**
Authors:
Kenji Koide,
Masashi Yokozuka,
Shuji Oishi,
Atsuhiko Banno
Abstract:
This paper presents a real-time 3D map** framework based on global matching cost minimization and LiDAR-IMU tight coupling. The proposed framework comprises a preprocessing module and three estimation modules: odometry estimation, local map**, and global map**, which are all based on the tight coupling of the GPU-accelerated voxelized GICP matching cost factor and the IMU preintegration fact…
▽ More
This paper presents a real-time 3D map** framework based on global matching cost minimization and LiDAR-IMU tight coupling. The proposed framework comprises a preprocessing module and three estimation modules: odometry estimation, local map**, and global map**, which are all based on the tight coupling of the GPU-accelerated voxelized GICP matching cost factor and the IMU preintegration factor. The odometry estimation module employs a keyframe-based fixed-lag smoothing approach for efficient and low-drift trajectory estimation, with a bounded computation cost. The global map** module constructs a factor graph that minimizes the global registration error over the entire map with the support of IMU constraints, ensuring robust optimization in feature-less environments. The evaluation results on the Newer College dataset and KAIST urban dataset show that the proposed framework enables accurate and robust localization and map** in challenging environments.
△ Less
Submitted 1 February, 2022; v1 submitted 1 February, 2022;
originally announced February 2022.
-
Globally Consistent 3D LiDAR Map** with GPU-accelerated GICP Matching Cost Factors
Authors:
Kenji Koide,
Masashi Yokozuka,
Shuji Oishi,
Atsuhiko Banno
Abstract:
This paper presents a real-time 3D LiDAR map** framework based on global matching cost minimization. The proposed method constructs a factor graph that directly minimizes matching costs between frames over the entire map, unlike pose graph-based approaches that minimize errors in the pose space. For real-time global matching cost minimization, we use a voxel data association-based GICP matching…
▽ More
This paper presents a real-time 3D LiDAR map** framework based on global matching cost minimization. The proposed method constructs a factor graph that directly minimizes matching costs between frames over the entire map, unlike pose graph-based approaches that minimize errors in the pose space. For real-time global matching cost minimization, we use a voxel data association-based GICP matching cost factor that is able to fully leverage GPU parallel processing. The combination of the matching cost factor and GPU computation enables constraint of the relative pose between frames with a small overlap and creation of a densely connected factor graph. The map** process is managed based on a voxel-based overlap metric that can quickly be evaluated on a GPU. We incorporate the proposed method with an external loop detection method in order to help the voxel-based matching cost factors to avoid convergence in a local solution. The experimental result on the KITTI dataset shows that the proposed approach improves the estimation accuracy of long trajectories.
△ Less
Submitted 24 December, 2023; v1 submitted 15 September, 2021;
originally announced September 2021.
-
4D Attention: Comprehensive Framework for Spatio-Temporal Gaze Map**
Authors:
Shuji Oishi,
Kenji Koide,
Masashi Yokozuka,
Atsuhiko Banno
Abstract:
This study presents a framework for capturing human attention in the spatio-temporal domain using eye-tracking glasses. Attention map** is a key technology for human perceptual activity analysis or Human-Robot Interaction (HRI) to support human visual cognition; however, measuring human attention in dynamic environments is challenging owing to the difficulty in localizing the subject and dealing…
▽ More
This study presents a framework for capturing human attention in the spatio-temporal domain using eye-tracking glasses. Attention map** is a key technology for human perceptual activity analysis or Human-Robot Interaction (HRI) to support human visual cognition; however, measuring human attention in dynamic environments is challenging owing to the difficulty in localizing the subject and dealing with moving objects. To address this, we present a comprehensive framework, 4D Attention, for unified gaze map** onto static and dynamic objects. Specifically, we estimate the glasses pose by leveraging a loose coupling of direct visual localization and Inertial Measurement Unit (IMU) values. Further, by installing reconstruction components into our framework, dynamic objects not captured in the 3D environment map are instantiated based on the input images. Finally, a scene rendering component synthesizes a first-person view with identification (ID) textures and performs direct 2D-3D gaze association. Quantitative evaluations showed the effectiveness of our framework. Additionally, we demonstrated the applications of 4D Attention through experiments in real situations.
△ Less
Submitted 8 July, 2021;
originally announced July 2021.
-
Adaptive Hyperparameter Tuning for Black-box LiDAR Odometry
Authors:
Kenji Koide,
Masashi Yokozuka,
Shuji Oishi,
Atsuhiko Banno
Abstract:
This study proposes an adaptive data-driven hyperparameter tuning framework for black-box 3D LiDAR odometry algorithms. The proposed framework comprises offline parameter-error function modeling and online adaptive parameter selection. In the offline step, we run the odometry estimation algorithm for tuning with different parameters and environments and evaluate the accuracy of the estimated traje…
▽ More
This study proposes an adaptive data-driven hyperparameter tuning framework for black-box 3D LiDAR odometry algorithms. The proposed framework comprises offline parameter-error function modeling and online adaptive parameter selection. In the offline step, we run the odometry estimation algorithm for tuning with different parameters and environments and evaluate the accuracy of the estimated trajectories to build a surrogate function that predicts the trajectory estimation error for the given parameters and environments. Subsequently, we select the parameter set that is expected to result in good accuracy in the given environment based on trajectory error prediction with the surrogate function. The proposed framework does not require detailed information on the inner working of the algorithm to be tuned, and improves its accuracy by adaptively optimizing the parameter set. We first demonstrate the role of the proposed framework in improving the accuracy of odometry estimation across different environments with a simulation-based toy example. Further, an evaluation on the public dataset KITTI shows that the proposed framework can improve the accuracy of several odometry estimation algorithms in practical situations.
△ Less
Submitted 8 July, 2021; v1 submitted 1 July, 2021;
originally announced July 2021.
-
LiTAMIN2: Ultra Light LiDAR-based SLAM using Geometric Approximation applied with KL-Divergence
Authors:
Masashi Yokozuka,
Kenji Koide,
Shuji Oishi,
Atsuhiko Banno
Abstract:
In this paper, a three-dimensional light detection and ranging simultaneous localization and map** (SLAM) method is proposed that is available for tracking and map** with 500--1000 Hz processing. The proposed method significantly reduces the number of points used for point cloud registration using a novel ICP metric to speed up the registration process while maintaining accuracy. Point cloud r…
▽ More
In this paper, a three-dimensional light detection and ranging simultaneous localization and map** (SLAM) method is proposed that is available for tracking and map** with 500--1000 Hz processing. The proposed method significantly reduces the number of points used for point cloud registration using a novel ICP metric to speed up the registration process while maintaining accuracy. Point cloud registration with ICP is less accurate when the number of points is reduced because ICP basically minimizes the distance between points. To avoid this problem, symmetric KL-divergence is introduced to the ICP cost that reflects the difference between two probabilistic distributions. The cost includes not only the distance between points but also differences between distribution shapes. The experimental results on the KITTI dataset indicate that the proposed method has high computational efficiency, strongly outperforms other methods, and has similar accuracy to the state-of-the-art SLAM method.
△ Less
Submitted 1 March, 2021;
originally announced March 2021.
-
VITAMIN-E: VIsual Tracking And MappINg with Extremely Dense Feature Points
Authors:
Masashi Yokozuka,
Shuji Oishi,
Thompson Simon,
Atsuhiko Banno
Abstract:
In this paper, we propose a novel indirect monocular SLAM algorithm called "VITAMIN-E," which is highly accurate and robust as a result of tracking extremely dense feature points. Typical indirect methods have difficulty in reconstructing dense geometry because of their careful feature point selection for accurate matching. Unlike conventional methods, the proposed method processes an enormous num…
▽ More
In this paper, we propose a novel indirect monocular SLAM algorithm called "VITAMIN-E," which is highly accurate and robust as a result of tracking extremely dense feature points. Typical indirect methods have difficulty in reconstructing dense geometry because of their careful feature point selection for accurate matching. Unlike conventional methods, the proposed method processes an enormous number of feature points by tracking the local extrema of curvature informed by dominant flow estimation. Because this may lead to high computational cost during bundle adjustment, we propose a novel optimization technique, the "subspace Gauss--Newton method", that significantly improves the computational efficiency of bundle adjustment by partially updating the variables. We concurrently generate meshes from the reconstructed points and merge them for an entire 3D model. The experimental results on the SLAM benchmark dataset EuRoC demonstrated that the proposed method outperformed state-of-the-art SLAM methods, such as DSO, ORB-SLAM, and LSD-SLAM, both in terms of accuracy and robustness in trajectory estimation. The proposed method simultaneously generated significantly detailed 3D geometry from the dense feature points in real time using only a CPU.
△ Less
Submitted 16 December, 2019; v1 submitted 23 April, 2019;
originally announced April 2019.
