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

When performing total hip replacement (THR) surgery, high-quality preparation of acetabulum is critical as it contributes to the patient’s recovery speed and the consistency of bone ingrowth. Conventionally, surgeons prepare the acetabulum manually by reaming it with a handheld electric drill and a reamer. It not only increases the surgeon’s workload but more importantly, it is difficult to control the reaming depth and direction accurately. Utilizing an admittance-controlled (AC) collaborative robot (cobot) to enable physical human-robot collaboration (pHRC) possesses a promising solution. For primitive AC, a compromise must be made between compliance and task accuracy. In this paper, we present a novel variable admittance control (VAC) design that considers the reactive force of bone while ensuring the passivity and stability of the system during pHRC-assisted acetabular preparation. The qualitative results show that VAC was more desirable by users than the conventional manual reaming method. Compared to other pHRC controls, quantitative results on user energy consumption, reaming error, and smoothness showed the proposed VAC can achieve a balance between physical workload and acetabular quality. Compared to manual reaming, VAC reduced the reaming error by 67.47% and improved the final acetabulum surface smoothness by 18.30%.

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.

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 proposes a novel hierarchical methodology to planning safe UAV trajectories in complex environments. We start by improving a canonical hybrid A* in relation to high memory requirements, performance degradation, and the low efficiency customarily observed in the initial global trajectory suggested by the planner. Then, the Marden theorem is applied - for the first time in local path planning - to generate continuous, non-intersecting, enclosed, and safe flight corridors, termed homotopic enclosed safe motion corridors (HESMCs) hereafter. This is efficiently realized through a series of unique ellipsoids along the initial route. Meanwhile, the optimized motion trajectory along the corridors is built by considering two waypoints and prescribed performance functions. The resolved path is safe and complete, with a comprehensive Lyapunov stability analysis included to ensure accurate and efficient trajectory tracking. The simulation and physical tests demonstrate the superiority of our proposed planner over existing state-of-the-art methods, with consistent and significant improvements in processing time and guaranteed completeness. Note to Practitioners—The authors perceived the contribution of the manuscript of particular relevance to users of UAVs seeking advanced safety in their guidance and navigational solutions, offering a blend of theoretical innovation and practical applicability. The work introduces a distinct hierarchical motion planner specifically designed to enhance safety and reliability in UAV navigation. Key to this is the development of an improved hybrid A* algorithm for global planning, which effectively tackles practical issues such as high memory consumption and performance degradation. A significant theoretical contribution is the application of the Marden theorem in local optimization. This facilitates the generation of homotopic enclosed motion corridors using unique safe boundary ellipsoids, thus reducing navigation complexity and the risk of failure during task execution. Additionally, the proposed scheme emphasizes the generation of motion trajectories considering position errors and prescribed performance functions, supplemented by a thorough Lyapunov stability analysis. Looking ahead, we aim to extend the proposed scheme in the context of UAV swarms for more efficient navigation in complex environments.

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.

Global localization can estimate geo-referenced locations (e.g., longitude and latitude), which is a fundamental capability for autonomous vehicles. Most existing solutions rely on the Global Navigation Satellite Systems (GNSS). Their accuracy could be degraded by the multi-path effects or occlusions of GNSS signals in urban environments. Some GNSS-free methods could achieve global localization by comparing the current on-line sensory data with pre-built databases/maps. However, they require tedious human efforts to drive a vehicle to collect and maintain the databases/maps. Moreover, most of these methods use front-looking cameras or LiDARs, so the captured data could be easily contaminated by dynamic objects (e.g., moving vehicles and pedestrians). To provide a solution to these problems, this paper proposes a novel global localization method by comparing an image from a sky-looking fish-eye camera with the publicly available OpenStreetMap (OSM), and using particle filter to achieve real-time metric localization in dynamic traffic environments. To evaluate our method, we extend a public dataset with OSM data, which are retrieved through the given geo-referenced location information. Experimental results demonstrate the effectiveness and efficiency of our method.

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.

Robot audition systems with multiple microphone arrays have many applications in practice. However, the accurate calibration of multiple microphone arrays remains challenging because there are many unknown parameters to be identified, including the relative transforms (i.e., orientation and translation) and asynchronous factors (i.e., initial time offset and sampling clock difference) between microphone arrays. To tackle these challenges, in this article, we adopt batch simultaneous localization and mapping (SLAM) for joint calibration of multiple asynchronous microphone arrays and sound source localization. Using the Fisher information matrix (FIM) approach, we first conduct the observability analysis (i.e., parameter identifiability) of the abovementioned calibration problem and establish necessary/sufficient conditions under which the FIM and the Jacobian matrix have full column rank, which implies the identifiability of the unknown parameters. We also discover several scenarios where the unknown parameters are not uniquely identifiable. Subsequently, we propose an effective framework to initialize the unknown parameters, which is used as the initial guess in batch SLAM for multiple microphone array calibration, aiming to further enhance optimization accuracy and convergence. Extensive numerical simulations and real experiments have been conducted to verify the performance of the proposed method. The experimental results show that the proposed pipeline achieves higher accuracy with fast convergence in comparison to methods that use the noise-corrupted ground truth of the unknown parameters as the initial guess in the optimization and other existing frameworks.

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.

Sensor arrays generating differential received signal strength (DRSS) measurements have found many applications in robotics. However, accurate calibration of these sensor arrays remains a challenge. Most existing methods are impractical in that they assume to know signal source positions or certain parameters (i.e., path loss exponent), and try to estimate the others. In this paper, we adopt graph simultaneous localization and mapping (SLAM) as a general framework for jointly estimating the source positions and parameters of the DRSS sensor array. Our contributions are twofold. On the one hand, by using a Fisher information matrix approach, we conduct a systematic observability analysis of the corresponding SLAM setup for the calibration problem. On the other hand, we propose an effective procedure to select the initial value which is fed to Levenberg-Marquardt iterations for further improving optimization accuracy and convergence. Extensive simulation and hardware experiments show that the proposed method renders high-quality calibration results. All the codes and data are publicly available at https://github.com/SUSTech2022/DRSS-sensor-array-calibration.

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 focuses on designing a consistent and efficient filter for visual-inertial localization given a prebuilt map. First, we propose a new Lie group with its algebra based on which a novel invariant extended Kalman filter (invariant EKF) is designed. We theoretically prove that, when we do not consider the uncertainty of map information, the proposed invariant EKF is able to naturally preserve the correct observability properties of the system. To consider the uncertainty of map information, we introduce a Schmidt filter. With the Schmidt filter, the uncertainty of map information can be taken into consideration to avoid overconfident estimation while the computation cost only increases linearly with the size of the map keyframes. In addition, we introduce an easily implemented observability-constrained technique because directly combining the invariant EKF with the Schmidt filter cannot maintain the correct observability properties of the system that considers the uncertainty of map information. Finally, we validate our proposed system's high consistency, accuracy, and efficiency via extensive simulations and real-world experiments.

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.

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.

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.

Large scale 3D maps constructed via LiDAR sensor are widely used on intelligent vehicles for localization in outdoor scenes. However, loading, communication and processing of the original dense maps are time consuming for onboard computing platform, which calls for a more concise representation of maps to reduce the complexity but keep the performance of localization. In this paper, we propose a teacher-student learning paradigm to compress the 3D point cloud map. Specifically, we first find a subset of LiDAR points with high number of observations to preserve the localization performance, which is regarded as the teacher of map compression. An efficient optimization strategy is proposed to deal with the massive data in original map. With the supervision of compressed map, a student model is built by training a random forest model fed with geometric feature descriptors of each point. As a result, the student model is able to compress the map without referring to the expensive numerical optimization. Additionally, by incorporating the features, the innovative student model can be generalized to other new maps while no re-training is required. We conduct thorough experiments on multi-session dataset and KITTI dataset to demonstrate the effectiveness and efficiency of the proposed learning paradigm, and the comparison with other map compression methods. The final results show that the learned student model can achieve efficient map compression with comparable LiDAR based localization performance to the original map at the same time.

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 article, we present inertial lidar localization autocalibration and mapping: an offline probabilistic framework for localization, mapping, and extrinsic calibration based on a 3-D lidar and a six-degree-of-freedom inertial measurement unit. Most of today's lidars collect geometric information about the surrounding environment by sweeping lasers across their field of view. Consequently, 3-D points in one lidar scan are acquired at different timestamps. If the sensor trajectory is not accurately known, the scans are affected by the phenomenon known as motion distortion. The proposed method leverages preintegration with a continuous representation of the inertial measurements to characterize the system's motion at any point in time. It enables precise correction of the motion distortion without relying on any explicit motion model. The system's pose, velocity, biases, and time shift are estimated via a full batch optimization that includes automatically generated loop closure constraints. The autocalibration and the registration of lidar data rely on planar and edge features matched across pairs of scans. The performance of the framework is validated through simulated and real-data experiments.

Sensor array-based systems, which adopt time difference of arrival (TDOA) measurements among the sensors, have found many robotic applications. However, for existing frameworks and systems to be useful, the sensor array needs to be calibrated accurately. Of particular interest in this article are microphone array-based robot audition systems. In our recent work, by using a moving sound source, and the graph-based formulation of simultaneous localization and mapping (SLAM), we have proposed a framework for joint sound source localization and calibration of microphone array geometrical information, together with the estimation of microphone time offset and clock difference/drift rates. However, a thorough study on the identifiability question, termed observability analysis here, in the SLAM framework for microphone array calibration and sound source localization, is still lacking in the literature. In this article, we will fill the abovementioned gap via a Fisher information matrix approach. Motivated by the equivalence between the full column rankness of the Fisher information matrix and the Jacobian matrix, we leverage the structure of the latter associated with the SLAM formulation, and present necessary and sufficient conditions guaranteeing its full column rankness, which lead to parameter identifiability. We have thoroughly discussed the 3-D case with asynchronous (with both time offset and clock drifts, or with only one of them) and synchronous microphone array, respectively. These conditions are closely related to the motion varieties of the sound source and the microphone array configuration, and have intuitive and physical interpretations. Based on the established conditions, we have also discovered some particular cases where observability is impossible. Connections with calibration of other sensors will also be discussed, amongst others. To our best knowledge, this is the first systematic work on observability analysis of SLAM-based microphone array calibration and sound source localization. The tools and concepts used in this article are also applicable to other TDOA sensing modalities such as ultrawide band (UWB) sensors.

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.

Map based visual inertial localization is a crucial step to reduce the drift in state estimation of mobile robots. The underlying problem for localization is to estimate the pose from a set of 3D-2D feature correspondences, of which the main challenge is the presence of outliers, especially in changing environment. In this paper, we propose a robust solution based on efficient global optimization of the consensus maximization problem, which is insensitive to high percentage of outliers. We first introduce translation invariant measurements (TIMs) for both points and lines to decouple the consensus maximization problem into rotation and translation subproblems, allowing for a two-stage solver with reduced search space. Then we show that (i) the rotation can be estimated by minimizing TIMs using only 1-dimensional branch-and-bound (BnB), (ii) the translation can be estimated by running 1-dimensional search for each of the three axes with prioritized progressive voting. Compared with the popular randomized solver, our solver achieves deterministic global convergence without requiring an initial value. Furthermore, ours is exponentially faster compared with existing BnB based methods. Finally, our experiments on both simulation and real-world datasets demonstrate that the proposed method gives accurate pose estimation even in the presence of 90% outliers (only 2 inliers).

Global localization in 3D point clouds is a challenging task for mobile vehicles in outdoor scenarios, which requires the vehicle to localize itself correctly in a given map without prior knowledge of its pose. This is a critical component of autonomous vehicles or robots on the road for handling localization failures. In this paper, based on reduced dimension scan representations learned from neural networks, a solution to global localization is proposed by achieving place recognition first and then metric pose estimation in the global prior map. Specifically, we present a semi-handcrafted feature learning method for 3D Light detection and ranging (LiDAR) point clouds using artificial statistics and siamese network, which transforms the place recognition problem into a similarity modeling problem. Additionally, the sensor data using dimension reduced representations require less storage space and make the searching easier. With the learned representations by networks and the global poses, a prior map is built and used in the localization framework. In the localization step, position only observations obtained by place recognition are used in a particle filter algorithm to achieve precise pose estimation. To demonstrate the effectiveness of our place recognition and localization approach, KITTI benchmark and our multi-session datasets are employed for comparison with other geometric-based algorithms. The results show that our system can achieve both high accuracy and efficiency for long-term autonomy.

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

In this paper, we introduce a probabilistic framework for INertial Lidar Localisation And MApping (IN2LAMA). Most of today's lidars are based on spinning mechanisms that do not capture snapshots of the environment. As a result, movement of the sensor can occur while scanning. Without a good estimation of this motion, the resulting point clouds might be distorted. In the lidar mapping literature, a constant velocity motion model is commonly assumed. This is an approximation that does not necessarily always hold. The key idea of the proposed framework is to exploit preintegrated measurements over upsampled inertial data to handle motion distortion without the need for any explicit motion-model. It tightly integrates inertial and lidar data in a batch on-manifold optimisation formulation. Using temporally precise upsampled preintegrated measurement allows frame-to-frame planar and edge features association. Moreover, features are re-computed when the estimate of the state changes, consolidating front-end and back-end interaction. We validate the effectiveness of the approach through simulated and real data.

Underwater robots are subject to position drift due to the effect of ocean currents and the lack of accurate localisation while submerged. We are interested in exploiting such position drift to estimate the ocean current in the surrounding area, thereby assisting navigation and planning. We present a Gaussian process (GP)-based expectation-maximisation (EM) algorithm that estimates the underlying ocean current using sparse GPS data obtained on the surface and dead-reckoned position estimates. We first develop a specialised GP regression scheme that exploits the incompressibility of ocean currents to counteract the underdetermined nature of the problem. We then use the proposed regression scheme in an EM algorithm that estimates the best-fitting ocean current in between each GPS fix. The proposed algorithm is validated in simulation and on a real dataset, and is shown to be capable of reconstructing the underlying ocean current field. We expect to use this algorithm to close the loop between planning and estimation for underwater navigation in unknown ocean currents.

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.

In this paper, we present a probabilistic framework to recover the extrinsic calibration parameters of a lidar-IMU sensing system. Unlike global-shutter cameras, lidars do not take single snapshots of the environment. Instead, lidars collect a succession of 3D-points generally grouped in scans. If these points are assumed to be expressed in a common frame, this becomes an issue when the sensor moves rapidly in the environment causing motion distortion. The fundamental idea of our proposed framework is to use preintegration over interpolated inertial measurements to characterise the motion distortion in each lidar scan. Moreover, by using a set of planes as a calibration target, the proposed method makes use of lidar point-to-plane distances to jointly calibrate and localise the system using on-manifold optimisation. The calibration does not rely on a predefined target as arbitrary planes are detected and modelled in the first lidar scan. Simulated and real data are used to show the effectiveness of the proposed method.

Robust online incremental SLAM applications require metrics to evaluate the impact of current measurements. Despite its prevalence in graph pruning, information-theoretic metrics solely are insufficient to detect outliers. The optimal value of the objective function is a better choice to detect outliers but cannot be computed unless the problem is solved. In this paper, we show how the objective function change can be predicted in an incremental pose-graph optimization scheme, without actually solving the problem. The predicted objective function change can be used to guide online decisions or detect outliers. Experiments validate the accuracy of the predicted objective function, and an application to outlier detection is also provided, showing its advantages over M-estimators.

We consider a multi-robot variant of the mission monitoring problem. This problem arises in tasks where a robot observes the progress of another robot that is stochastically following a known trajectory, among other applications. We formulate and solve a variant where multiple tracker robots must monitor a single target robot, which is important because it enables the use of multi-robot systems to improve task performance in practice, such as in marine robotics missions. Our algorithm coordinates the behaviour of the trackers by computing optimal single-robot paths given a probabilistic representation of the other robots' paths. We employ a decentralised scheme that optimises over probability distributions of plans and has useful analytical properties. The planned trajectories collectively maximise the probability of observing the target throughout the mission with respect to probabilistic motion and observation models. We report simulation results for up to 8 robots that support our analysis and indicate that our algorithm is a feasible solution for improving the performance of mission monitoring systems.

Device-free localization (DFL) is an emerging wireless network target localization technique that does not need to attach any electronic device with the target. It is remaining as a challenging research problem due to the weak wireless signals and the uncertain wireless communication environment. In this paper, a novel Gaussian Process (GP) based wireless propagation model is proposed to describe the likelihood relationship between the target location and the changes of the RSS measurement for a wireless link. Sequentially Particle Filter (PF) is applied to the DFL for estimating the location of the target, after the GP model is trained using the experimental measurements of the link. Experimental results demonstrate that the proposed GP-PF algorithm can track the target with much better localization accuracy than the Support Vector Machine (SVM) based PF approach.

The main contribution of this paper is an invariant extended Kalman filter (EKF) for visual inertial navigation systems (VINS). It is demonstrated that the conventional EKF based VINS is not invariant under the stochastic unobservable transformation, associated with a translation and a rotation about the gravitational direction. This can lead to inconsistent state estimates as the estimator does not obey a fundamental property of the physical system. To address this issue, we use a novel uncertainty representation to derive a Right Invariant error extended Kalman filter (RIEKF-VINS) that preserves this invariance property. RIEKF-VINS is then adapted to the multi-state constraint Kalman filter framework to obtain a consistent state estimator. Both Monte Carlo simulations and real-world experiments are used to validate the proposed method.

© 2016 IEEE. The simultaneous localization and mapping (SLAM) problem has been a research focus for many years and have reached a mature state. However, more robust solutions to the SLAM problem are still required, especially in large noise level scenarios. Because of the strong non-linearity of the SLAM problem, it is vital to start from a good initial value to avoid being trapped in local minima. In this paper, we propose a new SLAM formulation transforming the unconstrained Least Squares formulation into a constrained optimization problem. Algorithms based on this new formulation can naturally start from good initial value. Different from other constrained optimization problem, this new formulation can be efficiently solved with Sequential Quadratic Programming (SQP) methods. Based on SQP, we propose an incremental SQP algorithm to solve SLAM, which shows great advantage over Gauss Newton (g2o implementation) when working in large noise level scenarios. Experimental results show the validity of the proposed approach.

The main contribution of this paper is a high frequency, low-complexity, on-board visual-inertial odometry system for quadrotor micro air vehicles. The system consists of an extended Kalman filter (EKF) based state estimation algorithm that fuses information from a low cost MEMS inertial measurement unit acquired at 200Hz and VGA resolution images from a monocular camera at 50Hz. The dynamic model describing the quadrotor motion is employed in the estimation algorithm as a third source of information. Visual information is incorporated into the EKF by enforcing the epipolar constraint on features tracked between image pairs, avoiding the need to explicitly estimate the location of the tracked environmental features. Combined use of the dynamic model and epipolar constraints makes it possible to obtain drift free velocity and attitude estimates in the presence of both accelerometer and gyroscope biases. A strategy to deal with the unobservability that arises when the quadrotor is in hover is also provided. Experimental data from a real-time implementation of the system on a 50 gram embedded computer are presented in addition to the simulations to demonstrate the efficacy of the proposed system.

Simultaneous localization and mapping (SLAM) in robotics, and a number of related problems that arise in sensor networks are instances of estimation problems over weighted graphs. This paper studies the relation between the graphical representation of such problems and estimationtheoretic concepts such as the Cramér-Rao lower bound (CRLB) and D-optimality. We prove that the weighted number of spanning trees, as a graph connectivity metric, is closely related to the determinant of CRLB. This metric can be efficiently computed for large graphs by exploiting the sparse structure of underlying estimation problems. Our analysis is validated using experiments with publicly available pose-graph SLAM datasets.

We propose a scalable algorithm to take advantage of the separable structure of simultaneous localization and mapping (SLAM). Separability is an overlooked structure of SLAM that distinguishes it from a generic nonlinear least-squares problem. The standard relative-pose and relative-position measurement models in SLAM are affine with respect to robot and features' positions. Therefore, given an estimate for robot orientation, the conditionally optimal estimate for the rest of the state variables can be easily computed by solving a sparse linear least-squares problem. We propose an algorithm to exploit this intrinsic property of SLAM by stripping the problem down to its nonlinear core, while maintaining its natural sparsity. Our algorithm can be used in conjunction with any Newton-based solver and is applicable to 2-D/3-D pose-graph and feature-based SLAM. Our results suggest that iteratively solving the nonlinear core of SLAM leads to a fast and reliable convergence as compared to the state-of-the-art sparse back-ends.

There are many benefits for the deployment of multiple autonomous industrial robots to carry out a task, particularly if the robots act in a highly collaborative manner. Collaboration can be possible when each robot is able to autonomously explore the environment, localize itself, create a map of the environment and communicate with other robots. This paper presents an approach to the modeling of the collaboration problem of multiple robots determining optimal base positions and orientations in an environment by considering the team objectives and the information shared amongst the robots. It is assumed that the robots can communicate so as to share information on the environment, their operation status and their capabilities. The approach has been applied to a team of robots that are required to perform complete surface coverage tasks such as grit-blasting and spray painting in unstructured environments. Case studies of such applications are presented to demonstrate the effectiveness of the approach.

Building and using maps is a fundamental issue for bionic robots in field applications. A dense surface map, which offers rich visual and geometric information, is an ideal representation of the environment for indoor/outdoor localization, navigation, and recognition tasks of these robots. Since most bionic robots can use only small light-weight laser scanners and cameras to acquire semi-dense point cloud and RGB images, we propose a method to generate a consistent and dense surface map from this kind of semi-dense point cloud and RGB images. The method contains two main steps: (1) generate a dense surface for every single scan of point cloud and its corresponding image(s) and (2) incrementally fuse the dense surface of a new scan into the whole map. In step (1) edge-aware resampling is realized by segmenting the scan of a point cloud in advance and resampling each sub-cloud separately. Noise within the scan is reduced and a dense surface is generated. In step (2) the average surface is estimated probabilistically and the non-coincidence of different scans is eliminated. Experiments demonstrate that our method works well in both indoor and outdoor semi-structured environments where there are regularly shaped objects.

When multiple industrial robots are deployed in field applications such as grit blasting and spray painting of steel bridges, the environments are unstructured for robot operation and the robot positions may not be arranged accurately. Coordination of these multiple robots to maximize productivity through area partitioning and allocation is crucial. This paper presents a novel approach to area partitioning and allocation by utilizing multiobjective optimization and voronoi partitioning. Multiobjective optimization is used to minimize: (1) completion time, (2) proximity of the allocated area to the robot, and (3) the torque experienced by each joint of the robot during task execution. Seed points of the voronoi graph for voronoi partitioning are designed to be the design variables of the multiobjective optimization algorithm. Results of three different simulation scenarios are presented to demonstrate the effectiveness of the proposed approach and the advantage of incorporating robots' torque capacity.

Based on the latest achievements in computer vision and RGB-D SLAM, a practical way for dense moving object segmentation and thus a new framework for robust dense RGB-D SLAM in challenging dynamic scenarios is put forward. As the state-of-the-art method in RGB-D SLAM, dense SLAM is very robust when there are motion blur or featureless regions, while most of those sparse feature-based methods could not handle them. However, it is very susceptible to dynamic elements in the scenarios. To enhance its robustness in dynamic scenarios, we propose to combine dense moving object segmentation with dense SLAM. Since the object segmentation results from the latest available algorithm in computer vision are not satisfactory, we propose some effective measures to improve upon them so that better results can be achieved. After dense segmentation of dynamic objects, dense SLAM can be employed to estimate the camera poses. Quantitative results from the available challenging benchmark dataset have proved the effectiveness of our method.

This paper considers path planning for underwater robot in navigation tasks. The main challenge is how to deal with uncertainties in the underwater environment such as motion model error and sensing error. To overcome this challenge, two high level control methods have been presented and compared, which are based on the Model Predictive Control (MPC) strategy and the Partially Observable Markov Decision Process (POMDP) model, respectively. Navigation time, collision frequency, energy consumption and accuracy in localization are used as the assessment criteria for the two methods. It is shown that the MPC-based method is more efficient for our application scenarios while the POMDP-based method can provide more robust solutions.

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.

SLAM can be viewed as an estimation problem over graphs. It is well known that the topology of each dataset affects the quality of the corresponding optimal estimate. In this paper we present a formal analysis of the impact of graph structure on the reliability of the maximum likelihood estimator. In particular, we show that the number of spanning trees in the graph is closely related to the D-optimality criterion in experimental design. We also reveal that in a special class of linear-Gaussian estimation problems over graphs, the algebraic connectivity is related to the E-optimality design criterion. Furthermore, we explain how the average node degree of the graph is related to the ratio between the minimum of negative log-likelihood achievable and its value at the ground truth. These novel insights give us a deeper understanding of the SLAM problem. Finally we discuss two important applications of our analysis in active measurement selection and graph pruning. The results obtained from simulations and experiments on real data confirm our theoretical findings.

In the state-of-the-art approaches to SLAM, the problem is often formulated as a non-linear least squares. SLAM back-ends often employ iterative methods such as Gauss-Newton or Levenberg-Marquardt to solve that problem. In general, there is no guarantee on the global convergence of these methods. The back-end might get trapped into a local minimum or even diverge depending on how good the initial estimate is. Due to the large noise in odometry data, it is not wise to rely on dead reckoning for obtaining an initial guess, especially in long trajectories. In this paper we demonstrate how M-estimation can be used as a bootstrapping technique to obtain a reliable initial guess. We show that this initial guess is more likely to be in the basin of attraction of the global minimum than existing bootstrapping methods. As the main contribution of this paper, we present new insights about the similarities between robustness against outliers and robustness against a bad initial guess. Through simulations and experiments on real data, we substantiate the reliability of our proposed method.

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.

A multiobjective optimization model is presented in this paper for the Autonomous Straddle Carriers Scheduling (ASCS) problem in automated container terminals, which is more practical than the single objective model. The model considers three objectives [i.e., Straddle Carriers (SCs) traveling time, SC waiting time and finishing time of high-priority container-transferring jobs], and their weighted sum is investigated as the representative example. The presented model is formulated as a pickup and delivery problem with time windows in the form of binary integer programming. An exact algorithm based on Branch-and-Bound with Column Generation (BBCG) is employed for solving the multiobjective ASCS problem. Based on the map of an actual fully automated container terminal, simulation results are compared with the single-objective scheduling to demonstrate the effectiveness and flexibility of the presented multiobjective model, as well as the efficacy of the BBCG algorithm for autonomous SC scheduling.

This paper presents a new state vector and a map joining algorithm for range-only SLAM problems. Local maps are built by least squares optimization using the new state vector and a landmark initialization strategy which is an improvement on our preliminary work [1]. The map joining algorithm combines the local maps using least squares optimization to maintain the estimation consistency. Both the local map building and the map joining algorithm maintain a list of “unused range observations” to minimize the potential for information loss. The accuracy of the proposed method is evaluated using a simulation dataset, and an experimental dataset provided by the Robotics Institute at Carnegie Mellon University (CMU).

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.

This paper presents the development of a low-cost vision-based robotic wheelchair system towards autonomous convoying. The non-holonomic follower vehicle obtains visual real-time pose data of a known coplanar target installed on the back of the leading vehicle. This allows the tracking vehicle to mimic the path of the preceding vehicle, while maintaining a safe distance behind it with the aid of a controller based on the robot's kinematics constraints. A back-end visual filter is proposed in the planning strategy to overcome the noisy environmental information acquired from the camera as it tracks the vehicle in front. The effectiveness of the approach is evaluated in an indoor setting using data obtained from an instrumented wheelchair platform and a low-cost camera, and validated with observations from a laser range finder and derived (known) maps of the environment.

Map joining is an efficient strategy for solving feature based SLAM problems. This paper demonstrates that joining of two 2D local maps, formulated as a nonlinear least squares problem has at most two local minima, when the associated uncertainties can be described using spherical covariance matrices. Necessary and sufficient condition for the existence of two minima is derived and it is shown that more than one minimum exists only when the quality of the local maps used for map joining is extremely poor. The analysis explains to some extent why a number of optimization based SLAM algorithms proposed in the recent literature that rely on local search strategies are successful in converging to the globally optimal solution from poor initial conditions, particularly when covariance matrices are spherical. It also demonstrates that the map joining problem has special properties that may be exploited to reliably obtain globally optimal solutions to the SLAM problem.

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 demonstrates that 2D pose SLAM has an underlining near convex structure when formulated as a least squares (LS) optimization problem. By introducing new variables and some approximations, the LS pose SLAM problem can be formulated as a quadratically constrained quadratic programming (QCQP) problem. The QCQP formulation can then be relaxed into a semi-definite programming (SDP) problem which is convex. Unique solution to the convex SDP problem can be obtained without initial state estimate and can be used to construct a candidate solution to the original LS pose SLAM problem. Simulation datasets and the Intel Research Lab dataset have been used to demonstrate that when the relative pose information contain noises with reasonable level, the candidate solution obtained through the relaxation is very close to the optimal solution to the LS SLAM problem. Thus in practice, the candidate solution can serve as either an approximate solution or a good initial guess for a local optimization algorithm to obtain the optimal solution to the LS pose SLAM problem.

This paper formulates the SLAM problem using 2D laser data as an optimization problem. The environment is modeled as a set of curves and the variables of the optimization problem are the robot poses as well as the parameters describing the curves. There are two key differences between this SLAM formulation and existing SLAM methods. First, the environment is represented by continuous curves instead of point clouds or occupancy grids. Second, all the laser readings, including laser beams which returns its maximum range value, have been included in the objective function. As the objective function to be optimized contains discontinuities, it can not be solved by standard gradient based approaches and thus a Genetic Algorithm (GA) based method is applied. Matching of laser scans acquired from relatively far apart robot poses is achieved by applying GA on top of the Iterative closest point (ICP) algorithm. The new SLAM formulation and the use of a global optimization algorithm successfully avoid the convergence to local minimum for both the scan matching and the SLAM problem. Both simulated and experimental data are used to demonstrate the effectiveness of the proposed techniques.

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.

In this paper, an optimisation model based on Pickup and Delivery Problem with Time Windows (PDPTW), and an exact algorithm based on Branch-and-Bound with Column Generation (BBCG), are presented for Autonomous Straddle Carriers Scheduling (ASCS) problem at automated container terminals. The ASCS problem is firstly modeled into a PDPTW, which is formulated as a Binary Integer Programming (BIP) and then solved by Column Generation (CG) in the Branch-and-Bound (BB) framework. The BBCG algorithm is also compared to another two exact algorithms [i.e., Binary integer Programming with Dynamic Programming (BPDP) and Exhaustive Search with Permutation and Combination (ESPC)] for the ASCS problem solving. Based on the map of an actual automated container terminal, simulation results and discussions are presented to demonstrate the effectiveness and efficiency of the presented model and algorithm for autonomous vehicle scheduling.

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.

The main contribution of this paper is a mathematical model describing performance metrics for coordinating multiple mobile robots in a seaport container terminal. The scenario described here requires dealing with many difficult practical challenges such as the presence of multiple levels of container stacking and sequencing, variable container orientations, and vehicular dynamics that require finite acceleration and deceleration times. Furthermore, in contrast to the automatically guided vehicle planning problem in a manufacturing environment, the container carriers described here are free ranging. Although, the port structure imposes a set of “virtual” roadways along which the vehicles are allowed to travel, path planning is essential in preventing contention and collisions. A performance metric which minimises total yard-vehicle usage, while producing robust traffic plans by encouraging both early starting and finishing of jobs is presented for different vehicle fleet sizes and job allocation scenarios.

In many mobile robotic manipulation tasks it is desirable to interact with the robots surroundings without actually grasping the object being manipulated. Non-prehensile manipulation allows a robot to interact in situations which would otherwise be impossible due to size or weight. This paper presents the derivation of a mathematical model of an object pushed by a single point and sliding in the presence of friction where the dynamic effects of mass and inertia are significant. This model is validated using numerical simulation. The derived dynamic model is also compared with a kinematic approximation from literature, showing that under certain conditions, the motion of a pushed object is similar to the motion of a non-holonomic vehicle. Finally, the results of experimental investigations are discussed and promising directions for further work are proposed.

In recent SLAM (simultaneous localization and mapping) literature, Pose Only optimization methods have become increasingly popular. This is greatly supported by the fact that these algorithms are computationally more efficient, as they focus more on the robots trajectory rather than dealing with a complex map. Implementation simplicity allows these to handle both 2D and 3D environments with ease. This paper presents a detailed evaluation on the reliability and accuracy of Pose Only SLAM, and aims at providing a definitive answer to whether optimizing poses is more advantages than optimizing features. Focus is centered around TORO, a Tree based network optimization algorithm, which has gained increased recognition within the robotics community. We compare this with Least Squares, which is often considered one of the best Maximum Likelihood method available. Results are based on both simulated and real 2D environments, and presented in a way where our conclusions can be substantiated.

Most people believe SLAM is a complex nonlinear estimation/optimization problem. However, recent research shows that some simple iterative methods based on linearization can sometimes provide surprisingly good solutions to SLAM without being trapped into a local minimum. This demonstrates that hidden structure exists in the SLAM problem that is yet to be understood. In this paper, we first analyze how far SLAM is from a convex optimization problem. Then we show that by properly choosing the state vector, SLAM problem can be formulated as a nonlinear least squares problem with many quadratic terms in the objective function, thus it is clearer how far SLAM is from a linear least squares problem. Furthermore, we explain that how the map joining approaches reduce the nonlinearity/nonconvexity of the SLAM problem.

This paper presents a statistically consistent SLAM algorithm where the environment is represented using a collection of B-Splines. The use of B-Splines allow environment to be represented without having to extract specific geometric features such as lines or points. Our previous work proposed a new observation model that enables raw measurements taken from a laser range finder to be transferred into relative position information between the control points of a B-Spline and the robot pose where the observation is made. One of the unresolved issues in the work was the estimation of the observation covariance, which is addressed through an analytical approach in this paper. As the uncertainty associated with the observation model is accurately defined and an optimization approach is used in the estimation process, the proposed SLAM algorithm can produce consistent estimates. Both simulation and experimental data are used for evaluation of the results.

This paper provides a novel state vector and covariance sub-matrix recovery algorithm for a recently developed submap based exactly sparse extended information filter (EIF) SLAM algorithm - sparse local submap joining filter (SLSJF). The algorithm achieves exact recovery instead of approximate recovery. The recovery algorithm is very efficient because of an incremental Cholesky factorization approach and a natural reordering of the global state vector. Simulation results show that the computation cost of the SLSJF is much lower as compared with the sequential map joining algorithm using extended Kalman filter (EKF). The SLSJF with the proposed recovery algorithm is also successfully applied to the Victoria Park data set.

This paper considers the trajectory planning problem for line-feature based SLAM in structured indoor environments. The robot poses and line features are estimated using Smooth and Mapping (SAM) which is found to provide more consistent estimates than the Extended Kalman Filter (EKF). The objective of trajectory planning is to minimise the uncertainty of the estimates and to maximise coverage. Trajectory planning is performed using Model Predictive Control (MPC) with an attractor incorporating long term goals. This planning is demonstrated both in simulation and in a real-time experiment with a Pioneer2DX robot.

This paper presents a novel local submap joining algorithm for building large-scale feature-based maps: sparse local submap joining filter (SLSJF). The input to the filter is a sequence of local submaps. Each local submap is represented in a coordinate frame defined by the robot pose at which the map is initiated. The local submap state vector consists of the positions of all the local features and the final robot pose within the submap. The output of the filter is a global map containing the global positions of all the features as well as all the robot start/end poses of the local submaps. Use of an extended information filter (EIF) for fusing submaps makes the information matrix associated with SLSJF exactly sparse. The sparse structure together with a novel state vector and covariance submatrix recovery technique makes the SLSJF computationally very efficient. The SLSJF is a canonical and efficient submap joining solution for large-scale simultaneous localization and mapping (SLAM) problems that makes use of consistent local submaps generated by any reliable SLAM algorithm. The effectiveness and efficiency of the new algorithm is verified through computer simulations and experiments.

A problem of searching with multiple searchers and scouts is presented. Unlike most search problems that terminate as soon as the target is found, successful detection by scouts only improve on the current knowledge of the moving target's location, such that the searchers can more effectively find and service the target in the future. The team must correspondingly plan not only to maximize the probability of the searchers directly finding the target, but also give them the best chance of exploiting any new information from potential scout detections. It is shown that this need to plan for replanning can be addressed by equivalently solving a series of simpler detection search problems that always do terminate on detection. Optimal and heuristic solution methods for this searcher/scout problem are derived, such that the capabilities of all the sensing platforms in a search task are harnessed even when only a subset are capable of actually servicing the target.

This paper investigates the convergence properties and consistency of extended Kalman filter (EKF) based simultaneous localization and mapping (SLAM) algorithms. Proofs of convergence are provided for the nonlinear two-dimensional SLAM problem with point landmarks observed using a range-and-bearing sensor. It is shown that the robot orientation uncertainty at the instant when landmarks are first observed has a significant effect on the limit and/or the lower bound of the uncertainties of the landmark position estimates. This paper also provides some insights to the inconsistencies of EKF based SLAM that have been recently observed. The fundamental cause of EKF SLAM inconsistency for two basic scenarios are clearly stated and associated theoretical proofs are provided.

The main contribution of this paper is a theoretical analysis of the extended Kalman filter (EKF) based solution to the simultaneous localisation and mapping (SLAM) problem. The convergence properties for the general nonlinear two-dimensional SLAM are provided. The proofs clearly show that the robot orientation error has a significant effect on the limit and/or the lower bound of the uncertainty of the landmark location estimates. Furthermore, some insights to the performance of EKF SLAM and a theoretical analysis on the inconsistencies in EKF SLAM that have been recently observed are given

The main contribution of this paper is a new SLAM algorithm for the mapping of large scale environments by combining local maps. The local maps can be generated by traditional extended Kalman filter (EKF) based SLAM. Relationships between the locations of the landmarks in the local map are then extracted and used in an extended information filter (EIF) to build a global map. An important feature is that the information matrix for the global map is exactly sparse, leading to significant computational advantages. This paper thus presents an algorithm that combines the advantages of both the existing local map joining SLAM algorithms, which reduces the linearization error in EKF SLAM and allows computationally demanding global map fusion to be scheduled off-line, and the decoupled SLAM (D-SLAM) algorithm, which provides an efficient strategy for building large maps using relative location information. The effectiveness of the new algorithm is illustrated through computer simulations.

We consider a search for a target moving within a known indoor environment partitioned into interconnected regions of varying sizes. The knowledge of target location is described as a probability distribution over the regions, and the searcher can only move from one region to another as the structure allows. The objective is to find a feasible path through the regions that maximizes the probability of locating the target within fixed time. This problem generalizes the existing optimal searcher path problem (OSP) by additionally stipulating a minimum amount of time that a finite-speed searcher must spend to travel through a region before reaching the next. We propose a technique to obtain the upper bound of detection for solving the problem in a branch and bound framework. Comparisons show that the technique is also superior to known bounding methods for the original optimal searcher path problem

Active SLAM poses the challenge for an autonomous robot to plan efficient paths simultaneous to the SLAM process. The uncertainties of the robot, map and sensor measurements, and the dynamic and motion constraints need to be considered in the planning process. In this paper, the active SLAM problem is formulated as an optimal trajectory planning problem. A novel technique is introduced that utilises an attractor combined with local planning strategies such as model predictive control (a.k.a. receding horizon) to solve this problem. An attractor provides high level task intentions and incorporates global information about the environment for the local planner, thereby eliminating the need for costly global planning with longer horizons. It is demonstrated that trajectory planning with an attractor results in improved performance over systems that have local planning alone

In this paper, the possibility and necessity of multi-step trajectory planning in Extended Kalman Filter (EKF) based SLAM is investigated. The objective of the trajectory planning here is to minimize the estimation error of the robot and landmark locations subject to a given time horizon. We show that the problem can be regarded as an optimization problem for a gradually identified model. A numerical method is proposed for trajectory planning using a variant of the nonlinear Model Predictive Control (MPC). The proposed method is optimal in the sense that the control action is computed using all the information available at the time of decision making. Simulation results are included to compare the results from the one-step look-ahead trajectory planning and the proposed multi-step look-ahead technique.

The main contribution of this paper is an algorithm for integrating motion planning and simultaneous localisation and mapping (SLAM). Accuracy of the maps and the robot locations computed using SLAM is strongly dependent on the characteristics of the environment, for example feature density, as well as the speed and direction of motion of the robot. Appropriate control of the robot motion is particularly important in bearing-only SLAM, where the information from a moving sensor is essential. In this paper a near minimum time path planning algorithm with a finite planning horizon is proposed for bearing-only SLAM. The objective of the algorithm is to achieve a predefined mapping precision while maintaining acceptable vehicle location uncertainty in the minimum time. Simulation results have shown the effectiveness of the proposed method.

The main contribution of this paper is an algorithm for autonomous search that minimizes the expected time for detecting multiple targets present in a known built environment. The proposed technique makes use of the probability distribution of the target(s) in the environment, thereby making it feasible to incorporate any additional information, known a-priori or acquired while the search is taking place, into the search strategy. The environment is divided into a set of distinct regions and an adjacency matrix is used to describe the connections between them. The costs of searching any of the regions as well as the cost of travel between them can be arbitrarily specified. The search strategy is derived using a dynamic programming algorithm. The effectiveness of the algorithm is illustrated using an example based on the search of an office environment. An analysis of the computational complexity is also presented.

This paper provides a solution to the optimal trajectory planning problem in target localisation for multiple heterogeneous robots with bearing-only sensors. The objective here is to find robot trajectories that maximise the accuracy of the locations of the targets at a prescribed terminal time. The trajectory planning is formulated as an optimal control problem for a nonlinear system with a gradually identified model and then solved using nonlinear model predictive control (MPC). The solution to the MPC optimisation problem is computed through exhaustive expansion tree search (EETS) plus sequential quadratic programming (SQP). Simulations were conducted using the proposed methods. Results show that EETS alone performs considerably faster than EETS+SQP with only minor differences in information gain, and that a centralised approach outperforms a decentralised one in terms of information gain. We show that a centralised EETS provides a near optimal solution. We also demonstrate the significance of using a matrix to represent the information gathered.

In this paper, we propose a new algorithm for SLAM that makes use of a state vector consisting of quantities that describe the relative locations among features. In contrast to previous relative map strategies, the new state vector is compact and always consists of 2n - 3 elements (in a 2D environment) where n is the number of features in the map. It is also shown that the information from observations can be transformed and grouped into two parts: first one containing the information about the map and the second one containing the information about the robot location relative to the features in the map. Therefore the SLAM can be decoupled into two processes where mapping uses the first part of the transformed observation vector and localization becomes a 3-dimensional estimation problem. It is also shown that the information matrix of the map is exactly sparse, resulting in potential computational savings when an information filter is used for mapping. The new decoupled SLAM algorithm is called D-SLAM and is illustrated using simulation.

This paper provides a technique for minimal time robot motion control in the estimation-theoretic based simultaneous localizations and map building (SLAM) problem. We consider the scenario that the robot needs to go to a destination which is a prescribed location in the coordinate system referenced by its starting position. The task of the robot is to reach the destination within minimal time while localizing itself and building a map of the environment with a prescribed accuracy. This task may be a real navigation task or may be a subtask in a SLAM problem of a large unknown environment. A global sub-optimal control law is derived using dynamic programming techniques.