-
Continuous-time Trajectory Estimation: A Comparative Study Between Gaussian Process and Spline-based Approaches
Authors:
Jacob Johnson,
Joshua Mangelson,
Timothy Barfoot,
Randal Beard
Abstract:
Continuous-time trajectory estimation is an attractive alternative to discrete-time batch estimation due to the ability to incorporate high-frequency measurements from asynchronous sensors while kee** the number of optimization parameters bounded. Two types of continuous-time estimation have become prevalent in the literature: Gaussian process regression and spline-based estimation. In this pape…
▽ More
Continuous-time trajectory estimation is an attractive alternative to discrete-time batch estimation due to the ability to incorporate high-frequency measurements from asynchronous sensors while kee** the number of optimization parameters bounded. Two types of continuous-time estimation have become prevalent in the literature: Gaussian process regression and spline-based estimation. In this paper, we present a direct comparison between these two methods. We first compare them using a simple linear system, and then compare them in a camera and IMU sensor fusion scenario on SE(3) in both simulation and hardware. Our results show that if the same measurements and motion model are used, the two methods achieve similar trajectory accuracy. In addition, if the spline order is chosen so that the degree-of-differentiability of the two trajectory representations match, then they achieve similar solve times as well.
△ Less
Submitted 1 February, 2024;
originally announced February 2024.
-
Group-$k$ consistent measurement set maximization via maximum clique over k-Uniform hypergraphs for robust multi-robot map merging
Authors:
Brendon Forsgren,
Ram Vasudevan,
Michael Kaess,
Timothy W. McLain,
Joshua G. Mangelson
Abstract:
This paper unifies the theory of consistent-set maximization for robust outlier detection in a simultaneous localization and map** framework. We first describe the notion of pairwise consistency before discussing how a consistency graph can be formed by evaluating pairs of measurements for consistency. Finding the largest set of consistent measurements is transformed into an instance of the maxi…
▽ More
This paper unifies the theory of consistent-set maximization for robust outlier detection in a simultaneous localization and map** framework. We first describe the notion of pairwise consistency before discussing how a consistency graph can be formed by evaluating pairs of measurements for consistency. Finding the largest set of consistent measurements is transformed into an instance of the maximum clique problem and can be solved relatively quickly using existing maximum-clique solvers. We then generalize our algorithm to check consistency on a group-$k$ basis by using a generalized notion of consistency and using generalized graphs. We also present modified maximum clique algorithms that function on generalized graphs to find the set of measurements that is internally group-$k$ consistent. We address the exponential nature of group-$k$ consistency and present methods that can substantially decrease the number of necessary checks performed when evaluating consistency. We extend our prior work to multi-agent systems in both simulation and hardware and provide a comparison with other state-of-the-art methods.
△ Less
Submitted 4 August, 2023;
originally announced August 2023.
-
Group-$k$ Consistent Measurement Set Maximization for Robust Outlier Detection
Authors:
Brendon Forsgren,
Ram Vasudevan,
Michael Kaess,
Timothy W. McLain,
Joshua G. Mangelson
Abstract:
This paper presents a method for the robust selection of measurements in a simultaneous localization and map** (SLAM) framework. Existing methods check consistency or compatibility on a pairwise basis, however many measurement types are not sufficiently constrained in a pairwise scenario to determine if either measurement is inconsistent with the other. This paper presents group-$k$ consistency…
▽ More
This paper presents a method for the robust selection of measurements in a simultaneous localization and map** (SLAM) framework. Existing methods check consistency or compatibility on a pairwise basis, however many measurement types are not sufficiently constrained in a pairwise scenario to determine if either measurement is inconsistent with the other. This paper presents group-$k$ consistency maximization (G$k$CM) that estimates the largest set of measurements that is internally group-$k$ consistent. Solving for the largest set of group-$k$ consistent measurements can be formulated as an instance of the maximum clique problem on generalized graphs and can be solved by adapting current methods. This paper evaluates the performance of G$k$CM using simulated data and compares it to pairwise consistency maximization (PCM) presented in previous work.
△ Less
Submitted 6 September, 2022;
originally announced September 2022.
-
ShapeMap 3-D: Efficient shape map** through dense touch and vision
Authors:
Sudharshan Suresh,
Zilin Si,
Joshua G. Mangelson,
Wenzhen Yuan,
Michael Kaess
Abstract:
Knowledge of 3-D object shape is of great importance to robot manipulation tasks, but may not be readily available in unstructured environments. While vision is often occluded during robot-object interaction, high-resolution tactile sensors can give a dense local perspective of the object. However, tactile sensors have limited sensing area and the shape representation must faithfully approximate n…
▽ More
Knowledge of 3-D object shape is of great importance to robot manipulation tasks, but may not be readily available in unstructured environments. While vision is often occluded during robot-object interaction, high-resolution tactile sensors can give a dense local perspective of the object. However, tactile sensors have limited sensing area and the shape representation must faithfully approximate non-contact areas. In addition, a key challenge is efficiently incorporating these dense tactile measurements into a 3-D map** framework. In this work, we propose an incremental shape map** method using a GelSight tactile sensor and a depth camera. Local shape is recovered from tactile images via a learned model trained in simulation. Through efficient inference on a spatial factor graph informed by a Gaussian process, we build an implicit surface representation of the object. We demonstrate visuo-tactile map** in both simulated and real-world experiments, to incrementally build 3-D reconstructions of household objects.
△ Less
Submitted 10 March, 2022; v1 submitted 20 September, 2021;
originally announced September 2021.
-
Tactile SLAM: Real-time inference of shape and pose from planar pushing
Authors:
Sudharshan Suresh,
Maria Bauza,
Kuan-Ting Yu,
Joshua G. Mangelson,
Alberto Rodriguez,
Michael Kaess
Abstract:
Tactile perception is central to robot manipulation in unstructured environments. However, it requires contact, and a mature implementation must infer object models while also accounting for the motion induced by the interaction. In this work, we present a method to estimate both object shape and pose in real-time from a stream of tactile measurements. This is applied towards tactile exploration o…
▽ More
Tactile perception is central to robot manipulation in unstructured environments. However, it requires contact, and a mature implementation must infer object models while also accounting for the motion induced by the interaction. In this work, we present a method to estimate both object shape and pose in real-time from a stream of tactile measurements. This is applied towards tactile exploration of an unknown object by planar pushing. We consider this as an online SLAM problem with a nonparametric shape representation. Our formulation of tactile inference alternates between Gaussian process implicit surface regression and pose estimation on a factor graph. Through a combination of local Gaussian processes and fixed-lag smoothing, we infer object shape and pose in real-time. We evaluate our system across different objects in both simulated and real-world planar pushing tasks.
△ Less
Submitted 26 March, 2021; v1 submitted 13 November, 2020;
originally announced November 2020.
-
Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra
Authors:
Joshua G. Mangelson,
Maani Ghaffari,
Ram Vasudevan,
Ryan M. Eustice
Abstract:
An accurate characterization of pose uncertainty is essential for safe autonomous navigation. Early pose uncertainty characterization methods proposed by Smith, Self, and Cheeseman (SCC), used coordinate-based first-order methods to propagate uncertainty through non-linear functions such as pose composition (head-to-tail), pose inversion, and relative pose extraction (tail-to-tail). Characterizing…
▽ More
An accurate characterization of pose uncertainty is essential for safe autonomous navigation. Early pose uncertainty characterization methods proposed by Smith, Self, and Cheeseman (SCC), used coordinate-based first-order methods to propagate uncertainty through non-linear functions such as pose composition (head-to-tail), pose inversion, and relative pose extraction (tail-to-tail). Characterizing uncertainty in the Lie Algebra of the special Euclidean group results in better uncertainty estimates. However, existing approaches assume that individual poses are independent. Since factors in a pose graph induce correlation, this independence assumption is usually not reflected in reality. In addition, prior work has focused primarily on the pose composition operation. This paper develops a framework for modeling the uncertainty of jointly distributed poses and describes how to perform the equivalent of the SSC pose operations while characterizing uncertainty in the Lie Algebra. Evaluation on simulated and open-source datasets shows that the proposed methods result in more accurate uncertainty estimates. An accompanying C++ library implementation is also released.
This is a pre-print of a paper submitted to IEEE TRO in 2019.
△ Less
Submitted 18 June, 2019;
originally announced June 2019.
-
Guaranteed Globally Optimal Planar Pose Graph and Landmark SLAM via Sparse-Bounded Sums-of-Squares Programming
Authors:
Joshua G. Mangelson,
**sun Liu,
Ryan M. Eustice,
Ram Vasudevan
Abstract:
Autonomous navigation requires an accurate model or map of the environment. While dramatic progress in the prior two decades has enabled large-scale SLAM, the majority of existing methods rely on non-linear optimization techniques to find the MLE of the robot trajectory and surrounding environment. These methods are prone to local minima and are thus sensitive to initialization. Several recent pap…
▽ More
Autonomous navigation requires an accurate model or map of the environment. While dramatic progress in the prior two decades has enabled large-scale SLAM, the majority of existing methods rely on non-linear optimization techniques to find the MLE of the robot trajectory and surrounding environment. These methods are prone to local minima and are thus sensitive to initialization. Several recent papers have developed optimization algorithms for the Pose-Graph SLAM problem that can certify the optimality of a computed solution. Though this does not guarantee a priori that this approach generates an optimal solution, a recent extension has shown that when the noise lies within a critical threshold that the solution to the optimization algorithm is guaranteed to be optimal. To address the limitations of existing approaches, this paper illustrates that the Pose-Graph SLAM and Landmark SLAM can be formulated as polynomial optimization programs that are SOS convex. This paper then describes how the Pose-Graph and Landmark SLAM problems can be solved to a global minimum without initialization regardless of noise level using the Sparse-BSOS hierarchy. This paper also empirically illustrates that convergence happens at the second step in this hierarchy. In addition, this paper illustrates how this Sparse-BSOS hierarchy can be implemented in the complex domain and empirically shows that convergence happens also at the second step of this complex domain hierarchy. Finally, the superior performance of the proposed approach when compared to existing SLAM methods is illustrated on graphs with several hundred nodes.
△ Less
Submitted 15 March, 2022; v1 submitted 20 September, 2018;
originally announced September 2018.
-
Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors
Authors:
Ross Hartley,
Josh Mangelson,
Lu Gan,
Maani Ghaffari Jadidi,
Jeffrey M. Walls,
Ryan M. Eustice,
Jessy W. Grizzle
Abstract:
State-of-the-art robotic perception systems have achieved sufficiently good performance using Inertial Measurement Units (IMUs), cameras, and nonlinear optimization techniques, that they are now being deployed as technologies. However, many of these methods rely significantly on vision and often fail when visual tracking is lost due to lighting or scarcity of features. This paper presents a state-…
▽ More
State-of-the-art robotic perception systems have achieved sufficiently good performance using Inertial Measurement Units (IMUs), cameras, and nonlinear optimization techniques, that they are now being deployed as technologies. However, many of these methods rely significantly on vision and often fail when visual tracking is lost due to lighting or scarcity of features. This paper presents a state-estimation technique for legged robots that takes into account the robot's kinematic model as well as its contact with the environment. We introduce forward kinematic factors and preintegrated contact factors into a factor graph framework that can be incrementally solved in real-time. The forward kinematic factor relates the robot's base pose to a contact frame through noisy encoder measurements. The preintegrated contact factor provides odometry measurements of this contact frame while accounting for possible foot slippage. Together, the two developed factors constrain the graph optimization problem allowing the robot's trajectory to be estimated. The paper evaluates the method using simulated and real sensory IMU and kinematic data from experiments with a Cassie-series robot designed by Agility Robotics. These preliminary experiments show that using the proposed method in addition to IMU decreases drift and improves localization accuracy, suggesting that its use can enable successful recovery from a loss of visual tracking.
△ Less
Submitted 24 February, 2018; v1 submitted 15 December, 2017;
originally announced December 2017.
