We consider the problem of estimating object pose and shape from an RGB-D image. Our first contribution is to introduce CRISP, a category-agnostic object pose and shape estimation pipeline. The pipeline implements an encoder-decoder model for shape estimation. It uses FiLM-conditioning for implicit shape reconstruction and a DPT-based network for estimating pose-normalized points for pose estimation. As a second contribution, we propose an optimization-based pose and shape corrector that can correct estimation errors caused by a domain gap. Observing that the shape decoder is well behaved in the convex hull of known shapes, we approximate the shape decoder with an active shape model, and show that this reduces the shape correction problem to a constrained linear least squares problem, which can be solved efficiently by an interior point algorithm. Third, we introduce a self-training pipeline to perform self-supervised domain adaptation of CRISP. The self-training is based on a correct-and-certify approach, which leverages the corrector to generate pseudo-labels at test time, and uses them to self-train CRISP. We demonstrate CRISP (and the self-training) on YCBV, SPE3R, and NOCS datasets. CRISP shows high performance on all the datasets. Moreover, our self-training is capable of bridging a large domain gap. Finally, CRISP also shows an ability to generalize to unseen objects. Code, pre-trained models and videos of sample results are available on the project webpage.1

While global point cloud registration systems have advanced significantly in all aspects, many studies have focused on specific components, such as feature extraction, graph-theoretic pruning, or pose solvers. In this paper, we take a holistic view on the registration problem and develop an open-source and versatile C++ library for point cloud registration, called KISS-Matcher. KISS-Matcher combines a novel feature detector, Faster-PFH, that improves over the classical fast point feature histogram (FPFH). Moreover, it adopts a k-core-based graph-theoretic pruning to reduce the time complexity of rejecting outlier correspondences. Finally, it combines these modules in a complete, user-friendly, and ready-to-use pipeline. As verified by extensive experiments, KISS-Matcher has superior scalability and broad applicability, achieving a substantial speed-up compared to state-of-the-art outlier-robust registration pipelines while preserving accuracy. Our code will be available at https://github.com/MIT-SPARK/KISS-Matcher.

Deep learning has had remarkable success in robotic perception, but its data-centric nature suffers when it comes to generalizing to ever-changing environments. By contrast, physics-based optimization generalizes better, but it does not perform as well in complicated tasks due to the lack of high-level semantic information and reliance on manual parametric tuning. To take advantage of these two complementary worlds, we present PyPose: a robotics-oriented, PyTorch-based library that combines deep perceptual models with physics-based optimization. PyPose's architecture is tidy and well-organized, it has an imperative style interface and is efficient and user-friendly, making it easy to integrate into real-world robotic applications. Besides, it supports parallel computing of any order gradients of Lie groups and Lie algebras and 2nd-order optimizers, such as trust region methods. Experiments show that PyPose achieves more than 10× speedup in computation compared to the state-of-the-art libraries. To boost future research, we provide concrete examples for several fields of robot learning, including SLAM, planning, control, and inertial navigation.

We present Loc-NeRF, a real-time vision-based robot localization approach that combines Monte Carlo localization and Neural Radiance Fields (NeRF). Our system uses a pre-trained NeRF model as the map of an environment and can localize itself in real-time using an RGB camera as the only exteroceptive sensor onboard the robot. While neural radiance fields have seen significant applications for visual rendering in computer vision and graphics, they have found limited use in robotics. Existing approaches for NeRF-based localization require both a good initial pose guess and significant computation, making them impractical for real-time robotics applications. By using Monte Carlo localization as a workhorse to estimate poses using a NeRF map model, LocNeRF is able to perform localization faster than the state of the art and without relying on an initial pose estimate. In addition to testing on synthetic data, we also run our system using real data collected by a Clearpath Jackal UGV and demonstrate for the first time the ability to perform real-time and global localization (albeit over a small workspace) with neural radiance fields. We make our code publicly available at https://github.com/MIT-SPARK/Loc-NeRF.

In this article, we consider acategory-level perceptionproblem, where one is given 2-D or 3-D sensor data picturing an object of a given category (e.g., a car) and has to reconstruct the 3-D pose and shape of the object despite intraclass variability (i.e., different car models have different shapes). We consider anactive shape model, where—for an object category—we are given a library of potential computer-aided design models describing objects in that category, and we adopt a standard formulation where pose and shape are estimated from 2-D or 3-D keypoints via nonconvex optimization. Our first contribution is to developPACE3D$^{\star }$andPACE2D$^{\star }$, the firstcertifiably optimalsolvers for pose and shape estimation using 3-D and 2-D keypoints, respectively. Both the solvers rely on the design of tight (i.e., exact) semidefinite relaxations. Our second contribution is to develop outlier-robust versions of both the solvers, namedPACE3D# andPACE2D#. Toward this goal, we propose ROBIN(Reject Outliers Based on INvariants), a general graph-theoretic framework to prune outliers, which usescompatibility hypergraphsto model measurements' compatibility. We show that in category-level perception problems, these hypergraphs can be built from the winding orders of the keypoints (in 2-D) or their convex hulls (in 3-D), and many outliers can be filtered out via maximum hyperclique computation. The last contribution is an extensive experimental evaluation. Besides providing an ablation study on simulated datasets and on thePASCAL3D+ dataset, we combine our solver with a deep keypoint detector and show thatPACE3D# improves over the state of the art in vehicle pose estimation in theApolloScapedatasets, and its runtime is compatible with practical applications. We release our code athttps://github.com/MIT-SPARK/PACE.

Many estimation problems in robotics, computer vision, and learning require estimating unknown quantities in the face of outliers. Outliers are typically the result of incorrect data association or feature matching, and it is not uncommon to have problems where more than 90% of the measurements used for estimation are outliers. While current approaches for robust estimation (e.g., RANSAC or graduated non-convexity) are able to deal with moderate amounts of outliers, they fail to produce accurate estimates in the presence of many outliers. This paper develops an approach to prune outliers. First, we develop a theory of invariance that allows us to quickly check if a subset of measurements are mutually compatible without explicitly solving the corresponding estimation problem. Second, we develop a graph-theoretic framework, where measurements are modeled as vertices and mutual compatibility is captured by edges in a graph. We generalize existing results showing that the inliers form a clique in this compatibility graph and typically belong to the maximum clique. We also show that in practice the maximum k-core of the compatibility graph provides an approximation of the maximum clique, while being much faster to compute in large problems. The combination of these two contributions leads to ROBIN, our approach to Reject Outliers Based on INvariants, which allows us to quickly prune outliers in generic estimation problems. We demonstrate ROBIN in four geometric perception problems and show it boosts robustness of existing solvers (making them robust to more than 95% outliers), while running in milliseconds in large problems.

We propose the first fast and certifiable algorithm for the registration of two sets of three-dimensional (3-D) points in the presence of large amounts of outlier correspondences. Acertifiable algorithmis one that attempts to solve an intractable optimization problem (e.g., robust estimation with outliers) and provides readily checkable conditions to verify if the returned solution is optimal (e.g., if the algorithm produced the most accurate estimate in the face of outliers) or bound its suboptimality or accuracy. Toward this goal, we first reformulate the registration problem using atruncated least squares(TLS) cost that makes the estimation insensitive to a large fraction of spurious correspondences. Then, we provide a general graph-theoretic framework to decouple scale, rotation, and translation estimation, which allows solving in cascade for the three transformations. Despite the fact that each subproblem (scale, rotation, and translation estimation) is still nonconvex and combinatorial in nature, we show that 1) TLS scale and (component-wise) translation estimation can be solved in polynomial time via anadaptive votingscheme, 2) TLS rotation estimation can be relaxed to a semidefinite program (SDP) and the relaxation is tight, even in the presence of extreme outlier rates, and 3) the graph-theoretic framework allows drastic pruning of outliers by finding the maximum clique. We name the resulting algorithm TEASER (Truncated least squares Estimation And SEmidefinite Relaxation). While solving large SDP relaxations is typically slow, we develop a second fast and certifiable algorithm, named TEASER++, that usesgraduated nonconvexityto solve the rotation subproblem and leveragesDouglas-Rachford Splittingto efficiently certify global optimality. For both algorithms, we provide theoretical bounds on the estimation errors, which are the first of their kind for robust registration problems. Moreover, we test their performance on standard benchmarks, object detection datasets, and the3DMatchscan matching dataset, and show that 1) both algorithms dominate the state-of-the-art (e.g., RANSAC, branch-&-bound, heuristics) and are robust to more than$\text{99}\%$outliers when the scale is known, 2) TEASER++ can run in milliseconds and it is currently the fastest robust registration algorithm, and 3) TEASER++ is so robust it can also solve problems without correspondences (e.g., hypothesizing all-to-all correspondences), where it largely outperforms ICP and it is more accurate than Go-ICP while being orders of magnitude faster. We release a fast open-source C++ implementation of TEASER++.

This paper presents an underwater robotic sensor system for localizing acoustic transmitters when the robot's hydrophones cannot be time-synchronized. The development of the system is motivated by applications where tracking of marine animals that are tagged with an underwater acoustic transmitter is required. The system uses two novel real-time calibration algorithms that improve the accuracy of time of flight (TOF) and time difference of arrival (TDOA) measurements. The first algorithm corrects non-linear clock skews in TOF measurements based on temperature variation. The second algorithm compensates the localized relative clock skew between clocks using a mixed integer linear program. To validate the system's performance, an Autonomous Underwater Vehicle (AUV) was deployed to track a moving tag where GPS data was used as ground truth. Compared to traditional TOF and TDOA filtering methods, the results show that the proposed system can achieve reduction of mean localization errors by 59%, and a reduction of the standard deviation of measurements by 44%.