In this paper, we propose a third-order, i.e., white-noise-on-jerk, Gaussian Process (GP) Trajectory Representation (TR) framework for continuous-time (CT) motion estimation (ME) tasks. Our framework features a unified trajectory representation that encapsulates the kinematic models of both SO(3)$times$R3and SE(3) pose representations. This encapsulation strategy allows users to use the same implementation of measurement-based factors for either choice of pose representation, which facilitates experimentation and comparison to make a better choice for the ME task. In addition, unique to our framework, we derive the kinematic models with theclosed-form temporal derivatives of the local variables ofSO(3) and SE(3), which so far has only been approximated based on Taylor expansion in the literature. Our experiments show that these kinematic models can improve the estimation accuracy in high-speed scenarios. All analytical Jacobians of the interpolated states with respect to the support states of the trajectory representation, as well as the motion prior factors, are also provided for accelerated Gauss-Newton (GN) optimization. Our experiments demonstrate the efficacy and efficiency of the framework in various motion estimation tasks such as localization, calibration, and odometry, facilitating fast prototyping for ME researchers. We release the source code for the benefit of the community. Our project is available athttps://github.com/brytsknguyen/gptr.

We present a novel sample reduction scheme for random variables belonging to the SE(2) group by means of Dirac mixture approximation. For this, dual quaternions are employed to represent uncertain planar transformations. The Cramér-von Mises distance is modified as a smooth metric to measure the statistical distance between Dirac mixtures on the manifold of planar dual quaternions. Samples of reduced size are then obtained by minimizing the probability divergence via Riemannian optimization while interpreting the correlation between rotation and translation. We further deploy the proposed scheme for nonparametric modeling of estimates for nonlinear SE(2) estimation. Simulations show superior tracking performance of the sample reduction-based filter compared with Monte Carlo-based as well as parametric model-based planar dual quaternion filters.

Estimating orientations of objects in Euclidean space is an omnipresent challenge in robotics and autonomous systems. A useful representation of orientations involves unit quaternions. While the space of all unit quaternions forms a three-dimensional unit hypersphere, inverting the sign of a quaternion does not change the orientation described by it. Therefore, all possible orientations can be described by considering only a hemisphere of the unit hypersphere. In this paper, we propose a grid filter for arbitrary-dimensional unit hyperhemispheres and apply it to an orientation estimation task and another evaluation scenario. Our approach outperforms previous approaches that consider densities on the entire hypersphere.

We present a novel region growing algorithm for plane extraction of organized point clouds using the spherical convex hull. Instead of explicit plane parameterization, our approach interprets potential underlying planes as a series of geometric constraints on the sphere that are refined during region growing. Unlike existing schemes relying on downsampling for sequential execution in real time, our approach enables pixelwise plane extraction that is highly parallelizable. We further test the proposed approach with a fully parallel implementation on a GPU. Evaluation based on public data sets has shown state-of-the-art extraction accuracy and superior speed compared to existing approaches, while guaranteeing real-time processing at full input resolution of a typical RGB-D camera.

Predictive energy management has become a new focus of the automobile industry for its high potential of further reducing energy consumption. Based on previous works on predictive speed optimization using discrete dynamic programming (DDP), this paper introduces a novel approach of applying DDP with variable step size in stage variable discretization, which can realize a better tradeoff between precision and computational cost. In this approach, a “meshing” algorithm searches the points of interest (POI), such as speed limit change, traffic lights, and road curvatures, where changes in vehicle speed are expected. The algorithm increases the step-size resolution close to these points and reduces the resolutions in positions further away from POI, where the optimized vehicle speed is insensitive to the step size. With this approach, the position of POI can be precisely located to solve the DDP problem. In a test case with a relatively high density of POI, the computational cost is reduced by more than 53% by only sacrificing less than 1% of precision compared to a fixed step-size discretization with high resolutions. It can be expected that, with a lower density of POI, the computational cost will be reduced even further.

In this paper, we present a novel nonlinear progressive filtering approach for estimatingSE(2) states represented by unit dual quaternions. Unlike previously published approaches, the measurement model no longer needs to be assumed as identity. Our solution utilizes deterministic sampling on a Bingham-like probability distribution, which has been adapted to simultaneously model orientation and translation. During the measurement update step, the estimate gets progressively updated. Our approach inherently incorporates the nonlinear structure ofSE(2) and enables a flexible measurement update step. We also give an evaluation for planar rigid body motion estimation with a case study that is close to real-world scenarios.

In this paper, we present a novel approach to perform simultaneous localization and mapping (SLAM) for planar motions based on stochastic filtering with dual quaternion particles using low-cost range and gyro sensor data. Here, SE(2) states are represented by unit dual quaternions and further get stochastically modeled by a distribution from directional statistics such that particles can be generated by random sampling. To build the full SLAM system, a novel dual quaternion particle filter based on Rao-Blackwellization is proposed for the tracking block, which is further integrated with an occupancy grid mapping block. Unlike previously proposed filtering approaches, our method can perform tracking in the presence of multi-modal noise in unknown environments while giving reasonable mapping results. The approach is further evaluated using a walking robot with on-board ultrasonic sensors and an IMU sensor navigating in an unknown environment in both simulated and real-world scenarios.