In-Rack Test Tube Pose Estimation Using RGB-D Data

Early object detection methods relied on handcrafted features [5][6] along with SVM classifiers [7]. However, traditional methods lacked of effective image representation, subsequently limiting accuracy. The advent of convolutional neural networks (CNNs) changed this landscape dramatically [8]. R-CNN [9] pioneered region proposals for localization coupled with CNNs for classification. Later one-stage detectors like YOLO [10] eliminated region proposals, using a single network for bounding box prediction and classification. Transformers like DETR have also emerged, replacing hand-designed anchors and non-maximum suppression (NMS) with attention mechanisms. Modern object detectors [11][12] can achieve high accuracy while maintaining real-time performance.

In this paper, we employ YOLOv5 [13] to classify and localize the tube rack and various test tubes in 2D images. YOLOv5 excels in both speed and accuracy for object detection tasks. While learning-based object detectors require extensive training data to perform well, acquiring such data is a laborious and time-consuming process. In our previous work, we introduced an intuitive robotic data collection system that efficiently gathers training data for the object detector without requiring human intervention [14].

Pose estimation methods can be broadly categorized into two approaches: feature-based methods and template-based methods.

In feature-based methods, early research focused on estimating poses by establishing correspondences between the 2D input image and the 3D object model through the matching of 2D handcrafted features [1, 15]. However, these 2D feature matching methods tend to struggle when dealing with texture-less or symmetric objects. With the advent of consumer-grade depth sensors like Kinect and Realsense, feature-based methods such as [16, 17] shifted towards establishing correspondences between point clouds and 3D models using 3D handcrafted features. Nonetheless, similar to methods based on 2D features, those relying on 3D features also exhibit performance limitations when dealing with objects exhibiting symmetrical properties.

In template-based methods, a prototype image representation—known as a template—is compared to various locations within the input image. At each location, a similarity score is computed, and the best match is determined by comparing these similarity scores. A popular example of this approach is LINEMOD [2], where each template is represented by a map of color gradient orientations and surface normal orientations. However, it’s worth noting that template matching methods are prone to issues of occlusion between objects, as the similarity score of the template is likely to be low if the object is occluded.

Recently, the integration of deep learning has significantly enhanced the performance of 6D pose estimation algorithms. PoseCNN [18], a deep learning architecture, garnered attention for its proficiency in predicting 3D object poses from single RGB images. Subsequently, methods like YOLO6d [4] have enabled real-time 6D pose estimation. Nonetheless, it’s important to highlight that learning-based methods demand substantial training data to achieve satisfactory performance. This requirement can be time-consuming and often necessitates substantial manual effort for data labeling.

In this study, we proposed a feature-based method for in-rack test tube pose estimation. The method evaluates the test tube poses from point clouds based on a cylinder shaped feature fitting and novelly incorporating the correlation between each tube and its corresponding rack slot as a prior constraint. Even when the point cloud of a test tube is noisy or incomplete, our proposed method can still perform well. The experimental results clearly demonstrate the advantages of this method for detecting in-rack test tubes, as compared to prior work.

In this subsection, we present our method for estimating the pose of the test tube rack using point cloud template matching. As shown in Fig. 3 (c), we initially create a template using the top surface point cloud of the test tube rack. This template is chosen due to its dense point distribution and minimal occlusions.

The process of estimating the test tube rack pose involves two key steps. The initial phase calculates an approximate transformation between the template point cloud and the point cloud acquired from the vision sensor. To achieve this, we employ the Oriented Bounding Box (OBB) algorithm, a well-established technique that identifies the box with the least area containing all the given points. For reference, Fig. 3 (a) and (b) provide an instance of the OBB applied to a point cloud cluster. The pose of the OBB serves as an initial estimate for the template point cloud’s position.

Subsequently, we enhance the transformation using the ICP algorithm. ICP is employed to iteratively minimize the reprojection error associated with fixed correspondences between the template point cloud and the captured point cloud until convergence is reached. This process ensures an accurate estimation of the test tube rack’s pose. To visualize the results, Fig.  4 demonstrates the rough estimate derived from OBB and the refined estimate achieved through ICP. Notably, the yellow point cloud represents data from the vision sensor, while the blue point cloud depicts the template. Once the pose of the test tube rack is determined, we can subsequently deduce the poses of the individual test tubes from this information.

In this subsection, we present the method to estimate the poses of test tubes. The method firstly finds out the containing hole for each test tube by projecting the convex hull of its point cloud on the top surface of the test tube rack. Since we have already known the pose of the test tube rack, the position of each hole on the rack can also be concluded from it. Fig. 7(b) is the projection result of the example. The gray rectangles are the hole. The blue convex shapes are the projection of the convex hull of the test tube point cloud. The test tube is located in the hole containing the largest partition of the area of the its convex shape.

Support we have $N$ test tubes on the rack. The point cloud of $i$th test tube is notated as $\mathbf{P}^{i}=\{\mathbf{p}_{1}^{i},\ldots,\mathbf{p}_{j}^{i},\ldots\mathbf{p}_{M_{i}}^{i}\}$, where $\mathbf{p}_{j}^{i}\in\mathbb{R}^{3}$, $M_{i}$ is the number of points in the $i$th point cloud. The radius of $i$th test tube is $r_{i}$. Suppose the dimension of holes of the test tube rack are the same, and the notions of length, width, height are $2L$, $2W$, $H$ respectively.

As shown in Fig. 5, the local coordinate of the test tube is attached to the central bottom. To estimate the pose of the $i$th test tube, we use two parameters $\alpha_{i}$, $\beta_{i}$ to represent the orientation, where $\alpha_{i}$, $\beta_{i}$ are the Euler angles of $x$ axis and $y$ axis of the $i$th test tube coordinate. Then the orientation of the $i$th test tube can be defined as:

	$\displaystyle\mathbf{R}(\alpha_{i},\beta_{i})$	$\displaystyle=\begin{bmatrix}cos\beta_{i}&0&sin\beta_{i}\\ 0&1&0\\ -sin\beta_{i}&0&cos\beta_{i}\end{bmatrix}\begin{bmatrix}1&0&0\\ 0&cos\alpha_{i}&-sin\alpha_{i}\\ 0&sin\alpha_{i}&cos\alpha_{i}\end{bmatrix}$		(1)
		$\displaystyle=\begin{bmatrix}cos\beta_{i}&sin\beta_{i}sin\alpha_{i}&sin\beta_{i}cos\alpha_{i}\\ 0&cos\alpha_{i}&-sin\alpha_{i}\\ -sin\beta_{i}&\cos{\beta_{i}}sin\alpha_{i}&cos\beta_{i}cos\alpha_{i}\end{bmatrix}$

The distance between the $\mathbf{p}_{j}$ to the line segment that starts from origin point and points to $z$ direction of $i$th test tube coordinate is represented as:

$$D(\alpha_{i},\beta_{i},\mathbf{p}_{j},\mathbf{o}_{i})=||(\mathbf{o}_{i}-\mathbf{p}_{j})\times\begin{bmatrix}sin\beta_{i}cos\alpha_{i}\\ -sin\alpha_{i}\\ cos\beta_{i}cos\alpha_{i}\end{bmatrix}||$$

(2)

where $\mathbf{o}_{i}$ is the origin point of the $i$th test tube coordinate in the world coordinate, which can be concluded from the pose of the test tube rack. We use a cylinder that has the same radius as the test tube to represent the simplified geometry feature for the test tube. The pose of the cylinder is considered to be the same as the test tube. The orientation of the cylinder is controlled by the parameter $\alpha$ and $\beta$. Although some parts of the point cloud of the transparent test tube miss, the remaining part of the point cloud still has enough features for the cylinder to fit. Fig. 6 shows an example of using the cylinder to fit the point cloud. We suppose that the cylinder is in the correct pose when the point cloud is located near the surface of the cylinder. By rotating the cylinder, we can determine the optimal pose for the cylinder that point cloud is well distributed near its surface. The following equation is used to find out the pose.

$\displaystyle\min_{\alpha,\beta\in(-\pi,\pi]}$

$\displaystyle\sum_{k=1}^{M_{i}}(D(\alpha_{i},\beta_{i},\mathbf{p}_{k},\mathbf{o}_{i})-r_{i})$

(3)

We define $\alpha_{i}^{*}$ and $\beta_{i}^{*}$ as the solution to minimize the distance between extracted point cloud and the surface of the simplified cylinder model. The final pose of $i$th test tube is

$$i\text{th test tube pose}=\begin{bmatrix}\mathbf{R}(\alpha_{i}^{*},\beta_{i}^{*})&\mathbf{o}_{i}\\ \mathbf{0}&1\end{bmatrix}$$

(4)

Fig. 7(c) shows a result of the pose estimation of the example. In addition, we need to define a function to reject wrong cases. For the wrong cases that point clouds of test tubes are seriously corrupted, the simplified cylinder geometry feature no longer exists in the point cloud. So the estimated pose is meaningless. The meaningless estimated pose can be effectively found by examining the geometry constraints between the rack hole and test tube. The geometry constraints tell us the $\alpha_{i}$ and $\beta_{i}$ should be bound in the following range so that the $i$th test tube and test tube rack have no overlap.

		$\displaystyle tan(\alpha_{i})(W-\frac{r_{i}}{sin\alpha_{i}})>H$		(5)
		$\displaystyle tan(\pi-\alpha_{i})(W-\frac{r_{i}}{sin(\pi-\alpha_{i})})>H$
		$\displaystyle tan(\beta_{i})(L-\frac{r_{i}}{sin\beta_{i}})>H$
		$\displaystyle tan(\pi-\beta_{i})(L-\frac{r_{i}}{sin(\pi-\beta_{i})})>H$

In this section, we have assessed our proposed pose estimation algorithm for test tubes. Fig. 8(a) depicts the experimental setup employed for evaluating the accuracy of our pose estimation approach. In this configuration, a Photoneo Phoxi M 3D scanner is positioned above. The test tubes and rack is positioned on a flat surface table.

We conducted evaluations on three types of test tubes. These test tubes, along with their corresponding point clouds, are illustrated in Fig. 8(b). Among these, the “Tube 1” posed the greatest challenge due to its semi-transparent materials, resulting in corrupted and incomplete point cloud data. In contrast, the remaining two test tubes are with opaque tube caps, resulting in a relatively simpler detection process.

To evaluate the poses of the test tubes, we designed custom 3D-printed caps, as shown in Fig. 9(a). Each cap featured a square surface at its top. The square top surface can be easily extracted by employing plane segmentation algorithms. After extraction, these square features were employed as target point clouds, which served as a basis for fitting a square point cloud template via the ICP algorithm. This process yielded accurate transformations that were employed as the ground truth for the orientations of the test tubes. To further determine the translation of the test tubes, we measured the physical distance between the center of the square top surface and the origin point of the test tubes. With the ground truth data in hand, we proceeded to replace the 3D-printed caps with the original caps of the test tubes, while maintaining the test tube’s pose unchanged. Subsequently, we performed scans to capture point cloud data for the test tubes with their original tube caps. This approach enabled a comprehensive comparison of our pose estimation algorithm’s performance under realistic conditions. Note that while a highly accurate sensor isn’t strictly required for our pose estimation algorithm, for the purpose of our evaluations, we opted for a high-precision sensor to obtain a point cloud in order to acquire a point cloud with minimal distortion. This choice ensured the establishment of a precise ground truth for the poses of the test tubes.

Regarding the evaluation metrics, we assessed the average rotational and translational errors along each axis. To compute the rotational error, we measured the angle between the quaternion of the attitude parameter (converted to Euler angles) and the correct angle for each rotation axis. Notably, since we defined the rotational symmetric axis for the test tube as the Z-axis, our assessment solely focused on the rotational error along the X and Y axes. On the other hand, the translation error was determined as the disparity between the translations observed along each axis.

The results of the pose estimation for the test tubes are presented in Table I. To facilitate a comprehensive evaluation, we have chosen the popular pose estimation technique of point cloud registration for comparison. This technique involves utilizing the global registration + ICP algorithm from the Open3D library, which efficiently establishes transformation between the template point cloud and the target point cloud. The template point cloud is generated in accordance with the CAD model of the respective test tubes.

When considering the pose estimation results for ”Tube 1,” it is evident that our proposed method significantly outperforms the global registration + ICP approach. This notable improvement can be attributed to the substantial dissimilarity between the realistic point cloud of the semi-transparent ”Tube 1” and the point cloud template derived from the CAD model. This disparity often leads to the failure of global registration, consequently yielding incorrect outcomes. On the contrary, for ”Tube 2” and ”Tube 3,” the presence of opaque caps in their point clouds allows for facile identification through the cap features. Moreover, when evaluating the rotational error, incorporating constraints derived from the rack slot further reduces the rotational error. Our strategic consideration of rack-based constraints aids in mitigating the impact of noise.

However, when examining the translational error associated with ”Tube 3,” our method demonstrates a relatively larger discrepancy in translational accuracy compared to the ICP method. This variance can be attributed to our reliance on the assumption that the bottom centers of the test tubes are roughly aligned with the central rack slot. In future iterations, we plan to refine our algorithm by removing this constraint, thereby enhancing its versatility and applicability across various scenarios.

In this subsection, we evaluate the time costs of our proposed method, including the time for the object detection in 2D image and the subsequent pose estimation on 3D point cloud. The specifications of our computing setup are as follows: an Intel i9-13900K CPU and an Nvidia RTX 4090 GPU. Note that the speed of the object detection stage primarily hinges on the GPU device, while the pose estimation stage is influenced by the CPU device. The results are shown in Table II. It includes the time taken for object detection per image,, rack pose estimation time per rack, the tube estimation time per tube using our method, and tube estimation time per tube through point cloud registration, utilized for comparison.

The results illustrate that the most time-consumption part in our framework is the rack pose estimation component, which necessitates performing the ICP algorithm on an extensive set of point clouds to ensure the precision of pose estimation. Another significant observation is the notable speed enhancement achieved by our proposed tube pose estimation method, showcasing an approximate threefold increase in speed compared to the traditional point cloud registration approach. This distinction becomes particularly significant when dealing with a substantial number of test tubes placed on the rack, a scenario frequently encountered in real-world conditions.