Non-rigid structure-from-motion (NRSfM), a promising technique for addressing the mapping challenges in monocular visual deformable simultaneous localization and mapping (SLAM), has attracted growing attention. We introduce a novel method, called Con-NRSfM, for NRSfM under conformal deformations, encompassing isometric deformations as a subset. Our approach performs point-wise reconstruction using 2D selected image warps optimized through a graph-based framework. Unlike existing methods that rely on strict assumptions, such as locally planar surfaces or locally linear deformations, and fail to recover the conformal scale, our method eliminates these constraints and accurately computes the local conformal scale. Additionally, our framework decouples constraints on depth and conformal scale, which are inseparable in other approaches, enabling more precise depth estimation. To address the sensitivity of the formulated problem, we employ a parallel separable iterative optimization strategy. Furthermore, a self-supervised learning framework, utilizing an encoder-decoder network, is incorporated to generate dense 3D point clouds with texture. Simulation and experimental results using both synthetic and real datasets demonstrate that our method surpasses existing approaches in terms of reconstruction accuracy and robustness. The code for the proposed method will be made publicly available on the project website:https://sites.google.com/view/con-nrsfm.

Vision-based localization for autonomous driving has been of great interest among researchers. When a pre-built 3D map is not available, the techniques of visual simultaneous localization and mapping (SLAM) are typically adopted. Due to error accumulation, visual SLAM (vSLAM) usually suffers from long-term drift. This paper proposes a framework to increase the localization accuracy by fusing the vSLAM with a deep-learning based ground-to-satellite (G2S) image registration method. In this framework, a coarse (spatial correlation bound check) to fine (visual odometry consistency check) method is designed to select the valid G2S prediction. The selected prediction is then fused with the SLAM measurement by solving a scaled pose graph problem. To further increase the localization accuracy, we provide an iterative trajectory fusion pipeline. The proposed framework is evaluated on two well-known autonomous driving datasets, and the results demonstrate the accuracy and robustness in terms of vehicle localization. The code will be available at https://github.com/YanhaoZhang/SLAM-G2S-Fusion.

Efficiently searching for target objects in complex environments that contain various types of furniture, such as shelves, tables, and beds, is crucial for mobile robots, but it poses significant challenges due to various factors such as localization errors, limited field of view, and visual occlusion. To address this problem, we propose a Partially Observable Markov Decision Process (POMDP) formulation with a growing state space for object search in a 3D region. We solve this POMDP by carefully designing a perception module and developing a planning algorithm, called Growing Partially Observable Monte-Carlo Planning (GPOMCP), based on online Monte-Carlo tree search and belief tree reuse with a novel upper confidence bound. We have demonstrated that belief tree reuse is reasonable and achieves good performance when the belief differences are limited. Additionally, we introduce a guessed target object with an updating grid world to guide the search in the information-less and reward-less cases, like the absence of any detected objects. We tested our approach using Gazebo simulations on four scenarios of target finding in a realistic indoor living environment with the Fetch robot simulator. Compared to the baseline approaches, which are based on POMCP, our results indicate that our approach enables the robot to find the target object with a higher success rate faster while using the same computational requirements.

Fluoroscopy-guided endovascular interventions by using X-ray images are challenging. The catheter needs to be manipulated precisely inside the aorta, while only 2-D views from the X-ray fluoroscopy are currently used to help the surgeons. Because the catheter is operated in a 3-D space, a visualization of the deforming 3-D aorta will be useful as guidance for catheter manipulation. Existing 3-D reconstruction methods fall short in only focusing on the deformation reconstruction of the aortic 3-D centerline, or using additional prior knowledge of 3-D catheter position for estimating the aortic 3-D deformation. In this article, we propose a novel framework that reconstructs the aortic 3-D deformation by fusing a preoperative 3-D model and two intraoperative X-ray images. Different from existing methods, the proposed framework reconstructs aortic deformation using a coarse-to-fine pipeline by first reconstructing the aortic 3-D centerline and then reconstructing the 3-D shape. To obtain the accurate features for the fluoroscopic-based 3-D reconstruction, we extract semantic features from the X-ray images, and compute the distance field to efficiently calculate the 3-D–2-D nonrigid correspondence. Nonlinear least squares optimization is used to solve the deformation of both centerline and shape. The proposed framework is validated using phantom and patient datasets, whose results demonstrate improved efficiency and accuracy compared with the existing methods. This framework provides a valuable clinical tool for endovascular interventions.

This article considers simultaneous localization and mapping (SLAM) problem for robots in situations where accurate estimates for some of the robot poses, termed anchors, are available. These may be acquired through external means, for example, by either stopping the robot at some previously known locations or pausing for a sufficient period of time to measure the robot poses with an external measurement system. The main contribution is an efficient algorithm for selecting a fixed number of anchors from a set of potential poses that minimizes estimated error in the SLAM solution. Based on a graph-topological connection between the D-optimality design metric and the tree-connectivity of the pose-graph, the anchor selection problem can be formulated approximately as a submatrix selection problem for reduced weighted Laplacian matrix, leading to a cardinality-constrained submodular maximization problem. Two greedy methods are presented to solve this submodular optimization problem with a performance guarantee. These methods are complemented by Cholesky decomposition, approximate minimum degree permutation, order reuse, and rank-1 update that exploit the sparseness of the weighted Laplacian matrix. We demonstrate the efficiency and effectiveness of the proposed techniques on public-domain datasets, Gazebo simulations, and real-world experiments.

Right invariant extended Kalman filter (RIEKF) based simultaneous localization and mapping (SLAM) proposed recently has shown to be able to produce more consistent SLAM estimates as compared with traditional EKF based SLAM methods, including some improved EKF SLAM methods such as observability constrained-EKF (OC-EKF) SLAM. Latest results have demonstrated that its performance is very close to optimization based SLAM algorithms such as iSAM. In this paper, we propose to use RIEKF SLAM algorithm in active SLAM where both the predicted SLAM results for choosing control actions and the actual estimated SLAM results applying the selected control actions are computed using RIEKF algorithms. The advantages over traditional EKF based active SLAM are the more accurate and consistent predicted uncertainty estimates which result in robustness of the active SLAM algorithm. The advantages over optimization based active SLAM is the reduced computational cost. Simulation results are presented to validate the advantages of the proposed algorithm3.

SLAM in deformable environments is a very challenging research topic. Some research works have been presented by different research groups in the past few years. However, there are still some challenging research questions remaining unanswered. This paper discusses some of these research questions focusing on the case when point features are used to describe the deformable environments. The SLAM problems are formulated as extensions of point feature based SLAM in static environments, including both optimisation based offline SLAM and filter based online SLAM. To illustrate the problems and questions more clearly, some concepts and results using simple 2D examples are presented. The MATLAB source codes of the results are made publicly available (https://github.com/cyb1212/DeformableSLAM2D.git) to help the readers understand the problems more clearly.

Two-dimensional (2-D)/3-D pose-graph simultaneous localization and mapping (SLAM) is a problem of estimating a set of poses based on noisy measurements of relative rotations and translations. This article focuses on the relation between the graphical structure of pose-graph SLAM and Fisher information matrix (FIM), Cramér-Rao lower bounds (CRLB), and its optimal design metrics (T-optimality and D-optimality). As a main contribution, based on the assumption of isotropic Langevin noise for rotation and block-isotropic Gaussian noise for translation, the FIM and CRLB are derived and shown to be closely related to the graph structure, in particular, the weighted Laplacian matrix. We also prove that total node degree and weighted number of spanning trees, as two graph connectivity metrics, are, respectively, closely related to the trace and determinant of the FIM. The discussions show that, compared with the D-optimality metric, the T-optimality metric is more easily computed but less effective. We also present upper and lower bounds for the D-optimality metric, which can be efficiently computed and are almost independent of the estimation results. The results are verified with several well-known datasets, such as Intel, KITTI, sphere, and so on.

In this paper, we present an on-line active pose-graph simultaneous localization and mapping (SLAM) frame-work for robots in three-dimensional (3D) environments using graph topology and sub-maps. This framework aims to find the best trajectory for loop-closure by re-visiting old poses based on the T-optimality and D-optimality metrics of the Fisher information matrix (FIM) in pose-graph SLAM. In order to reduce computational complexity, graph topologies are introduced, including weighted node degree (T-optimality metric) and weighted tree-connectivity (D-optimality metric), to choose a candidate trajectory and several key poses. With the help of the key poses, a sampling-based path planning method and a continuous-time trajectory optimization method are combined hierarchically and applied in the whole framework. So as to further improve the real-time capability of the method, the sub-map joining method is used in the estimation and planning process for large-scale active SLAM problems. In simulations and experiments, we validate our approach by comparing against existing methods, and we demonstrate the on-line planning part using a quad-rotor unmanned aerial vehicle (UAV).

The active SLAM problem considered in this paper aims to plan a robot trajectory for simultaneous localization and mapping (SLAM) as well as for an area coverage task with robot pose uncertainty. Based on a model predictive control (MPC) framework, these two problems are solved respectively by different methods. For the uncertainty minimization MPC problem, based on the graphical structure of the 2D feature-based SLAM, a non-convex constrained least-squares problem is presented to approximate the original problem. Then, using variable substitutions, it is further transformed into a convex problem, and then solved by a convex optimization method. For the coverage task considering robot pose uncertainty, it is formulated and solved by the MPC framework and the sequential quadratic programming (SQP) method. In the whole process, considering the computation complexity, we use linear SLAM, which is a submap joining approach, to reduce the time for planning and estimation. Finally, various simulations are presented to validate the effectiveness of the proposed approach.