Pose Estimation of Primitive-Shaped Objects from a Depth Image Using Superquadric Representation

Abstract

This paper presents a method for estimating the six Degrees of Freedom (6DoF) pose of texture-less primitive-shaped objects from depth images. As the conventional methods for object pose estimation require rich texture or geometric features to the target objects, these methods are not suitable for texture-less and geometrically simple shaped objects. In order to estimate the pose of the primitive-shaped object, the parameters that represent primitive shapes are estimated. However, these methods explicitly limit the number of types of primitive shapes that can be estimated. We employ superquadrics as a primitive shape representation that can represent various types of primitive shapes with only a few parameters. In order to estimate the superquadric parameters of primitive-shaped objects, the point cloud of the object must be segmented from a depth image. It is known that the parameter estimation is sensitive to outliers, which are caused by the miss-segmentation of the depth image. Therefore, we propose a novel estimation method for superquadric parameters that are robust to outliers. In the experiment, we constructed a dataset in which the person grasps and moves the primitive-shaped objects. The experimental results show that our estimation method outperformed three conventional methods and the baseline method.

1. Introduction

The 3D pose estimation and tracking of objects plays an important role in object grasping by robots, scene understanding, augmented/virtual reality, and other applications. In the computer vision field, numerous methods have been proposed to estimate the six Degrees of Freedom (DoF) pose of the object from an RGB image [1,2,3] or depth image [4,5,6]. Most approaches extract handcrafted features [4,5] or learned features [1,2]. Although the feature-based method is powerful for various types of objects, the method requires rich textures or rich geometric features on the objects in order to detect feature points for matching.

The 3D objects can be tracked by estimating the sequential six Degrees of Freedom (6DoF) pose of the object. To estimate the object pose between successive frames, Iterative Closest Point (ICP) [7] is widely employed [8,9]. ICP registers two point clouds by minimizing the Euclidean distance between corresponding points. However, when the ICP algorithm is applied to the objects that have a limited number of geometrical features, the pose estimation is not accurate and unstable due to the difficulty in obtaining the correct corresponding keypoints. In this paper, we aim to tackle the problem of pose estimation against geometrically simple (primitive-shaped), texture-less objects from sequential depth images. In this case, the above feature point-based methods and ICP pose estimation are unsuitable.

As primitive shapes can be represented by just few parameters, a model fitting method, such as RANdom SAmple Consensus (RANSAC) [10] or Hough voting algorithm [11], can be applied to estimate the pose and shape parameters of primitive-shaped objects. For example, as part of the shape parameters of primitive-shaped objects, three parameters (height, width, and depth) exist to represent a cuboid, two parameters (height and radius) exist to represent a cylinder, and one parameter (radius) exists to represent a sphere. These parameters can be estimated by setting the cost function against each primitive shape representation. However, these methods for primitive shape pose estimation explicitly use limited kinds of shape representation, which also limits the application of the pose estimation of primitive-shaped objects.

Superquadric is one of an ideal shape representation for adapting various kinds of shapes using a single equation [12]. Applying the superquadric to an object enables the object to be expressed by various primitive shapes—such as cuboids, cylinders, and spheres—with several parameters in the equation. As we aim to estimate the pose of geometrically simple objects, we assume that the objects can be represented by superquadrics.

A naive approach to estimating the pose of objects that are represented by superquadrics is to apply the method proposed by Solina et al. [13]. They proposed a method to estimate the superquadric parameters and 6DoF pose parameters from the 3D point cloud of the object. After extracting the 3D point cloud of the object from the depth image, superquadric and pose parameters can be estimated using their method. However, the superquadric parameter estimation methods takes the input of the 3D point cloud of the object at the optimization, thereby requiring the segmentation against the obtained depth image the result of parameter estimation also relies on the quality of depth image segmentation. That is, if the object point clouds have outlier points that are caused by miss-segmentation, the parameters that fit the outliers are estimated, and these points would lead to a low accuracy of the pose estimation.

In order to achieve a robust superquadric pose estimation, it is important to exclude the 3D points that are not 3D points of the objects. One simple idea to exclude the outlier point is to employ the threshold value to cut off those points that are further from the object centroid. However, the threshold value differs among objects in accordance with their scale and shape, thereby defining the threshold hyper parameter against each object. Therefore, we introduce the coefficient that implicitly down-weighs outlier points and is invariant to the shape and scale of objects.

In this paper, we present a method for estimating the 6DoF pose of primitive-shaped objects from sequential depth images. In order to estimate the pose parameters, we propose the novel pose prediction method of primitive-shaped objects using superquadric representation that is robust to outliers and irrelevant to the shape or scale of the object. The results of the proposed method is shown in Figure 1. At the initial frame, we geometrically segment the depth image using normal vector concavities and depth continuities. Second, we label the binary segmented depth image by using the connected component algorithm. Thereafter, we estimate the superquadric shape, scale, and pose parameters to the each primitive-shaped object. In successive frames, we match the label map at the initial frame and the current frame to find the object, and we update the pose parameter of each primitive-shaped object.

As our method enables us to estimate the pose of superquadrics even if the outlier points exist in the object point cloud, our method can handle a case in which a person freely moves objects. In such a case, miss-segmentation can occur easily. In the experiment, we captured four scenes in which the user interacts with four primitive-shaped objects to show the robustness of our proposed method. We compare the pose estimation results with two baseline methods. The experimental results show that our method outperformed two baseline methods, thereby verifying the effectiveness of our proposed method.

2. Related Work

As our work is about primitive shape pose estimation using superquadric representation, we introduce the pose estimation of primitive shapes and research using superquadric representation.

2.1. Pose Estimation of Primitive-Shaped Objects

Recently, due to the development of the deep learning technique, 6DoF pose estimation using Convolutional Neural Networks from a single RGB image have been well developed [1,14]. However, as we aim to estimate the pose of visually simple (no color) objects, RGB information does not contribute to the estimation. The object pose can be also estimated from a single depth image. Moreover, the pose estimation methods based on learnable features [1,14] or handcrafted features [4,15] require rich textures or rich geometric keypoints. As we aim to estimate the pose of geometrically and visually simple objects, these approaches are unsuitable to employ.

Numerous approaches of primitive shape pose estimation employed the RANSAC algorithm to estimate the pose of the primitives [16,17,18,19,20,21,22]. Ana et al. [19] fit the parameters of plane, sphere, and cylinder to object point cloud by M-estimation SAmple and Consensus (MSAC) [23] and the final decision on the selection of the primitive shape is taken based on number of inliers during estimation. Drost et al. [22] employ a local Hough Transform algorithm to estimate pre-defined geometrically primitive shapes. Most approaches define primitive shapes in each. For example, three parameters (width, height, and depth) are needed to represent a cuboid, two parameters (radius and height) are needed to represent a cylinder. As primitive-shaped objects are represented in all previous work, only limited types of primitive shapes can be handled. In contrast, our method can handle any primitive shapes as long as it can be represented by superquadrics.

Sano et al. [16] proposed a method to estimate the pose of a fixed-size cuboid for Spatial Augmented Reality. Although they handle a single type of primitive shape, they propose a method that estimates the pose of a cube sequentially, while the user interactively moves the target object. They estimate the pose parameter of cubes by efficient planar region detection using RGB-D superpixel segmentation. Their method has two main limitations: first, the size of the objects is known beforehand and, second, they only can estimate the pose and track the cuboid. Our work overcomes these two limitations.

2.2. Superquadrics

Superquadric functions are an extension of quadric surfaces and include supertoroids, superhyperboloids, and superellipsoids. Superellipsoids are most commonly used in object modeling because they define closed surfaces. Examples of elementary objects that can be represented with superellipsoids are depicted in Figure 2. Recently, superquadrics have been widely used for shape abstraction [24], object grasping [25,26], object localization [27], and object recognition [28].

A superquadric in an object-centered coordinate system can be defined by the inside–outside function with a scale parameter $(s_x,s_y,s_z)$ and a shape parameter $({\epsilon }_1,{\epsilon }_2)$:

$$F(x,y,z,\Lambda )={\left\{{\left(\frac{x}{s_x}\right)}^{\frac{2}{{\epsilon }_2}}+{\left(\frac{y}{s_y}\right)}^{\frac{2}{{\epsilon }_2}}\right\}}^{\frac{{\epsilon }_2}{{\epsilon }_1}}+{\left(\frac{z}{s_z}\right)}^{\frac{2}{{\epsilon }_1}},$$

(1)

where $\Lambda$ is a tuple as $(s_x,s_y,s_z,{\epsilon }_1,{\epsilon }_2)$. Parameters $s_x$, $s_y$, and $s_z$ are scale parameters that define the superquadric size at the x, y, and z coordinates, respectively. The superquadric function F in Equation (1) with a unit scale can be re-written with the following parametric equation:

$$\mathbf{x}(\eta ,\omega )=\left[\begin{array}{c}co{s}^{{\epsilon }_1}(\eta )co{s}^{{\epsilon }_2}(\omega )\\ co{s}^{{\epsilon }_1}(\eta )si{n}^{{\epsilon }_2}(\omega )\\ si{n}^{{\epsilon }_1}(\eta )\end{array}\right],$$

(2)

$$-\pi /2\le \eta \le \pi /2,-\pi \le \omega \le \pi .$$

Parameters ${\epsilon }_1$ and ${\epsilon }_2$ are shape representation parameters that express squares along the z axis and the x-y plane. Given a point $(x,y,z)$, if $F<1$, the point is inside the superquadric, if $F>1$, the point is outside the superquadric, and if $F=1$ the point lies on the surface of the superquadric. Further, the inside–outside description can be expressed in a generic coordinate system by adding six additional variables, representing the superquadric pose $\Phi$ (three for translation $(t_x,t_y,t_z)$ and three Euler angles for rotation $({\theta }_x,{\theta }_y,{\theta }_z)$), with a total of eleven independent variables, i.e., $q\in {\mathbb{R}}^{11}$.

3. Methodology

The overview of our proposed method is illustrated in Figure 3. At the initial frame of the scene obtained from the depth sensor, such as Kinect, superquadric and pose parameters are estimated from segmented object 3D point cloud. At the successive frames, we do not estimate both superquadric parameters and pose parameters, but we only update pose parameters due to the instability of superquadric parameter estimation.

In Section 3.1 and Section 3.2, we apply both the initial and successive frames. In Section 3.3, we apply only the initial frame, and in Section 3.4 and Section 3.5, we apply only the successive frames.

3.1. Preprocessing

First, in the preprocessing stage, a depth map ${\mathcal{D}}_t$ at current frame t is transformed into a metric vertex map ${\mathcal{V}}_t(\mathbf{u})={\mathbf{K}}^{-1}\dot{\mathbf{u}}{\mathcal{D}}_t(\mathbf{u})$, with the known camera intrinsic matrix $\mathbf{K}$, a depth map pixel $\mathbf{u}={(x,y)}^{\mathrm{T}}$ in the image domain $\mathbf{u}\in \mathsf{\Omega }\subset {\mathbb{R}}^2$ and its homogeneous representation $\dot{\mathbf{u}}$. The vertex map ${\mathcal{V}}_t(\mathbf{u})$ is then smoothed by applying a median filter. The normal map of the current frame ${\mathbb{N}}_t$ is simply generated from the vertex map ${\mathcal{V}}_t$ by a cross-product of neighborhood pixels [29].

We assume that the z-axis of all the objects are horizontal to the normal vector of the floor plane at the initial depth frame ${\mathcal{D}}_0$. The preprocessing of plane estimation to the initial depth map is applied to extract the normal vector of the floor plane ${\mathbf{n}}_p$. We apply RANSAC-based plane parameter estimation to estimate the vector ${\mathbf{n}}_p$.

3.2. Geometric Segmentation

As primitive shapes have only convex surfaces, we segment the depth map ${\mathcal{D}}_t$ into convex shapes. Tateno et al. [30] adapted the concave region penalty to the normal edge-based segmentation in their SLAM pipeline. We employ their segmentation approach. They classify each pixel irrespective of whether the pixel is an object edge by employing two operators. The first operator detects concave boundaries by computing the dot product between the normal vector of target pixel and each normal of the eight-connected neighboring pixels. The second operator takes into account the maximum 3D point-to-plane distance between the target pixel and its eight neighbors. In order to establish the threshold of the second operator, we employ an uncertainty measure computed following the noise model [31].

As a result, we obtain a binary geometric edge map ${\mathcal{B}}_t$ at frame t, ${\mathcal{B}}_t$ from the input depth frame. To the edge map ${\mathcal{B}}_t$, we apply a four-neighborhood connected component analysis algorithm to yield a label map ${\mathcal{L}}_t$, where each element $\mathbf{u}$ is associated with a segment label ${\mathcal{L}}_t(\mathbf{u})=l_j$. Unlike the method in [30], we do not segment the points on the floor plane to extract the object point cloud.

3.3. Superquadric Fitting

The i-th superquadric surface ${\mathcal{O}}_i$, which best represents the object, is estimated from given K 3D points $p_k=(x_k,y_k,z_k)$ of the i-th object’s 3D points cloud. The superquadric surface ${\mathcal{O}}_i$ is represented by ${\Lambda }_i$ and ${\Phi }_i$. The minimization of the algebraic distance from points to the superquadric surface can be solved by defining a non-linear least-squares minimization problem:

$$\underset{{\Lambda }_i,{\Phi }_i}{min}\sum _{k=0}^K{(\sqrt{s_x{s}_y{s}_z}(F^{{\epsilon }_1}({\mathrm{Tr}}_{{\Phi }_i}(p_k);{\Lambda }_i)-1))}^2,$$

(3)

where ${(F({\mathrm{Tr}}_{\Phi }(p_i);\Lambda )-1)}^2$ imposes the point to superquadric surface distance minimization, where the term $\sqrt{s_x{s}_y{s}_z}$ is proportional to the superquadric volume, compensates for the fact that the previous equation is biased toward larger superquadric surfaces. $T{r}_{\Phi }(·)$ is a rigid transformation by 6DoF pose $\Phi$. The Levenberg–Marquardt algorithm [32] is used to minimize the non-linear function Equation (3).

Moreover, Equation (3) is numerically unstable when ${\epsilon }_1,{\epsilon }_2<0.1$ and the superquadric surface has concavities with ${\epsilon }_1,{\epsilon }_2>2$. We apply constraints when minimizing Equation (3) for shape parameters $0.1\le {\epsilon }_1,{\epsilon }_2\le 2$ and for the scale parameters $s_1,s_2,s_3>0$.

As the function in Equation (3) is not a convex function, the initial parameters determine on which local minimum the minimization converges. It is important to estimate the rough parameters: translation, rotation, scale, and shape parameters. First, as it is difficult to roughly estimate the shape of the object, the initial shape parameters ${\epsilon }_1$ and ${\epsilon }_2$ are set to 1, which implies that the shape of the initial model is an ellipsoid. Second, the centroid of all 3D data points can be used to estimate the initial translation. Third, to compute the initial rotation, we gauge the covariance matrix of all 3D data points. From this covariance matrix, we can compute three pairs of eigenvectors and eigenvalues. The largest eigenvector of the covariance matrix always points in the direction of the largest variance of the data, while the magnitude of this largest vector equals the corresponding eigenvalue. The second-largest eigenvector is always orthogonal to the largest and points in the direction of the second largest spread of the data, which is the same as the third vector. Therefore, the eigenvectors can be used as the initial rotation parameters, and the eigenvalues can be used as the initial scale parameters.

3.4. Label Matching

As the labels of the label maps ${\mathcal{L}}_t$ and ${\mathcal{L}}_{t+1}$ do not correspond to each other, labels between sequential frames must be matched. We re-label the label map ${\mathcal{L}}_{t+1}$ using the overlapping area of two frames. We denote the number of pixels that satisfies the label of label map ${\mathcal{L}}_t(\mathbf{u})$ is $l_i$ and the label of label map ${\mathcal{L}}_{t+1}$ is $l_j$ as ${\Pi }_t(l_i,l_j)$. We normalize this term in the following manner:

$${\widehat{\Pi }}_t(l_i,l_j)=\frac{{\Pi }_t(l_i,l_j)}{{\Pi }_t(l_i)},$$

(4)

where ${\Pi }_t(l_i)$ is the number of pixels with the label $l_i$ at label map ${\mathcal{L}}_t$. We re-label $l_j$ by finding the label $l^{*}$ at the ${\mathcal{L}}_t$, which maximizes ${\widehat{\Pi }}_t(l_i,l_j)$:

$$l^{*}{=}_{l_i\in {\mathcal{L}}_t}\{{\widehat{\Pi }}_t(l_i,l_j)\}.$$

(5)

If ${max}_{l_i\in {\mathcal{L}}_t}\{{\widehat{\Pi }}_t(l_i,l_j)\}>\tau$, we re-label the label $l_j$ at ${\mathcal{L}}_{t+1}$ to $l^{*}$.

3.5. Superquadric Tracking

As superquadric fitting is numerically unstable, we do not sequentially estimate parameters $\Lambda$ based on the assumption that the shape of the objects does not change sequentially. We only update the pose parameters $\Phi$ at consecutive frames. The naive approach to estimate the pose ${\Phi }^t$ at frame t is

$$\underset{{\Phi }_i^t}{min}\sum _{k=0}^{K_i}{((F({\mathrm{Tr}}_{{\Phi }_i^t}(p_k^t);{\Lambda }_i)-1))}^2,$$

(6)

where ${\Lambda }_i$ is the estimated superquadric parameters at the initial frame.

However, the outliers which are caused by miss-segmentation lead to the low accuracy of pose estimation. Therefore, we down-weigh the points that are far away from the center of objects during optimization by introducing a coefficient, $\beta$.

A naive approach to down-weighing distant points is employing threshold value based on the Euclidean distance from the centroid of the object and each point. However, there can be two possible problems here. First, as the distance is defined in absolute scale, the threshold value might differ between different scaled objects. Second, calculating the distance in Euclidean Space is not compatible for a superquadric parameter estimation, because the distance in superquadric shape is non-linear. Therefore, we introduce a scale-invariant non-linear threshold parameter to down-weigh miss-segmented points for robust pose parameter estimation.

We introduce coefficient $\beta$ to the minimization equation, Equation (6), to eliminate the points distant from the origin in an object-centered coordinate system. First, we transform the point cloud $\mathbf{p}$ by the transformation matrix ${\Phi }_{t-1}$ at the previous frame. For a point defined by a vector ${\mathbf{p}}_o=(x_o,y_o,z_o)$ in the transformed point cloud, there is a scalar value $\beta$ and vector ${\mathbf{p}}_s=(x_s,y_s,z_s)$ on the superquadric surface, which satisfies ${\mathbf{p}}_s=\beta {\mathbf{p}}_{\mathbf{o}}$. Thus, for vector ${\mathbf{p}}_s$, the following equation holds:

$$F(\beta x_o,\beta y_o,\beta z_o)={\left\{{\left(\frac{\beta x_o}{s_x}\right)}^{\frac{2}{{\epsilon }_2}}+{\left(\frac{\beta y_o}{s_y}\right)}^{\frac{2}{{\epsilon }_2}}\right\}}^{\frac{{\epsilon }_2}{{\epsilon }_1}}+{\left(\frac{\beta z_o}{s_z}\right)}^{\frac{2}{{\epsilon }_1}}=1.$$

(7)

From this equation, it follows that

$$\beta =F^{-\frac{{\epsilon }_1}{2}}(x_o,y_o,z_o).$$

(8)

Instead of estimating the pose ${\Phi }_t$ directly, we estimate the residual pose ${\Phi }_{t-1:t}$. Therefore, the equation below is applied to estimate the residual pose ${\Phi }_{t-1:t}$ (using $x_{+}\equiv max(0,x)$):

$$\underset{{\Phi }_i^{t-1:t}}{min}\sum _{k=0}^{K_i}{({(\beta -{\beta }_{th})}_{+}(F({\mathrm{Tr}}_{{\Phi }_i^t}({\mathrm{Tr}}_{{\Phi }_i^{t-1:t}}(p_k^t));{\Lambda }_i)-1))}^2.$$

(9)

We define the hyperparameter ${\beta }_{th}$ as $0.75$ throughout the experiment.

4. Experiments

4.1. Dataset

As there is no dataset that has a sequence in which the person moves primitive-shaped objects, we created the dataset using Kinect v.1 to evaluate our pose estimation method. In this paper, we did not conduct the evaluation using the synthetic dataset, because the effectiveness of the proposed method can only be validated using the data captured in the real environment. The main task of this paper is to robustly estimate the pose of primitive shaped objects in the case that the object point clouds cannot be accurately segmented. Although the sensor noise can be added to the acquired synthetic data, it is difficult to simulate the miss-segmentation.

Four primitive-shaped objects were used in the experiments. The objects used in the experiment were cube (length = 20 cm), tall cylinder (radius = $7.5$ cm, height = 40 cm), wide cylinder (radius = 15 cm, height = 5 cm), and half sphere (radius = 10 cm). These objects are illustrated in Figure 4. In the dataset, there are 10 scenes included, and each scene comprised approximately 220 frames. At each scene, the user lifted, piled, and moved each object. The examples of frames in the scene are depicted in Figure 5.

The ground truth pose is needed to evaluate our proposed method. We used Cloudcompare [33] to annotate the ground truth pose. First, the synthesized primitive-shaped objects are generated. The detailed explanation to generate the object point cloud is explained in the later section. For example, we set superquadric parameter $(a_1,a_2,a_3,{\epsilon }_1,{\epsilon }_2)=(10.0,10.0,10.0,0.1,0.1)$ for generating a cube, and $(a_1,a_2,a_3,{\epsilon }_1,{\epsilon }_2)=(7.5,7.5,20.0,0.1,1.0)$ for generating a tall cylinder. Third, we manually align the generated model to the point cloud of each frame. By extracting the transformation matrix after the alignment, we can obtain the ground truth pose of each primitive-shaped object.

4.2. Evaluation Metrics

We employed two metrics: 3D error and 2D error. 3D error measures the 3D Euclidean distance between point cloud transformed by ground truth 6DoF pose and by predicted pose. 2D error measures the 2D image pixel distance between the pixel in which the 3D point is projected onto the image using ground-truth pose and the predicted pose.

For the 3D error, we employed the 3D average distance metric for the symmetric object proposed by Hinterstoisser et al. [34]:

$$E_{3D}=\frac{1}{\left|\nu \right|}\sum _{{\mathbf{x}}_1\in \nu }\underset{{\mathbf{x}}_2\in \nu }{min}\left|\right|{\mathrm{Tr}}_{\widehat{\Phi }}({\mathbf{x}}_1)-{\mathrm{Tr}}_{\Phi }({\mathbf{x}}_2){\left|\right|}_2,$$

(10)

where x is a point sampled from object point cloud $\nu$, $\widehat{\Phi }$ is the ground truth pose, and $\Phi$ is the predicted pose.

For the 2D error, we employed 2D projection metric (2D error) which is suited for applications such as a augmented reality [35]. An error is calculated as follows:

$$E_{2D}=\frac{1}{\left|\nu \right|}\sum _{{\mathbf{x}}_1\in \nu }\underset{{\mathbf{x}}_2\in \nu }{min}\left|\right|\mathsf{\Gamma }({\mathrm{Tr}}_{\widehat{\Phi }}({\mathbf{x}}_1))-\mathsf{\Gamma }({\mathrm{Tr}}_{\Phi }({\mathbf{x}}_2)){\left|\right|}_2,$$

(11)

where $\mathsf{\Gamma }$ is a camera intrinsic parameters which project the 3D points to the 2D image plane. Both metrics are that lower is better.

We sample M points from the superquadric surface to generate x. By sampling a point $\mathbf{x}$ from a unit superquadric surface, according to Equation (2), the points that have a high curvature are emphasized. For an unbiased sample distribution, we need to apply equidistant rendering using spherical angles, as introduced by Bardinet et al. [36],

$$\mathbf{x}(\eta ,\omega )=\left[\begin{array}{c}\rho cos(\eta )cos(\omega )\\ \rho cos(\eta )sin(\omega )\\ \rho sin(\eta )\end{array}\right],$$

(12)

where,

$$\rho ={({({|cos(\omega )cos(\eta )|}^{\frac{2}{{\epsilon }_2}}+{|sin(\omega )cos(\eta )|}^{\frac{2}{{\epsilon }_2}})}^{\frac{{\epsilon }_2}{{\epsilon }_1}}+{|sin(\eta )|}^{\frac{2}{{\epsilon }_1}})}^{\frac{-{\epsilon }_1}{2}}.$$

(13)

The generated ground-truth models are visualized in Figure 4 right. Note that this model is only used to annotate the ground-truth pose and calculate the evaluation metrics.

4.3. The Comparison with Other Methods and the Baseline Method

In order to evaluate the effectiveness of our pose estimation, we compared the pose estimation accuracy with four methods. First, we compare with Iterative Closest Point (ICP) algorithm with point-to-plane metric [7], which achieves faster convergence than the point-to-point metric. Unlike the point-to-point metric, which has a closed-form solution, the point-to-plane metric is usually solved using the standard nonlinear least squares method, we employ the Levenberg–Marquardt algorithm. Second, we compare with feature-based RANSAC method, proposed by Buch et al. [37]. We employ Fast Point Feature Histograms (FPFH) feature extractor to extract the point features and match the features using RANSAC with pre-rejection step pose in the estimation loop in order to avoid verification of pose hypotheses that are likely to be wrong. Thirdly, we compare with Normal Distributions Transform (NDT) algorithm [38]. At last, we compare with our baseline method, which does not employ coefficient $\beta$ at Equation (9), to evaluate the effectiveness of excluding the outlier points implicitly.

Note that the baseline and our method estimates the shape, scale, and the pose of each object at the initial frame, and only the pose parameter is updated for the successive frames. For the other methods (ICP [7], NDT [38], RANSAC [37]), we estimate the pose between two successive frames using objects’ point cloud.

Sano et al. [16] also estimate the poses of a cube from a depth image, for instance. However, as the conventional methods for primitive shape pose estimation have to know the type of primitive shaped objects (e.g., cuboid, cylinder and sphere) before estimating the pose. Instead, we employ superquadrics to represent the primitive shapes so that the representative primitive shapes can be represented using a single equation. Moreover, the method proposed by Sano et al. [16] has an assumption that the two faces of the cuboid can be seen from the camera. As the fair comparison against these methods is difficult, we did not conduct the comparison against them.

5. Qualitative Evaluation

In this section, we evaluate the accuracy of the pose estimated by our approach. The qualitative results are illustrated in Figure 6. We present results of superquadric and pose parameter estimation from four scenes. The images in column Figure 6a depict RGB images of the scene, Figure 6b the visualized normal map, Figure 6c the binary segmented image (Section 3.2), Figure 6d the labeled image, and Figure 6e the visualized superquadric surface and ground plane (colored in grey). Note that the RGB images are shown just as the reference, and the pixels of the person are eliminated due to the label matching in Section 3.4. The figure confirms that the shape and the pose of each primitive-shaped object are estimated in different scenes. Moreover, it is evident that the pose of primitives can be successfully estimated even if the user touches and moves with the objects.

We compare the pose estimation results with three conventional methods and one baseline method. The qualitative results are shown in Figure 7. Note that the estimated superquadric surface is used to visualize the result of pose estimation. In these scenes, miss-segmentation was occurred due to the interaction with the user and the object (drew a rectangle in red). Although the pose estimation failed with the baseline and the conventional methods (Figure 7b–e), our proposed method successfully estimated the pose of each object (Figure 7f). As the geometric segmentation cannot distinguish the pixels of person and object, the pose estimation is conducted with an entire labeled pixel. This verifies the robustness of the proposed method against the miss-segmentation of object pixels.

6. Quantitative Evaluation

The quantitative results for each object are summarized in

Table 1. Error of primitive shape pose estimation (per object). It is evident that our proposed method outperforms the baseline and the other methods.

	3D Error [cm] ↓
Object	ICP [7]	NDT [38]	RANSAC [37]	Baseline	Ours
Sphere	4.213	8.645	0.642	0.072	0.070
Tall cylinder	3.194	193.7	2.001	2.738	0.502
Wide cylinder	2.104	28.65	2.013	2.140	0.808
Cube	5.212	41.46	1.829	0.867	0.420
	2D Error [px]↓
Object	ICP [7]	NDT [38]	RANSAC [37]	Baseline	Ours
Sphere	24.03	44.81	14.50	4.232	4.012
Tall cylinder	19.52	45.76	15.46	29.78	7.418
Wide cylinder	11.83	71.25	19.97	31.26	3.478
Cube	24.52	72.59	27.10	7.423	5.010

. We calculate the average error of the entire 10 scenes in our dataset and summarized per primitive-shaped object. We exclude the frames that failed to match the point cloud (Section 3.4) in order to purely compare the object pose estimation accuracy. It can be seen that the proposed method outperforms the conventional methods [7,37,38] and the baseline method for all of the objects. Even though the RANSAC-based method [37] outperformed the baseline for cylindrical objects, introducing the coefficient $\beta$ improves the performance.

The quantitative results for each scene are summarized in

Table 2. Error of primitive shape pose estimation (per scene) using our dataset. S1 to S10 denotes the indices of the scene in the dataset. The rightmost column shows the average of entire scenes.

	3D Error [cm] ↓
Method	S1	S2	S3	S4	S5	S6	S7	S8	S9	S10	Ave.
ICP [7]	0.080	5.012	1.723	1.409	1.382	5.887	5.964	1.392	6.935	6.919	3.670
NDT [38]	37.52	70.77	52.23	62.04	73.35	64.01	60.28	112.4	97.52	95.24	72.53
RANSAC [37]	0.714	2.145	3.077	0.184	0.785	4.335	0.362	1.201	1.720	1.524	1.604
Baseline	0.096	0.230	5.800	0.197	0.245	4.232	1.113	0.015	0.064	0.010	1.200
Ours	0.052	0.695	1.135	1.342	0.268	0.995	0.007	0.003	0.015	0.010	0.452
	2D Error [px]↓
Method	S1	S2	S3	S4	S5	S6	S7	S8	S9	S10	Ave.
ICP [7]	3.378	67.10	8.973	5.301	1.204	33.25	33.25	1.562	21.32	24.53	19.98
NDT [38]	149.8	19.12	56.44	18.10	34.57	65.65	133.1	32.42	25.31	53.21	58.78
RANSAC [37]	9.816	27.54	19.60	18.72	15.23	28.42	11.48	21.53	23.52	12.52	18.83
Baseline	3.280	3.447	43.26	5.724	4.678	23.62	15.76	1.780	3.594	2.130	10.73
Ours	2.152	2.803	4.609	17.23	6.452	13.63	1.670	1.381	16.10	2.059	6.809

. It can be seen that our method outperformed the conventional and baseline methods for most of the scenes. For the scenes in which the baseline outperforms the proposed method, our robust pose estimation method failed to track the objects. The example failure is visualized in Figure 8. As our method implicitly exclude the 3D points which are distant from the superquadric surface, the point clouds of fast-moving objects are not considered for pose estimation. In the bottom figure, the pose of the sphere was not updated due to the fast movement of the object.

7. Discussion

Currently, the proposed method cannot run at real-time speeds, but instead around 30 FPS. Although geometric segmentation and label matching can run at a reasonable frame rate (over 30FPS), pose estimation cannot. The computational time of the proposed method is summarized in

Table 3. Computational time of the each step (ms). Note that the pose of objects is estimated independently, and the overall system takes a time to run if a large number of objects exist in the scene.

Process	Segmentation	Matching	Pose Update
Time (ms)	20.4	13.4	97.2 per object

. Note that the proposed system was implemented on a Windows 10 64-bit laptop with an Intel Core i7-6950X 3.00 GHz CPU, and 16 GB memory. We did not use any GPUs in the system. Further, we were able to confirm that the pose update process is crucial. Even if there is a single object in the scene, the pose update takes 97 ms and this is not in real-time.

To achieve the same results in a real-time system, we can employ the fast parameter estimation method proposed by Duncan et al. [39]. They proposed a method to estimate the superquadric parameters in near real-time. They down-sampled the input point cloud recursively until the optimization failed. Although they achieved 40 ms, this is not still suitable for augmented reality or a robot grasping system. Another solution is implementing the Levenberg–Marquardt algorithm using GPU [40]. Currently, as our system does not use any GPU acceleration, it can be expected to estimate parameters in real-time.

In this paper, three types of primitive shapes (cuboid, sphere, and cylinder) and four objects were used to construct the dataset. Even though the types of primitive shaped objects are limited, the dataset includes the representative primitive shapes that are used in the conventional primitive shape estimation methods [16,19]. Also, unlike existing methods, our method uses superquadrics for the primitive shape representation, which enables us to estimate the shape and the pose regardless of the type of the primitive shape (sphere, cylinder, cuboid, etc.). Although only four objects were used in the experiments, Figure 6 and Figure 7 show that the superquadrics (shape and scale) parameters were correctly estimated. As the shape and scale parameters are estimated at the initial frame, our method can be extended to other primitive shaped objects as long as the objects can be represented by superquadrics. The purpose of this paper is not object shape classification, so we did not conduct experiments with a variety of primitive shaped objects, but this can be an important subject for future study.

8. Conclusions

In this paper, we proposed a method for estimating the six Degrees of Freedom (6DoF) pose of texture-less primitive-shaped objects from a depth image. The method is robust to outliers, which are caused by the miss-segmentation of the depth image. To achieve robustness, we introduced the novel pose estimation method. We implicitly ignore the points that are distant from the object point cloud to ensure that the optimization is conducted with the only object’s point cloud. Further, we generated a new data set in the experiments. In this dataset, the user interactively moves the primitive-shaped objects. The experimental results revealed the effectiveness of our novel estimation method by comparing the pose accuracy for primitive shaped objects.

There are several future works that can be considered. First, as mentioned in the discussion section, we aim to achieve real-time computation of superquadric parameters. Second, currently, out pose estimation method relies on the preprocessing steps, such as geometric segmentation and segment matching, of the obtained depth image to extract the object point cloud. However, the pose of the objects cannot be estimated if these preprocessing steps failed to extract the point cloud of the target object. We will further investigate the method to estimate the pose of the objects from the raw depth image.