Minimal parametrization of 3D lines plays a critical role in camera localization and structural mapping. Existing representations in robotics and computer vision predominantly handle independent lines, overlooking structural regularities such as sets of parallel lines that are pervasive in man-made environments. This paper introduces RiemanLine, a unified minimal representation for 3D lines formulated on Riemannian manifolds that jointly accommodates both individual lines and parallel-line groups. Our key idea is to decouple each line landmark into global and local components: a shared vanishing direction optimized on the unit sphere, and scaled normal vectors constrained on orthogonal subspaces, enabling compact encoding of structural regularities. For n parallel lines, the proposed representation reduces the parameter space from 4n (orthonormal form) to 2n+2, naturally embedding parallelism without explicit constraints. We further integrate this parameterization into a factor graph framework, allowing global direction alignment and local reprojection optimization within a unified manifold-based bundle adjustment. Extensive experiments on ICL-NUIM, TartanAir, and synthetic benchmarks demonstrate that our method achieves significantly more accurate pose estimation and line reconstruction, while reducing parameter dimensionality and improving convergence stability.

Inconsistency issue is one crucial challenge for the performance of extended Kalman filter (EKF) based methods for state estimation problems, which is mainly affected by the discrepancy of observability between the EKF model and the underlying dynamic system. In this work, some sufficient and necessary conditions for observability maintenance are first proved. We find that under certain conditions, an EKF can naturally maintain correct observability if the corresponding linearization makes unobservable subspace independent of the state values. Based on this theoretical finding, a novel affine EKF (Aff-EKF) framework is proposed to overcome the inconsistency of standard EKF (Std-EKF) by affine transformations, which not only naturally satisfies the observability constraint but also has a clear design procedure. The advantages of our Aff-EKF framework over some commonly used methods are demonstrated through mathematical analyses. The effectiveness of our proposed method is demonstrated on three different simultaneous localization and mapping (SLAM) applications and one 3D cooperative localization (CL) problem. Specifically, following the proposed procedure, the naturally consistent Aff-EKFs can be explicitly derived for these problems. The consistency improvement of these Aff-EKFs are validated by Monte Carlo simulations.

In computer-assisted orthopedic surgery (CAOS), accurate pre-operative to intra-operative bone registration is an essential and critical requirement for providing navigational guidance. This registration process is challenging since the intra-operative 3D points are sparse, only partially overlapped with the pre-operative model, and disturbed by noise and outliers. The commonly used method in current state-of-the-art orthopedic robotic system is bony landmarks based registration, but it is very time-consuming for the surgeons. To address these issues, we propose a novel partial-to-full registration framework based on gradient-SDF for CAOS. The simulation experiments using bone models from publicly available datasets and the phantom experiments performed under both optical tracking and electromagnetic tracking systems demonstrate that the proposed method can provide more accurate results than standard benchmarks and be robust to 90% outliers. Importantly, our method achieves convergence in less than 1 second in real scenarios and mean target registration error values as low as 2.198 mm for the entire bone model. Finally, it only requires random acquisition of points for registration by moving a surgical probe over the bone surface without the need for correspondences, thus showing significant potential clinical value. The code of the framework is available*.

With the growing number of patients experiencing knee-related conditions, total knee arthroplasty (TKA) has become a common procedure, where a 3D visualisation of the patient’s tibia and fibula is essential for preoperative planning. Traditional imaging techniques, such as computed tomography (CT), often expose patients to high levels of radiation or impose significant financial costs. As an alternative, this paper proposes a novel approach that reconstructs a 3D model of the tibia and fibula using only two X-ray images (taken from the coronal and sagittal planes) and a general template, significantly reducing radiation exposure and financial burden. Our algorithm of 3D reconstruction for patient-specific anatomies combines point-based deformation with deep learning techniques. Initially, the general model undergoes a preliminary deformation to match the patient tibia and fibula dimensions. This pre-deformed model then serves as a template, followed by a fine deformation process via a self-supervised graph convolutional network (GCN), whose parameters are trained iteratively by comparing the template projection and the X-ray measurements. Following tests in simulations, cadaver experiments, and in-vivo experiments, our proposed algorithm demonstrates state-of-the-art accuracy and exceptional robustness across different evaluation metrics. Our code is available at https://github.com/DrKaiPan/tfDeform_GCN.git

This paper introduces EDeformNet, a novel method for real-time 3D reconstruction of fishing nets using sparse positional measurements. Currently, net deployment during large-scale fishing operations is challenging as the submerged lattice deformations that occur in response to the various environmental factors are not visible to the vessel operator. EDeformNet extends Embedded Deformation Graphs (EDGs), a commonly used technique in template-based nonrigid 3D reconstruction that allows control of embedded spaces through sparse control point correspondences. These can be suitably derived from acoustic tracking beacons attached to the net. EDeformNet enhances the standard EDG optimization scheme by including constraints that preserve surface normals at control points and guard distances between vertices in the template mesh. These improvements are proven to enable an accurate representation of the complex deformations and movements typical in purse seine nets, the fishing technique where the algorithm has been tested, which standard EDG is unable to attain. Moreover, EDeformNet also proposes a tailored strategy that dynamically adjusts the net template according to the known length of the deployed portion of the fishing net. This approach reconstructs exclusively the submerged portion of the fishing net, avoiding extraneous data from above-water sections and enhancing accuracy under realistic fishing conditions. The proposed method is validated using realistic 3D physics simulations in Blender, where quantifiable comparisons demonstrate that EDeformNet effectively captures the spatial dynamics of purse-seining. Compared to standard EDG, EDeformNet achieves superior performance, resulting in at least a 25% improvement across the array of challenging temporal scenarios studied.

This paper presents a novel tightly coupled Filter-based monocular visual-inertial-wheel odometry (VIWO) system for ground robots, designed to deliver accurate and robust localization in long-term complex outdoor navigation scenarios. As an external sensor, the camera enhances localization performance by introducing visual constraints. However, obtaining a sufficient number of effective visual features is often challenging, particularly in dynamic or low-texture environments. To address this issue, we incorporate the line features for additional geometric constraints. Unlike traditional approaches that treat point and line features independently, our method exploits the geometric relationships between points and lines in 2D images, enabling fast and robust line matching and triangulation. Additionally, we introduce Motion Consistency Check (MCC) to filter out potential dynamic points, ensuring the effectiveness of point feature updates. The proposed system was evaluated on publicly available datasets and benchmarked against state-of-the-art methods. Experimental results demonstrate superior performance in terms of accuracy, robustness, and efficiency. The source code is publicly available at: https://github.com/Happy-ZZX/PL-VIWO.

Multiview point cloud registration is a fundamental task for constructing globally consistent 3D models. Existing approaches typically rely on feature extraction and data association across multiple point clouds. However, these processes are challenging to obtain global optimal solution in complex environments. In this paper, we introduce a novel correspondence-free multiview point cloud registration method. Specifically, we represent the global map as a depth map and leverage raw depth information to formulate a non-linear least squares optimisation that jointly estimates poses of point clouds and the global map. Unlike traditional feature-based bundle adjustment methods, which rely on explicit feature extraction and data association, our method bypasses these by associating multi-frame point clouds with a global depth map through their corresponding poses. This data association is implicitly incorporated and dynamically refined during the optimisation process. Extensive evaluations on real-world datasets demonstrate that our method outperforms state-of-the-art approaches in accuracy, particularly in challenging environments where feature extraction and data association are difficult.

This paper focuses on developing a performance guaranteed state estimation algorithm for 2D pose graph problems for mobile robots. Different from probabilistic methods, the measurement noises are only assumed to be bounded without any prior knowledge about their distributions. Based on the interval analysis, we first propose a vanilla sequential contractor that iteratively uses edge-wise noise bounds to contract pose intervals at the nodes, which can provide the guaranteed feasible domains that contain the ground-truth values. Then, to improve the efficiency in solving large-scale pose graphs, an efficient batch contractor is developed by improving the update order and exploiting a relaxation of the nonlinear measurement functions. The effectiveness and efficiency of our approaches are validated on simulated and real-world datasetsNote to Practitioners—Pose graph is one of the most popular formulations for the state estimations of mobile robots. There have been many probabilistic algorithms for pose graphs based on the Gaussian-like measurement noise assumption. However, the measurement noises in many practical situations may not follow Gaussian distributions but have hard bounds. Consequently, the existing pose graph algorithms are far away from achieving the expected high reliability in the practical safety-critical applications such as autonomous driving. To achieve guaranteed performance, an efficient interval based approach is proposed for the large-scale pose graph problems with hard bound measurement noises. It can provide the guaranteed hard error bounds for the robot poses, which has the potential in uncertainty quantification, reliability analysis and outlier detection of safety-critical systems.

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.

Joint optimization of poses and features has been extensively studied and demonstrated to yield more accurate results in feature-based SLAM problems. However, research on jointly optimizing poses and non-feature-based maps remains limited. Occupancy maps are widely used non-feature-based environment representations because they effectively classify spaces into obstacles, free areas, and unknown regions, providing robots with spatial information for various tasks. In this paper, we propose Occupancy-SLAM, a novel optimization-based SLAM method that enables the joint optimization of robot trajectory and the occupancy map through a parameterized map representation. The key novelty lies in optimizing both robot poses and occupancy values at different cell vertices simultaneously, a significant departure from existing methods where the robot poses need to be optimized first before the map can be estimated. This paper focuses on 2D laser-based SLAM to investigate how to jointly optimize robot poses and the occupancy map. In our formulation, the state variables in optimization include all the robot poses and the occupancy values at discrete cell vertices in the occupancy map. Moreover, a multi-resolution optimization framework that utilizes occupancy maps with varying resolutions in different stages is introduced. A variation of Gauss-Newton method is proposed to solve the optimization problem at different stages to obtain the optimized occupancy map and robot trajectory. The proposed algorithm is efficient and converges easily with initialization from either odometry inputs or scan matching, even when only limited key-frame scans are used. Furthermore, we propose an occupancy submap joining method, enabling more effective handling of large-scale problems by incorporating the submap joining process into the Occupancy-SLAM framework. Evaluations using simulations and practical 2D laser datasets demonstrate that the proposed approach can robustly obtain more accurate robot trajectories and occupancy maps than state-of-the-art techniques with comparable computational time. Preliminary results in the 3D case further confirm the potential of the proposed method in practical 3D applications, achieving more accurate results than existing methods. The code is made available to benefit the robotics community11https://github.com/WANGYINGYU/Occupancy-SLAM.

3D surface reconstruction in deformable environments presents significant challenges. Template-based methods have proven robust for achieving accurate reconstructions by utilising images and textured triangulated meshes as reference data. These methods rely on feature detection in both the reference and current images to establish corresponding points, leveraging reprojection and deformation constraints for precise reconstruction. However, challenges arise when features are not uniformly distributed across mesh triangles, potentially resulting in sparse or coarse reconstructions. Moreover, the combined computational cost of reprojection and deformable constraints often leads to prolonged optimisation times. This study aims to enhance efficiency in reconstructing deformations within the field of view. Our approach involves back-projecting vertices from a reference mesh onto the reference image plane and subsequently tracking them directly in the subsequent image. This method assumes the resulting observations are sufficiently accurate, encoding the deformation within this information. By eliminating the re-projection constraint and focusing solely on a deformation constraint based on Euclidean distances between vertices, we significantly reduce computational and memory costs. The results of our proposed algorithm demonstrate a notable reduction in computational cost and memory cost, while maintaining reconstruction accuracy comparable to related methods. The code of our algorithm is publicly available at https://github.com/DominikSlomma/Efficient-and-Accurate-Template-based-Reconstruction-of-Deformable-Surfaces

Optimizing robot poses and the map simultaneously has been shown to provide more accurate SLAM results. However, for non-feature based SLAM approaches, directly optimizing all the robot poses and the whole map will greatly increase the computational cost, making SLAM problems difficult to solve in large-scale environments. To solve the 2D non-feature based SLAM problem in large-scale environments more accurately and efficiently, we propose the grid-based submap joining method. Specifically, we first formulate the 2D grid-based submap joining problem as a non-linear least squares (NLLS) form to optimize the global occupancy map and local submap frames simultaneously. We then prove that in solving the NLLS problem using Gauss-Newton (GN) method, the increments of the poses in each iteration are independent of the occupancy values of the global occupancy map. Based on this property, we propose a pose-only GN algorithm equivalent to full GN method to solve the NLLS problem. The proposed submap joining algorithm is very efficient due to the independent property and the pose-only solution. Evaluations using simulations and publicly available practical 2D laser datasets confirm the outperformance of our proposed method compared to the state-of-the-art methods in terms of efficiency and accuracy, as well as the ability to solve the grid-based SLAM problem in very large-scale environments.

The 3D reconstruction of patient specific bone models plays a crucial role in orthopaedic surgery for clinical evaluation, surgical planning and precise implant design or selection. This paper considers the problem of reconstructing a patient-specific 3D tibia and fibula model from only two 2D X-ray images and one 3D general model segmented from the lower leg CT scans of one randomly selected patient. Currently, the bone 3D reconstruction mainly relies on computed tomography (CT) and magnetic resonance imaging (MRI) scanning-based mode segmentation which result in high radiation exposure or expensive costs. While, the proposed algorithm can accurately and efficiently deform a 3D general model to achieve a patient-specific 3D model that matches the patient's tibia and fibula projections in two 2D X-rays. The algorithm undergoes a preliminary deformation, 2D contour registration, and opti-misation based on the deformation graph that represents the shape deformation of models. Evaluations using simulations, cadaver and in-vivo experiments demonstrate that the proposed algorithm can effectively reconstruct the patient's 3D tibia and fibula surface model with high accuracy.

Common medical image segmentation tasks require large training datasets with pixel-level annotations which are very expensive and time-consuming to prepare. To overcome such limitation and achieve the desired segmentation accuracy, a novel Weakly-Interactive-Mixed Learning (WIML) framework is proposed by efficiently using weak labels. On one hand, utilize weak labels to reduce annotation time for high-quality strong labels by designing a Weakly-Interactive Annotation (WIA) part of the WIML which prudently introduces interactive learning into the weakly-supervised segmentation strategy. On the other hand, utilize weak labels and very few strong labels to achieve desired segmentation accuracy by designing a Mixed-Supervised Learning (MSL) part of the WIML which can boost the segmentation accuracy by providing strong prior knowledge during training. Besides, a multi-task Full-Parameter-Sharing Network (FPSNet) is proposed to better implement this framework. Specifically, to further reduce annotation time, attention modules (scSE) are integrated into FPSNet to improve the class activation map (CAM) performance for the first time. To further improve segmentation accuracy, a Full-Parameter-Sharing (FPS) strategy is designed in FPSNet to alleviate the overfitting of the segmentation task supervised by very few strong labels. The proposed method is validated on the BraTS 2019 and LiTS 2017 datasets, and experiments demonstrate that the proposed method WIML-FPSNet outperforms several state-of-the-art segmentation methods with minimal annotation efforts.

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.

Bundle Adjustment (BA) takes a crucial part in Structure from Motion (SfM) which refines a visual reconstruction by optimizing the camera poses and feature positions. The performance of BA can differ depending on the parametrization methods. This paper evaluates two bundle adjustment techniques using standard BA function from MATLAB and Parallax BA. The two BA techniques are compared using data from the “Starry Night” and “MALAGA Parking-6L” with different initial inputs. The accuracy and convergence properties of the two BA methods have been evaluated. The effect of the different parameterization techniques and initial information was also analyzed. In most cases, the results of Parallax BA show better accuracy with lower final reprojection error and are less sensitive to the initialization values. It is evaluated that the parallax angle avoids the singularity issue commonly found in Standard BA, which shows that Parallax BA outperforms Standard BA. Furthermore, visual-inertial SLAM (VI-SLAM), based on Parallax BA, has been presented. It is much more reliable than a pure-vision system, showing further improved performance in terms of robustness and accuracy, even with less feature observation. The open-source code can be found in: https://github.com/uts-hb/ParallaxBA.git

This paper considers active SLAM problem for 3D deformable environments where the trajectory of the robot is planned to optimize the SLAM results. A planning strategy combining an efficient global planner with an accurate local planner is proposed to solve the problem. Simulation results under different scenarios have shown that the proposed active SLAM algorithm provides a good balance between accuracy and efficiency as compared to the local planner and the global planner. The MATLAB code of this first active SLAM algorithm for 3D deformable environments is made publicly available4.

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.

In colonoscopy procedures, it is important to rebuild and visualize the colonic surface to minimize the missing regions and reinspect for abnormalities. Due to the fast camera motion and deformation of the colon in standard forward-viewing colonoscopies, traditional simultaneous localization and mapping (SLAM) systems work poorly for 3D reconstruction of colon surfaces and are prone to severe drift. Thus in this paper, a preoperative colon model segmented from CT scans is used together with the colonoscopic images to achieve the 3D colon reconstruction. The proposed framework includes dense depth estimation from monocular colonoscopic images using a deep neural network (DNN), visual odometry (VO) based camera motion estimation and an embedded deformation (ED) graph based non-rigid registration algorithm for deforming 3D scans to the segmented colon model. A realistic simulator is used to generate different simulation datasets with ground truth. Simulation results demonstrate the good performance of the proposed 3D colonic surface reconstruction method in terms of accuracy and robustness. In-vivo experiments are also conducted and the results show the practicality of the proposed framework for providing useful shape and texture information in colonoscopy applications.

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.

In this paper, we propose a novel algorithm for fusing a sequence of 3D images, named as Direct Bundle Adjustment (DBA). This algorithm simultaneously optimizes the global pose parameters of image frames and the intensity values of the fused global image using the 3D image data directly (without extracting features from the images). This one-step 3D image fusion approach is achieved by formulating the problem as an optimization problem to minimize the intensity differences between the global image and the corresponding points in the different local images. The proposed DBA method is particularly useful in the scenarios where distinct features are not available, such as Transesophageal Echocardiography (TEE) images. We validate the proposed method via simulated and in-vivo 3D TEE images. It is shown that the proposed method is robust to intensity noises and much more accurate than the conventional sequential fusion method.

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.

Embedded deformation graph is a widely used technique in deformable geometry and graphical problems. Although the technique has been transmitted to stereo (or RGB-D) camera based SLAM applications, it remains challenging to compromise the computational cost as the model grows. In practice, the processing time grows rapidly in accordance with the expansion of maps. In this paper, we propose an approach to decouple the nodes of deformation graph in large scale dense deformable SLAM and keep the estimation time to be constant. We observe that only partial deformable nodes in the graph are connected to visible points. Based on this fact, the sparsity of the original Hessian matrix is utilized to split the parameter estimation into two independent steps. With this new technique, we achieve faster parameter estimation with amortized computation complexity reduced from O(n2) to almost O(1). As a result, the computational cost barely increases as the map keeps growing. Based on our strategy, the computational bottleneck in large scale embedded deformation graph based applications will be greatly mitigated. The effectiveness is validated by experiments, featuring large scale deformation scenarios.

Current clinical endovascular interventions rely on 2D guidance for catheter manipulation. Although an aortic 3D surface is available from the pre-operative CT/MRI imaging, it cannot be used directly as a 3D intra-operative guidance since the vessel will deform during the procedure. This paper aims to reconstruct the live 3D aortic deformation by fusing the static 3D model from the pre-operative data and the 2D live imaging from fluoroscopy. In contrast to some existing deformation reconstruction frameworks which require 3D observations such as RGB-D or stereo images, fluoroscopy only presents 2D information. In the proposed framework, a 2D-3D registration is performed and the reconstruction process is formulated as a non-linear optimization problem based on the deformation graph approach. Detailed simulations and phantom experiments are conducted and the result demonstrates the reconstruction accuracy and robustness, as well as the potential clinical value of this framework.

Autonomous mobile vehicles are expected to perform persistent and accurate localization with low-cost equipment. To achieve this goal, we propose a stereo camera based visual localization method using a modified laser map, which takes the advantage of both the low cost of camera, and high geometric precision of laser data to achieve long-term performance. Considering that LiDAR and camera give measurements of the same environment in different modalities, the cross-modal invariance is investigated to modify the laser map for visual localization. Specifically, a map learning algorithm is introduced to sample the robust subsets in laser maps that are useful for visual localization using multi-session visual and laser data. Further, a generative map model is derived to describe this cross-modal invariance, based on which two types of measurements are defined to model the laser map points as appropriate visual observations. Tightly coupling these measurements within the local bundle adjustment during online sliding-window based visual odometry, the vehicle can achieve robust localization even one year after the map was built. The effectiveness of the proposed method is evaluated on both the public KITTI datasets and self-collected datasets in our campus, which include seasonal, illumination and object variations. On all experimental localization sessions, our method provides satisfactory results, even when the direction is opposite to that in the mapping session, verifying the superior performance of the laser map based visual localization method.

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).

This paper presents a linear approach for solving monocular simultaneous localization and mapping (SLAM) problems. The algorithm first builds a sequence of small initial submaps and then joins these submaps together in a divide-and-conquer (D&C) manner. Each of the initial submap is built using three monocular images by bundle adjustment (BA), which is a simple nonlinear optimization problem. Each step in the D&C submap joining is solved by a linear least squares together with a coordinate and scale transformation. Since the only nonlinear part is in the building of the initial submaps, the algorithm makes it possible to solve large-scale monocular SLAM while avoiding issues associated with initialization, iteration, and local minima that are present in most of the nonlinear optimization based algorithms currently used for large-scale monocular SLAM. Experimental results based on publically available datasets are used to demonstrate that the proposed algorithms yields solutions that are very close to those obtained using global BA starting from good initial guess.

This paper presents a strategy for large-scale SLAM through solving a sequence of linear least squares problems. The algorithm is based on submap joining where submaps are built using any existing SLAM technique. It is demonstrated that if submaps coordinate frames are judiciously selected, the least squares objective function for joining two submaps becomes a quadratic function of the state vector. Therefore, a linear solution to large-scale SLAM that requires joining a number of local submaps either sequentially or in a more efficient Divide and Conquer manner, can be obtained. The proposed Linear SLAM technique is applicable to both feature-based and pose graph SLAM, in two and three dimensions, and does not require any assumption on the character of the covariance matrices or an initial guess of the state vector. Although this algorithm is an approximation to the optimal full nonlinear least squares SLAM, simulations and experiments using publicly available datasets in 2D and 3D show that Linear SLAM produces results that are very close to the best solutions that can be obtained using full nonlinear optimization started from an accurate initial value. The C/C++ and MATLAB source codes for the proposed algorithm are available on OpenSLAM.

This paper compares the convergence of least squares based 2D bearing-only SLAM algorithms using different landmark parametrizations. It is shown that the requirement on the accuracy of the initial value vary significantly when using different landmark parametrizations. Especially, for small scale bearing-only SLAM problems, the region of attraction of the global minimum for Gauss-Newton iteration based bearing-only SLAM algorithm using parallax angle landmark parametrization is significantly larger as compared with those of bearing-only SLAM algorithms using other landmark parametrizations.

Self-localization of ground vehicles in densely populated urban environments poses a significant challenge. The presence of tall buildings in close proximity to traversable areas limits the use of GPS-based positioning techniques in such environments. This paper presents an approach to global localization on a hybrid metric-topological map using a monocular camera and wheel odometry. The global topology is built upon spatially separated reference places represented by local image features. In contrast to other approaches we employ a feature selection scheme ensuring a more discriminative representation of reference places while simultaneously rejecting a multitude of features caused by dynamic objects. Through fusion with additional local cues the reference places are assigned discrete map positions allowing metric localization within the map. The self-localization is carried out by associating observed visual features with those stored for each reference place. Comprehensive experiments in a dense urban environment covering a time difference of about 9 months are carried out. This demonstrates the robustness of our approach in environments subjected to high dynamic and environmental changes.

Recently RGB-D sensors have become very popular in the area of Simultaneous Localisation and Mapping (SLAM). The major advantage of these sensors is that they provide a rich source of 3D information at relatively low cost. Unfortunately, these sensors in their current forms only have a range accuracy of up to 4 metres. Many techniques which perform SLAM using RGB-D cameras rely heavily on the depth and are restrained to office type and geometrically structured environments. In this paper, a switching based algorithm is proposed to heuristically choose between RGB-BA and RGBD-BA based local maps building. Furthermore, a low cost and consistent optimisation approach is used to join these maps. Thus the potential of both RGB and depth image information are exploited to perform robust SLAM in more general indoor cases. Validation of the proposed algorithm is performed by mapping a large scale indoor scene where traditional RGB-D mapping techniques are not possible.

This paper presents a new unified feature parametrization approach for monocular SLAM. The parametrization is based on the parallax angle and can reliably represent both nearby and distant features, as well as features in the direction of camera motion and features observed only once. A new bundle adjustment (BA) algorithm using the proposed parallax angle parametrization is developed and shown to be more reliable as compared with existing BA algorithms that use Euclidean XYZ or inverse depth parametrizations. A new map joining algorithm that allows combining a sequence of local maps generated using BA with the proposed parametrization, that avoids the large computational cost of a global BA, and can automatically optimize the relative scales of the local maps without any loss of information, is also presented. Extensive simulations and a publicly available large-scale real dataset with centimeter accuracy ground truth are used to demonstrate the accuracy and consistency of the BA and map joining algorithms using the new parametrization. Especially, since the relative scales are optimized automatically in the proposed BA and map joining algorithms, there is no need to compute any relative scales even for a loop more than 1km.

This paper first demonstrates an interesting property of bundle adjustment (BA), “scale drift correction”. Here “scale drift correction” means that BA can converge to the correct solution (up to a scale) even if the initial values of the camera pose translations and point feature positions are calculated using very different scale factors. This property together with other properties of BA makes it the best approach for monocular Simultaneous Localization and Mapping (SLAM), without considering the computational complexity. This naturally leads to the idea of using local BA and map joining to solve large-scale monocular SLAM problem, which is proposed in this paper. The local maps are built through Scale-Invariant Feature Transform (SIFT) for feature detection and matching, random sample consensus (RANSAC) paradigm at different levels for robust outlier removal, and BA for optimization. To reduce the computational cost of the large-scale map building, the features in each local map are judiciously selected and then the local maps are combined using a recently developed 3D map joining algorithm. The proposed large-scale monocular SLAM algorithm is evaluated using a publicly available dataset with centimeter-level ground truth.