Reliable integration of prior information is crucial for self-verifying and self-updating HD maps. However, no public dataset includes the required triplet of prior maps, current maps, and sensor data. As a result, existing methods must rely on synthetic priors, which create inconsistencies and lead to a significant sim2real gap. To address this, we introduce ArgoTweak, the first dataset to complete the triplet with realistic map priors. At its core, ArgoTweak employs a bijective mapping framework, breaking down large-scale modifications into fine-grained atomic changes at the map element level, thus ensuring interpretability. This paradigm shift enables accurate change detection and integration while preserving unchanged elements with high fidelity. Experiments show that training models on ArgoTweak significantly reduces the sim2real gap compared to synthetic priors. Extensive ablations further highlight the impact of structured priors and detailed change annotations. By establishing a benchmark for explainable, prior-aided HD mapping, ArgoTweak advances scalable, self-improving mapping solutions. The dataset, baselines, map modification toolbox, and further resources are available at https://kth-rpl.github.io/ArgoTweak/.

In this paper, we present a novel approach to tightly couple visual and inertial measurements in a fixed-lag visual-inertial odometry (VIO) framework using information sparsification. To bound computational complexity, fixed-lag smoothers typically marginalize out variables, but consequently introduce a densely connected linear prior which significantly deteriorates accuracy and efficiency. Current state-of-the-art approaches account for the issue by selectively discarding measurements and marginalizing additional variables. However, such strategies are sub-optimal from an information-theoretic perspective. Instead, our approach performs a dense marginalization step and preserves the information content of the dense prior. Our method sparsifies the dense prior with a nonlinear factor graph by minimizing the information loss. The resulting factor graph maintains information sparsity, structural similarity, and nonlinearity. To validate our approach, we conduct real-time drone tests and perform comparisons to current state-of-the-art fixed-lag VIO methods in the EuRoC visual-inertial dataset. The experimental results show that the proposed method achieves competitive and superior accuracy in almost all trials. We include a detailed run-time analysis to demonstrate that the proposed algorithm is suitable for real-time applications.

This paper discusses the possibilities of extending and adapting the CHOMP motion planner [1] to work with a non-holonomic vehicle such as an autonomous truck with a single trailer. A detailed study has been done to find out the different ways of implementing these constraints on the motion planner. CHOMP, which is a successful motion planner for articulated robots produces very fast and collision-free trajectories. This nature is important for a local path adaptor in a multi-vehicle path planning for resolving path-conflicts in a very fast manner and hence, CHOMP was adapted. Secondly, this paper also details the experimental integration of the modified CHOMP with the sensor fusion and control system of an autonomous Volvo FH-16 truck and a set of experiments conducted on a real-time environment. Finally, additional simulations were also conducted to compare the performance of the different approaches developed to study the feasibility of employing CHOMP to autonomous vehicles.

An information-driven autonomous robotic exploration method on a continuous representation of unknown environments is proposed in this paper. The approach conveniently handles sparse sensor measurements to build a continuous model of the environment that exploits structural dependencies without the need to resort to a fixed resolution grid map. A gradient field of occupancy probability distribution is regressed from sensor data as a Gaussian process providing frontier boundaries for further exploration. The resulting continuous global frontier surface completely describes unexplored regions and, inherently, provides an automatic stop criterion for a desired sensitivity. The performance of the proposed approach is evaluated through simulation results in the well-known Freiburg and Cave maps.

Industrial environments are rarely static and often their configuration is continuously changing due to the material transfer flow. This is a major challenge for infrastructure free localization systems. In this paper we address this challenge by introducing a localization approach that uses a dual-timescale approach. The proposed approach - Dual-Timescale Normal Distributions Transform Monte Carlo Localization (DT-NDT-MCL) - is a particle filter based localization method, which simultaneously keeps track of the pose using an apriori known static map and a short-term map. The short-term map is continuously updated and uses Normal Distributions Transform Occupancy maps to maintain the current state of the environment. A key novelty of this approach is that it does not have to select an entire timescale map but rather use the best timescale locally. The approach has real-time performance and is evaluated using three datasets with increasing levels of dynamics. We compare our approach against previously proposed NDT-MCL and commonly used SLAM algorithms and show that DT-NDT-MCL outperforms competing algorithms with regards to accuracy in all three test cases.

The maps that are built by standard feature-based simultaneous localization and mapping (SLAM) methods cannot be directly used to compute paths for navigation, unless enriched with obstacle or traversability information, with the consequent increase in complexity. Here, we propose a method that directly uses the Pose SLAM graph of constraints to determine the path between two robot configurations with lowest accumulated pose uncertainty, i.e., the most reliable path to the goal. The method shows improved navigation results when compared with standard path-planning strategies over both datasets and real-world experiments.

We present an active exploration strategy that complements Pose SLAM [1] and optimal navigation in Pose SLAM [2]. The method evaluates the utility of exploratory and place revisiting sequences and chooses the one that minimizes overall map and path entropies. The technique considers trajectories of similar path length taking marginal pose uncertainties into account. An advantage of the proposed strategy with respect to competing approaches is that to evaluate information gain over the map, only a very coarse prior map estimate needs to be computed. Its coarseness is independent and does not jeopardize the Pose SLAM estimate. Moreover, a replanning scheme is devised to detect significant localization improvement during path execution. The approach is tested in simulations in a common publicly available dataset comparing favorably against frontier based exploration.

The probabilistic belief networks that result from standard feature-based simultaneous localization and map building cannot be directly used to plan trajectories. The reason is that they produce a sparse graph of landmark estimates and their probabilistic relations, which is of little value to find collision free paths for navigation. In contrast, we argue in this paper that Pose SLAM graphs can be directly used as belief roadmaps. We present a method that devises optimal navigation strategies by searching for the path in the pose graph with lowest accumulated robot pose uncertainty, independently of the map reference frame. The method shows improved navigation results when compared to shortest paths both over synthetic data and real datasets.

We present an approach to the problem of 3D map building in urban settings for service robots, using three-dimensional laser range scans as the main data input. Our system is based on the probabilistic alignment of 3D point clouds employing a delayed-state information-form SLAM algorithm, for which we can add observations of relative robot displacements efficiently. These observations come from the alignment of dense range data point clouds computed with a variant of the iterative closest point algorithm. The datasets were acquired with our custom built 3D range scanner integrated into a mobile robot platform. Our mapping results are compared to a GIS-based CAD model of the experimental site. The results show that our approach to 3D mapping performs with sufficient accuracy to derive traversability maps that allow our service robots navigate and accomplish their assigned tasks on a urban pedestrian area.

This paper shows results on outdoor vision-based loop closing for simultaneous localization and mapping. Our experiments show that for loops of over 50 m, the pose estimates maintained with a delayed-state extended information filter are consistent enough to guarantee assertion of vision- based pose constraints for loop closure, provided no necessary information links are added to the estimator. The technique computes relative pose constraints via a robust least squares minimization of 3D point correspondences, which are in turn obtained from the matching of SIFT features over candidate image pairs. We propose a loop closure test that checks both for closeness of means and for highly informative updates at the same time.