Given a set of putative 3D-3D point correspondences, we aim to remove outliers and estimate rigid transformation with 6 degrees of freedom (DOF). Simultaneously estimating these 6 DOF is time-consuming due to high-dimensional parameter space. To solve this problem, it is common to decompose 6 DOF, i.e. independently compute 3-DOF rotation and 3-DOF translation. However, high non-linearity of 3-DOF rotation still limits the algorithm efficiency, especially when the number of correspondences is large. In contrast, we propose to decompose 6 DOF into$(2+1)$and$(1+2)\ DOF$. Specifically,$(2+1)DOF$represent 2-DOF rotation axis and 1-DOF displacement along this rotation axis.$(1+2)\ DOF$indicate 1-DOF rotation angle and 2-DOF displacement orthogonal to the above rotation axis. To compute these DOF, we design a novel two-stage strategy based on inlier set maximization. By leveraging branch and bound, we first search for$(2+1)\ DOF$, and then the remaining$(1+2)\ DOF$. Thanks to the proposed transformation decomposition and two-stage search strategy, our method is deterministic and leads to low computational complexity. We extensively compare our method with state-of-the-art approaches. Our method is more accurate and robust than the approaches that provide similar efficiency to ours. Our method is more efficient than the approaches whose accuracy and robustness are comparable to ours.

Ultra-wideband (UWB) is a radio technology that is usually used for indoor localization. To complete the 3D localization task in a room, more than three UWB anchors should be installed without occlusion, then a UWB tag can be localized. An Inertial Measurement Unit (IMU) is usually fused to provide smoother results. However, the requirements for the installation of UWB anchors limit their application in a floor with multiple rooms and corridors. In this paper, we propose a localization method that needs fewer anchors in this environment. In addition to the range information from UWB anchors, non-holonomic constraints from the carrier and reference trajectory from the special environmental structure are added as observations of the error-state Extended Kalman Filter (EKF) system. The proposed method can complete the localization task in the corridor-like areas using a single UWB anchor, so fewer UWB anchors are needed in a floor with multiple rooms and corridors. The real-world experiment video is available at https://youtu.be/DT-02Id_94A.

This paper presents a comprehensive inertial aided 3D LiDAR SLAM system with hybrid geometric primitives in large-scale environments, including a tightly-coupled LiDAR-Inertial-Odometry (LIO), a global mapping module supported by learning-based loop closure detection and a sub-maps matching algorithm. An efficient method is developed to simultaneously extract explicit plane features and point features from each raw point cloud. To make full use of the structural information of the surroundings, plane features and point features (ground and edge) are tracked across a fix-sized group of LiDAR keyframes in the local map. For effective loop closure detection in large-scale environments, we integrate the learning-based point cloud network and a keyframe sequence matching method to detect loops. Finally, a novel, deterministic and near real-time plane-driven sub-maps matching algorithm is proposed to close the loops. The proposed SLAM system is validated with experiments on different types of environments.

Given a set of 3D-to-2D point correspondences corrupted by outliers, we aim to robustly estimate the absolute camera pose. Existing methods robust to outliers either fail to guarantee high robustness and efficiency simultaneously, or require an appropriate initial pose and thus lack generality. In contrast, we propose a novel approach based on the robust "L2-minimizing estimate" (L2E) loss. We first define a novel cost function by integrating the projection constraint into the L2E loss. Then to efficiently obtain the global minimum of this function, we propose a hybrid strategy of a local optimizer and branch-and-bound. For branch-and-bound, we derive effective function bounds. Our approach can handle high outlier ratios, leading to high robustness. It can run reliably regardless of whether the initial pose is appropriate, providing high generality. Moreover, given a decent initial pose, it is suitable for real-time applications. Experiments on synthetic and real-world datasets showed that our approach outperforms state-of-the-art methods in terms of robustness and/or efficiency.

This paper presents a novel solution for online trajectory planning of a full-size tractor-trailers vehicle composed of a car-like tractor and arbitrary number of passive full trailers. The motion planning problem for such systems was rarely addressed due to the complex nonlinear dynamics. A simulation-based prediction method is proposed to easily handle the complicated nonlinear dynamics and efficiently generate the obstacle-free and dynamically feasible trajectories. The vehicle dynamics model and a two-layer controller are used in the prediction. Implementation results on the real-world full-size industrial tractor-trailers vehicle are presented to validate the performance of the proposed methods.

This paper presents a novel dataset targeting three types of challenging environments for autonomous driving, i.e., the industrial logistics environment, the undulating hill environment and the mixed complex urban environment. To the best of the author's knowledge, similar dataset has not been published in the existing public datasets, especially for the logistics environment collected in the functioning Hong Kong Air Cargo Terminal (HACT). Structural changes always suddenly appeared in the airport logistics environment due to the frequent movement of goods in and out. In the structureless and noisy hill environment, the non-flat plane movement is usual. In the mixed complex urban environment, the highly dynamic residence blocks, sloped roads and highways are included in a single collection. The presented dataset includes LiDAR, image, IMU and GPS data by repeatedly driving along several paths to capture the structural changes, the illumination changes and the different degrees of undulation of the roads. The baseline trajectories are provided which are estimated by Simultaneous Localization and Mapping (SLAM).

In this paper, we propose a dynamic state estimation framework for lateral control of a heavy tractor-trailers system using only mass-produced low-cost sensors. This issue is challenging since the lateral velocity of the lead tractor is difficult to measure directly. The performance of existing dynamic model-based estimation methods will also be degraded, as different trailers and payloads cause the tractor model parameters to change. We address this issue by incorporating a kinematic estimator into a dynamic model-based estimation scheme. Accurate and reliable tire cornering stiffness and dynamics-informed lateral velocity of the lead tractor can be output in real-time by using our method. The stability and robustness of the proposed method are theoretically proved. The feasibility of our method is verified by full-scale experiments. It is also verified that the estimated model parameters and lateral states do improve the control performance by integrating the estimator into a lateral control system.

The image lines projected from parallel 3D lines intersect at a common point called the vanishing point (VP). Manhattan world holds for the scenes with three orthogonal VPs. In Manhattan world, given several lines in a calibrated image, we aim at clustering them by three unknown-but-sought VPs. The VP estimation can be reformulated as computing the rotation between the Manhattan frame and the camera frame. To compute this rotation, state-of-the-art methods are based on either data sampling or parameter search, and they fail to guarantee the accuracy and efficiency simultaneously. In contrast, we propose to hybridize these two strategies. We first compute two degrees of freedom (DOF) of the above rotation by two sampled image lines, and then search for the optimal third DOF based on the branch-and-bound. Our sampling accelerates our search by reducing the search space and simplifying the bound computation. Our search is not sensitive to noise and achieves quasi-global optimality in terms of maximizing the number of inliers. Experiments on synthetic and real-world images showed that our method outperforms state-of-the-art approaches in terms of accuracy and/or efficiency.

Point cloud based place recognition is still an open issue due to the difficulty in extracting local features from the raw 3D point cloud and generating the global descriptor, and it's even harder in the large-scale dynamic environments. In this paper, we develop a novel deep neural network, named LPD-Net (Large-scale Place Description Network), which can extract discriminative and generalizable global descriptors from the raw 3D point cloud. Two modules, the adaptive local feature extraction module and the graph-based neighborhood aggregation module, are proposed, which contribute to extract the local structures and reveal the spatial distribution of local features in the large-scale point cloud, with an end-to-end manner. We implement the proposed global descriptor in solving point cloud based retrieval tasks to achieve the large-scale place recognition. Comparison results show that our LPD-Net is much better than PointNetVLAD and reaches the state-of-the-art. We also compare our LPD-Net with the vision-based solutions to show the robustness of our approach to different weather and light conditions.

3D lines in structured environments encode particular regularity like parallelism and orthogonality. We leverage this structural regularity to estimate the absolute and relative camera poses. We decouple the rotation and translation, and propose a novel rotation estimation method. We decompose the absolute and relative rotations and reformulate the problem as computing the rotation from the Manhattan frame to the camera frame. To compute this rotation, we propose an accurate and efficient two-step method. We first estimate its two degrees of freedom (DOF) by two image lines, and then estimate its third DOF by another image line. For these lines, we assume their associated 3D lines are mutually orthogonal, or two 3D lines are parallel to each other and orthogonal to the third. Thanks to our two-step DOF estimation, our absolute and relative pose estimation methods are accurate and efficient. Moreover, our relative pose estimation method relies on weaker assumptions or less correspondences than existing approaches. We also propose a novel strategy to reject outliers and identify dominant directions of the scene. We integrate it into our pose estimation methods, and show that it is more robust than RANSAC. Experiments on synthetic and real-world datasets demonstrated that our methods outperform state-of-the-art approaches.

Place recognition and loop-closure detection are main challenges in the localization, mapping and navigation tasks of self-driving vehicles. In this paper, we solve the loop-closure detection problem by incorporating the deep-learning based point cloud description method and the coarse-to-fine sequence matching strategy. More specifically, we propose a deep neural network to extract a global descriptor from the original large-scale 3D point cloud, then based on which, a typical place analysis approach is presented to investigate the feature space distribution of the global descriptors and select several super keyframes. Finally, a coarse-to-fine strategy, which includes a super keyframe based coarse matching stage and a local sequence matching stage, is presented to ensure the loop-closure detection accuracy and real-time performance simultaneously. Thanks to the sequence matching operation, the proposed approach obtains an improvement against the existing deep-learning based methods. Experiment results on a self-driving vehicle validate the effectiveness of the proposed loop-closure detection algorithm.

Existing works on control of tractor-trailers systems only consider the kinematics model without taking dynamics into account. Also, most of them treat the issue as a pure control theory problem whose solutions are difficult to implement. This paper presents a trajectory tracking control approach for a full-scale industrial tractor-trailers vehicle composed of a carlike tractor and arbitrary number of passive full trailers. To deal with dynamic effects of trailing units, a force sensor is innovatively installed at the connection between the tractor and the first trailer to measure the forces acting on the tractor. The tractor's dynamic model that explicitly accounts for the measured forces is derived. A tracking controller that compensates the pulling/pushing forces in real time and simultaneously drives the system onto desired trajectories is proposed. The propulsion map between throttle opening and the propulsion force is proposed to be modeled with a fifth-order polynomial. The parameters are estimated by fitting experimental data, in order to provide accurate driving force. Stability of the control algorithm is rigorously proved by Lyapunov methods. Experiments of full-size vehicles are conducted to validate the performance of the control approach.