CHSEL: Producing Diverse Plausible Pose Estimates from Contact and Free
  Space Data
Sheng Zhong Nima Fazeli Dmitry Berenson
This paper proposes a novel method for estimating the set of plausible poses
of a rigid object from a set of points with volumetric information, such as
whether each point is in free space or on the surface of the object. In
particular, we study how pose can be estimated from force and tactile data
arising from contact. Using data derived from contact is challenging because it
is inherently less information-dense than visual data, and thus the pose
estimation problem is severely under-constrained when there are few contacts.
Rather than attempting to estimate the true pose of the object, which is not
tractable without a large number of contacts, we seek to estimate a plausible
set of poses which obey the constraints imposed by the sensor data. Existing
methods struggle to estimate this set because they are either designed for
single pose estimates or require informative priors to be effective. Our
approach to this problem, Constrained pose Hypothesis Set Elimination (CHSEL),
has three key attributes: 1) It considers volumetric information, which allows
us to account for known free space; 2) It uses a novel differentiable
volumetric cost function to take advantage of powerful gradient-based
optimization tools; and 3) It uses methods from the Quality Diversity (QD)
optimization literature to produce a diverse set of high-quality poses. To our
knowledge, QD methods have not been used previously for pose registration. We
also show how to update our plausible pose estimates online as more data is
gathered by the robot. Our experiments suggest that CHSEL shows large
performance improvements over several baseline methods for both simulated and
real-world data.