Improved Pose Graph Optimization for Planar Motions Using Riemannian Geometry on the Manifold of Dual Quaternions

We present a novel Riemannian approach for planar pose graph optimization problems. By formulating the cost function based on the Riemannian metric on the manifold of dual quaternions representing planar motions, the nonlinear structure of the SE(2) group is inherently considered. To solve the on-manifold least squares problem, a Riemannian Gauss-Newton method using the exponential retraction is applied. The proposed Riemannian pose graph optimizer (RPG-Opt) is further evaluated based on public planar pose graph data sets. Compared with state-of-the-art frameworks, the proposed method gives equivalent accuracy and better convergence robustness under large uncertainties of odometry measurements.Comment: 7 pages. Submitted to 21st IFAC World Congress (IFAC 2020

Bayesian Pose Graph Optimization via Bingham Distributions and Tempered Geodesic MCMC

We introduce Tempered Geodesic Markov Chain Monte Carlo (TG-MCMC) algorithm for initializing pose graph optimization problems, arising in various scenarios such as SFM (structure from motion) or SLAM (simultaneous localization and mapping). TG-MCMC is first of its kind as it unites asymptotically global non-convex optimization on the spherical manifold of quaternions with posterior sampling, in order to provide both reliable initial poses and uncertainty estimates that are informative about the quality of individual solutions. We devise rigorous theoretical convergence guarantees for our method and extensively evaluate it on synthetic and real benchmark datasets. Besides its elegance in formulation and theory, we show that our method is robust to missing data, noise and the estimated uncertainties capture intuitive properties of the data.Comment: Published at NeurIPS 2018, 25 pages with supplement

Directional statistics and filtering using libDirectional

In this paper, we present libDirectional, a MATLAB library for directional statistics and directional estimation. It supports a variety of commonly used distributions on the unit circle, such as the von Mises, wrapped normal, and wrapped Cauchy distributions. Furthermore, various distributions on higher-dimensional manifolds such as the unit hypersphere and the hypertorus are available. Based on these distributions, several recursive filtering algorithms in libDirectional allow estimation on these manifolds. The functionality is implemented in a clear, well-documented, and object-oriented structure that is both easy to use and easy to extend

Tracking the spin on a ping pong ball with the quaternion Bingham filter

A deterministic method for sequential estimation of 3-D rotations is presented. The Bingham distribution is used to represent uncertainty directly on the unit quaternion hypersphere. Quaternions avoid the degeneracies of other 3-D orientation representations, while the Bingham distribution allows tracking of large-error (high-entropy) rotational distributions. Experimental comparison to a leading EKF-based filtering approach on both synthetic signals and a ball-tracking dataset shows that the Quaternion Bingham Filter (QBF) has lower tracking error than the EKF, particularly when the state is highly dynamic. We present two versions of the QBF- suitable for tracking the state of first- and second-order rotating dynamical systems.National Science Foundation (U.S.) (Grant 1117325)United States. Office of Naval Research. Multidisciplinary University Research Initiative (Grant N00014-09-1-1051)United States. Air Force Office of Scientific Research (Grant FA2386-10-1-4135)Singapore. Ministry of Education (SUTD-MIT International Design Centre

Directional Estimation for Robotic Beating Heart Surgery

In robotic beating heart surgery, a remote-controlled robot can be used to carry out the operation while automatically canceling out the heart motion. The surgeon controlling the robot is shown a stabilized view of the heart. First, we consider the use of directional statistics for estimation of the phase of the heartbeat. Second, we deal with reconstruction of a moving and deformable surface. Third, we address the question of obtaining a stabilized image of the heart

Multitarget Tracking Using Orientation Estimation for Optical Belt Sorting

In optical belt sorting, accurate predictions of the bulk material particles’ motions are required for high-quality results. By implementing a multitarget tracker tailored to the scenario and deriving novel motion models, the predictions are greatly enhanced. The tracker’s reliability is improved by also considering the particles’ orientations. To this end, new estimators for directional quantities based on orthogonal basis functions are presented and shown to outperform the state of the art

Bayesian inference for dynamic pose estimation using directional statistics

The dynamic pose of an object, where the object can represent a spacecraft, aircraft, or mobile robot, among other possibilities, is defined to be the position, velocity, attitude, and angular velocity of the object. A new method to perform dynamic pose estimation is developed that leverages directional statistics and operates under the Bayesian estimation framework, as opposed to the minimum mean square error (MMSE) framework that conventional methods employ. No small attitude uncertainty assumption is necessary using this method, and, therefore, a more accurate estimate of the state can be obtained when the attitude uncertainty is large. Two new state densities, termed the Gauss-Bingham and Bingham-Gauss mixture (BGM) densities, are developed that probabilistically represent a state vector comprised of an attitude quaternion and other Euclidean states on their natural manifold, the unit hypercylinder. When the Euclidean states consist of position, velocity, and angular velocity, the state vector represents the dynamic pose. An uncertainty propagation scheme is developed for a Gauss-Bingham-distributed state vector, and two demonstrations of this uncertainty propagation scheme are presented that show its applicability to quantify the uncertainty in dynamic pose, especially when the attitude uncertainty becomes large. The BGM filter is developed, which is an approximate Bayesian filter in which the true temporal and measurement evolution of the BGM density, as quantified by the Chapman-Kolmogorov equation and Bayes\u27 rule, are approximated by a BGM density. The parameters of the approximating BGM density are found via integral approximation on a component-wise basis, which is shown to be the Kullback-Leibler divergence optimal parameters of each component. The BGM filter is then applied to three simulations in order to compare its performance to a multiplicative Kalman filter and demonstrate its efficacy in estimating dynamic pose. The BGM filter is shown to be more statistically consistent than the multiplicative Kalman filter when the attitude uncertainty is large --Abstract, page iii