Robot localization is a fundamental task in achieving true autonomy. Recently, many graph-based navigators have been proposed that combine an inertial measurement unit (IMU) with an exteroceptive sensor applying IMU preintegration to synchronize both sensors. IMUs are affected by biases that also have to be estimated. To increase the navigator robustness when faults appear on the perception system, IMU preintegration can be complemented with linear velocity measurements obtained from visual odometry, leg odometry, or a Doppler Velocity Log (DVL), depending on the robotic application. Moreover, higher grade IMUs are sensitive to the Earth rotation rate, which must be compensated in the preintegrated measurements. In this article, we propose a general purpose preintegration methodology formulated on a compact Lie group to set motion constraints on graph simultaneous localization and mapping problems considering the Earth rotation effect. We introduce the SE$_{N}(3)$group to jointly preintegrate IMU data and linear velocity measurements to preserve all the existing correlation within the preintegrated quantity. Field experiments using an autonomous underwater vehicle equipped with a DVL and a navigational grade IMU are provided and results are benchmarked against a commercial filter-based inertial navigation system to prove the effectiveness of our methodology.

2D SLAM is useful for mobile robots that are constrained to a 2D plane, for example in a warehouse, simplifying calculations in respect to the 3D case. The use of an IMU in such a context can enrich the estimation and make it more robust. In this paper we reformulate the IMU preintegration widely used in 3D problems for the 2D case, making use of Lie Theory. The Lie theory based formalization, first derived for a perfectly horizontal plane, allows us to easily extend it to problems where the plane is not orthogonal to the gravity vector. We implement the theory in a factor graph based estimation library, and carry out experiments to validate it on a mobile platform. Two experiments are carried out; on a horizontal and a sloped environment, and the sensor data is processed using our two 2D methods and a state-of-the-art 3D method.

State estimation, in particular estimation of the base position, orientation and velocity, plays a big role in the efficiency of legged robot stabilization. The estimation of the base state is particularly important because of its strong correlation with the underactuated dynamics, i.e. the evolution of center of mass and angular momentum. Yet this estimation is typically done in two phases, first estimating the base state, then reconstructing the center of mass from the robot model. The underactuated dynamics is indeed not properly observed, and any bias in the model would not be corrected from the sensors. While it has already been observed that force measurements make such a bias observable, these are often only used for a binary estimation of the contact state. In this paper, we propose to simultaneously estimate the base and the underactuation state by exploiting all measurements simultaneously. To this end, we propose several contributions to implement a complete state estimator using factor graphs. Contact forces altering the underactuated dynamics are pre-integrated using a novel adaptation of the IMU pre-integration method, which constitutes the principal contribution. IMU pre-integration is also used to estimate the positional motion of the base. Encoder measurements then participate to the estimation in two ways: by providing leg odometry displacements which contributes to the observability of IMU biases; and by relating the positional and centroidal states, thus connecting the whole graph and producing a tightly-coupled whole-body estimator. The validity of the approach is demonstrated on real data captured by the Solo12 quadruped robot.

We present a novel vehicle detection and tracking system that works solely on 3D LiDAR information. Our approach segments vehicles using a dual-view representation of the 3D LiDAR point cloud on two independently trained convolutional neural networks, one for each view. A bounding box growing algorithm is applied to the fused output of the networks to properly enclose the segmented vehicles. Bounding boxes are grown using a probabilistic method that takes into account also occluded areas. The final vehicle bounding boxes act as observations for a multi-hypothesis tracking system which allows to estimate the position and velocity of the observed vehicles. We thoroughly evaluate our system on the KITTI benchmarks both for detection and tracking separately and show that our dual-branch classifier consistently outperforms previous single-branch approaches, improving or directly competing to other state of the art LiDAR-based methods.

Recently, Differential Dynamic Programming (DDP) and other similar algorithms have become the solvers of choice when performing non-linear Model Predictive Control (nMPC) with modern robotic devices. The reason is that they have a lower computational cost per iteration when compared with off-the-shelf Non-Linear Programming (NLP) solvers, which enables its online operation. However, they cannot handle constraints, and are known to have poor convergence capabilities. In this paper, we propose a method to solve the optimal control problem with control bounds through a squashing function (i.e., a sigmoid, which is bounded by construction). It has been shown that a naive use of squashing functions damage the convergence rate. To tackle this, we first propose to add a quadratic barrier that avoids the difficulty of the plateau produced by the sigmoid. Second, we add an outer loop that adapts both the sigmoid and the barrier; it makes the optimal control problem with the squashing function converge to the original control-bounded problem. To validate our method, we present simulation results for different types of platforms including a multi-rotor, a biped, a quadruped and a humanoid robot.

This paper proposes the use of piecewise Cnsmooth curve for mobile-base motion planning and control, coined Timed-Elastic Smooth Curve (TESC) planner. Based on a Timed-Elastic Band, the problem is defined so that the trajectory lies on a spline in SE(2) with non-vanishing n-th derivatives at every point. Formulated as a multi-objective nonlinear optimization problem, it allows imposing soft constraints such as collision-avoidance, velocity, acceleration and jerk limits, and more. The planning process is realtime-capable allowing the robot to navigate in dynamic complex scenarios. The proposed method is compared against the state-of-the-art in various scenarios. Results show that trajectories generated by the TESC planner have smaller average acceleration and are more efficient in terms of total curvature and pseudo-kinetic energy while being produced with more consistency than state-of-the-art planners do.

The lack of depth information in camera images has triggered much work on their use for localization and mapping in robotics. In particular, specific landmark parametrizations that isolate the unknown depth in one variable, and that allows to handle the associated large uncertainties have been proposed. Recently, an innovative parametrization (Parallax Angle) has shown to outperform the others in the context of a Bundle Adjustment approach. This paper investigates the way to exploit this parametrization in an incremental graph-based SLAM approach, in a robotics context in which motions measures can be incorporated in the overall estimation. It presents the factors required to initialize landmarks and manage their observations. Simulation results show that the proposed algorithms are able to incrementally incorporate observations, and a discussion analyzes how the incremental updates on ISAM2 are affected by these new factors.

Localization is the key perceptual process closing the loop of autonomous navigation, allowing self-driving vehicles to operate in a deliberate way. To ensure robust localization, autonomous vehicles have to implement redundant estimation processes, ideally independent in terms of the underlying physics behind sensing principles. This paper presents a stereo radar odometry system, which can be used as such a redundant system, complementary to other odometry estimation processes, providing robustness for long-term operability. The presented work is novel with respect to previously published methods in that it contains: (i) a detailed formulation of the Doppler error and its associated uncertainty; (ii) an observability analysis that gives the minimal conditions to infer a 2D twist from radar readings; and (iii) a numerical analysis for optimal vehicle sensor placement. Experimental results are also detailed that validate the theoretical insights.

This paper develops a simple and low-cost method for 3D, high-rate vehicle state estimation, specially designed for free-flying Micro Aerial Vehicles (MAVs). We fuse observations from inertial measurement units and the recently appeared low-cost optical flow smart cameras. These smart cameras integrate a sonar altimeter, a triaxial gyrometer and an optical flow sensor, and directly provide metric ego-motion information in the form of body velocities and altitude. Compared to state-of-the-art visual-inertial odometry methods, we are able to drastically reduce the computational load in the main processor unit, and obtain an accurate estimation of the vehicle state at a high update rate of 100Hz. We thus extend the current use of these smart cameras from hovering purposes to odometry estimation. In order to propose a simple algorithmic solution, we investigate the performances of two Kalman filters, in the extended and error-state flavors, alongside a large number of algorithm variations, using simulations and real experiments with precise ground-truth. We observe that the marginal performance gain attained with these algorithm improvements does not pay for the effort of implementing them. We conclude that a classical EKF in its simplest form is sufficient for providing motion estimates that coherently exploit the available measurements.

We benchmark in this article three different landmark parametrizations in monocular 6DOF EKF-SLAM. These parametrizations are homogeneous points (HP), inverse-distance points (IDP, better known as inverse-depth), and the new anchored homogeneous points (AHP). The discourse used for describing them is chosen to highlight their differences and similarities, showing that they are just incremental variations of ones with respect to the others. We show for the first time a complete comparison of HP against IDP, two methods that are getting popular, and introduce also for the first time AHP, whose description falls precisely between the other two. The benchmarking is done by running all algorithms on the same data and by using the well-established NEES consistency analysis. Our conclusion is that the new AHP parametrization is the most interesting one for monocular EKF-SLAM (followed by IDP and then HP) because it greatly postpones the apparition of EKF inconsistency.

This paper proposes a new framework to recover balance against external forces by combining disturbance suppression and reactive stepping. In the view point of the feedback control, a reactive step can help to diminish the disturbance caused by an external force that should be compensated to maintain balance. In other words, if the adequate step is performed, the feedback controller does not have to compensate all of the external force by itself. Under this concept, we propose an original solution to distribute the compensation between a feedback controller and a reactive step, according to the period of support phase and a disturbance characteristic. We first clearly distinguish between the role of the disturbance suppression and the reactive stepping. Then, based on this distinction, the small disturbance of external force or happening late during the single-support phase, is mainly suppressed by state feedback. The large disturbance which is out of capability by feedback controller and at the beginning of the single-support phase, is absorbed by modifying reactively the next steps. The proposed method is validated through experimental results with the HRP-2 humanoid robot.

In this work, we present a new approach for optimum design of nonlinear filters based on support vector machines. Taking advantage on the general concept of binary filters and machine learning theory, this proposed approach, is based on the concept of a new filter structured, called support vector machine filter (SVMF) and statistical data analysis. This proposed filter approach, is used as an impulsive noise image denoising. The results obtained for the application at hand show that the proposed filter outperforms a new algorithm for elimination of impulsive noise recently reported and center weighted median in the image denoising task. The proposed filter can be successfully applied for the processing of images corrupted with impulsive noise while maintaining the visual quality and a low reconstruction error.

This paper presents 6-DOF monocular EKF-SLAM with undelayed initialization using linear landmarks with extensible endpoints, based on the Plu¿cker parametrization. A careful analysis of the properties of the Plu¿cker coordinates, defined in the projective space P5, permits their direct usage for undelayed initialization. Immediately after detection of a segment in the image, a Plu¿cker line is incorporated in the map. A single Gaussian pdf includes inside its 2-sigma region all possible lines given the observed segment, from arbitrarily close up to the infinity range, and in any orientation. The lines converge to stable 3D configurations as the moving camera gathers observations from new viewpoints. The line's endpoints, maintained out of the map, are constantly retro-projected from the image onto the line's local reference frame. An extending-only policy is defined to update them. We validate the method via Monte Carlo simulations and with real imagery data.

This paper explores the possibilities of using monocular simultaneous localization and mapping (SLAM) algorithms in systems with more than one camera. The idea is to combine in a single system the advantages of both monocular vision (bearings-only, infinite range observations but no 3-D instantaneous information) and stereovision (3-D information up to a limited range). Such a system should be able to instantaneously map nearby objects while still considering the bearing information provided by the observation of remote ones. We do this by considering each camera as an independent sensor rather than the entire set as a monolithic supersensor. The visual data are treated by monocular methods and fused by the SLAM filter. Several advantages naturally arise as interesting possibilities, such as the desynchronization of the firing of the sensors, the use of several unequal cameras, self-calibration, and cooperative SLAM with several independently moving cameras. We validate the approach with two different applications: a stereovision SLAM system with automatic self-calibration of the rig's main extrinsic parameters and a cooperative SLAM system with two independent free-moving cameras in an outdoor setting.

This paper is an invitation to use mono-vision techniques on stereo-vision equipped robots. By using monocular algorithms on both cameras, the advantages of mono-vision (bearing-only, with infinity range but no 3D instant information) and stereo-vision (3D information only up to a limited range) naturally add up to provide interesting possibilities, that are here developed and demonstrated using an EKF-based monocular SLAM algorithm. Mainly we obtain: a) fast 3D mapping with long term, absolute angular references; b) great landmark updating flexibility; and c) the possibility of stereo rig extrinsic self-calibration, providing a much more robust and accurate sensor. Experimental results show the pertinence of the proposed ideas, which should be easily exportable (and we encourage to do so) to other, more performing, vision-based SLAM algorithms.

This article presents a bearing only 3D SLAM algorithm which has the same complexity and optimality as the usual extended Kalman filter used in classical SLAM. We especially focus on the landmark initialization process, which relies on visual point features tracked in the sequence of acquired images: a probabilistic approach to estimate their parameters is presented. This induces a particular structure of the filter architecture, in which are memorized a set of past robot poses. Simulations are made to compare the influence of some parameters required by our approach, and results with an indoor robot and an airship are presented.

Most solutions to the SLAM problem in robotics have utilised range and beating sensors as the provided perception data is easy to incorporate, allowing immediate landmark initialization. This is not the case when using bearing-only information because the distance to the perceived landmarks is not directly provided. A whole estimate of a landmark position is only possible via a set of measurements taken from different points of view. The vast majority of contributions to this problem perform a parallel task to get this estimate, and hence the landmark initialization is delayed. We give a new insight to the problem and present a method to avoid this delay by initializing the whole ray that defines the direction of the landmark. We utilize a minimal and computationally efficient form to represent this ray and a new strategy for the subsequent updates. Simulations have been carried out to validate the proposed algorithms.