Robust shape estimation with false-positive contact detection

Observations were obtained by detecting events. An illustration of this event detection when using an acceleration is shown in Fig. 2. In this example, an event takes the form of the quadcopter colliding with the object or becoming unstable. If an event occurs and the acceleration becomes larger than the threshold, $y_{i}$ is labeled as being zero. When physical contact occurs, false-negative contact data can occur when the quadcopter collides with the object at low speed. Moreover, false-positive contact data are detected when the quadcopter becomes unstable.

As shown in Fig. 2, it is difficult to determine the threshold so that the number of false-negative and false-positive contact data will be zero. Although the number of items of false-positive contact data could be reduced by constructing more sophisticated classifiers, it is unrealistic to classify false-positive contact data with a 100 $\%$ accuracy rate for objects made of any material. Therefore, in the present study, we explored another approach by assuming that the generation of false-positive contact data is unavoidable.

With GPs, we are required to assume the distribution of observations. With homoscedastic GPs, the distribution is assumed to follow a normal distribution, as defined by

where $f_{i}$ and $\sigma^{2}$ represent the shape potential function and the noise variance with a constant value, respectively. This distribution may be unsuitable for application to our problem; contact data with false-positive samples cannot be explained by such a light-tailed distribution, owing to the nature of the false-positive contact data and its representation with three natural numbers (-1/0/1). Thus, it may be unsuitable for use with false-positive contact data.

Therefore, we introduce the Student’s t-distribution for the observations, given by

where $\nu$, $\lambda$ and $\Gamma$ represent the degree of freedom, a scale parameter and gamma function, respectively. As shown in Fig. 3, false-positive contact data can be explained with a greater degree of probability, owing to its heavy-tailed distribution.

In a GPIS framework, the shape potential function $f$ is modeled by GP regression for a given data set $\mathcal{D}=\{\textit{{x}}_{i},y_{i}\}$ ($i=1,\cdots,n$). In the GPIS, a prior of the shape potential functions $\textit{{f}}:=[f(\textit{{x}}_{1}),\cdots,f(\textit{{x}}_{n})]^{\rm{T}}{}$ is assumed to follow a normal distribution: $p(\textit{{f}}\mid\textit{{x}})=\mathcal{N}(\textit{{f}}\mid\textbf{0},\textit{{K}})$, where $\textit{{x}}:=[\textit{{x}}_{1},\cdots,\textit{{x}}_{n}]^{\rm{T}}{}$, and $\textit{{K}}\in R^{n\times n}$ is the training data covariance matrix. Moreover, the probability of observations $\textit{{y}}:=[y_{1},\cdots,y_{n}]^{\rm{T}}{}$ is given by the normal distribution with constant variance: that is, $p(\textit{{y}}\mid\textit{{f}})=\mathcal{N}(\textit{{y}}\mid\textit{{f}},\sigma^{2}\textit{{I}})$. Upon combining these probabilities, the posterior of the shape potential function is given by

for which the marginal likelihood $\int\mathcal{N}(\textit{{y}}\mid\textit{{f}},\sigma^{2}\textit{{I}})\mathcal{N}(\textit{{f}}\mid\textbf{0},\textit{{K}})d\textit{{f}}$ in Eq. (1) can be computed analytically as described in [32, 33]. Moreover, the predictive mean and variance can be computed as

where $k(\textit{{x}}_{i},\textit{{x}}_{j})$ is the covariance between the observed points $\textit{{x}}_{i}$ and $\textit{{x}}_{j}$, $i,j\in\{1,\cdots,n\}$, $\textit{{k}}(\textit{{x}}^{*})$ is the covariance vector between all the observed points $\textit{{x}}_{i}$ $(i=1,\cdots,n)$ and the test point $\textit{{x}}^{*}$, K has entries $K_{ij}=k(\textit{{x}}_{i},\textit{{x}}_{j})$, $i,j\in\{1,\cdots,n\}$. An estimate of the object’s shape is obtained by finding those points $\textit{{x}}^{*}$ where $\mu(\textit{{x}}^{*})\approx 0$ in Eq. (2).

Because all the diagonal terms of $\sigma^{2}\textit{{I}}$ in Eqs. (2) and (3) have the same value, false-positive contact data have the same influence on the predictive distribution as normal data. An issue, however, is whether the distribution of the observations is assumed to conform to a normal distribution with constant variance.

In this subsection, we propose a robust GPIS with false-positive contact data based on the GPIS method.

To realize this, we used the Student’s t-distribution as the distribution of the observations, given by

Given the prior of the shape potential functions with a normal distribution, $p(\textit{{f}}\mid\textit{{x}})=\mathcal{N}(\textit{{f}}\mid\textbf{0},\textit{{K}})$, the posterior of the shape potential function is given by

where $\boldsymbol{\psi}$ represents the hyperparameters in the covariance matrix K.

In the same way as in the GPIS method, an estimate of the object’s shape can be obtained from the posterior mean. However, the marginal likelihood $\int\mathcal{T}({\textit{{y}}\mid\textit{{f}}},\lambda,\nu)\mathcal{N}(\textit{{f}}\mid\textbf{0},\textit{{K}})d\textit{{f}}$ in Eq. (5) cannot be computed analytically. Thus, we conform to the variational approximation for GP regression with the Student’s t-distribution, as described in [27].

The Student’s t-distribution in Eq. (4) is a scale-mixture of an infinite number of normal distributions, given by

where $\alpha=\nu/2$ and $\beta=\nu/{2\lambda}$ represent the shape and inverse scale parameter, respectively. The Student’s t-distribution becomes heavy-tailed when $\alpha$ becomes small or $\beta$ becomes large, as described in Appendix. Since outliers are more likely to occur from a heavy-tailed distribution, it is suitable to explain false-positive contact data. As described later, these parameters are optimized such that the estimated shape best fits the observed data. Moreover, as $\alpha$ becomes small or $\beta$ becomes large, the noise variance, $\sigma^{2}$, becomes large with high probability, as described in Appendix.

Using Eq. (6), we approximate the posterior in the factorized form given by $p(\textit{{f}},\boldsymbol{\sigma}^{2}\mid\mathcal{D},\boldsymbol{\psi},\boldsymbol{\theta})\approx q(\textit{{f}})q(\boldsymbol{\sigma}^{2})$, where $\boldsymbol{\theta}:=[\alpha,\beta]^{\rm{T}}$ and $\boldsymbol{\sigma}^{2}=[\sigma_{1}^{2},\cdots,\sigma_{n}^{2}]^{\rm{T}}$. The approximate posterior of f and $\boldsymbol{\sigma}^{2}$ is given by

where m and A represent the mean and covariance of $q(\textit{{f}})$, and $\tilde{\alpha}_{i}$ and $\tilde{\beta}_{i}$ represent the shape and inverse scale for each item of data $i\ (i=1,\cdots,n)$, respectively. If we could approximate the posterior in an analytically tractable form, we could estimate an object’s shape from its posterior mean.

To realize this, we introduce the KL divergence between the approximation and the posterior distribution, given by

The KL divergence is minimized using an expectation maximization (EM) algorithm [34]. In the EM algorithm, the approximate posterior and parameter values are updated by repeating the two steps. In the expectation step (E-step), m, A in Eq. (7), $\tilde{\alpha}_{i}$ and $\tilde{\beta}_{i}$ in Eq. (8) are iteratively updated to minimize the KL divergence for given parameters $\boldsymbol{\theta}$ and $\boldsymbol{\psi}$. In the maximization step (M-step), performed after each E-step, $\boldsymbol{\theta}$ and $\boldsymbol{\psi}$ are iteratively updated to minimize the KL divergence for fixed $q(\textit{{f}})$ and $q(\boldsymbol{\sigma}^{2})$. The EM algorithm iterates the E- and M-steps until the KL divergence converges. Details of the method used to derive the update laws can be found in [27].

After the variational approximation, the mean and variance of the approximate posterior are computed as

where $\boldsymbol{\Sigma}$ represents a diagonal matrix with entries $\Sigma_{ii}=\tilde{\beta}_{i}/\tilde{\alpha}_{i}$ $(i=1,\cdots,n)$. Since the diagonal terms of $\boldsymbol{\Sigma}$ in Eqs. (9) and (10) differ from each other, we can introduce heteroscedacity into each item of contact data.

The object shape is reconstructed by finding those points $\textit{{x}}^{*}$ that satisfy $\mu(\textit{{x}}^{*})\approx 0$ in Eq. (9). As $\tilde{\beta}_{i}/\tilde{\alpha}_{i}$ becomes large, the contact data $i$ has little influence on $\mu(\textit{{x}}^{*})$. Therefore, such contact data has little influence on the shape estimates.

Finally, we present the false-positive contact detection. After the variational approximation, we can obtain the uncertainty of data $i$, given by

The noise variance, $\sigma^{2}$, in Eq. (6) becomes large with high probability as $u_{i}$ becomes large. Therefore, we can judge the uncertainty for each item of contact data using Eq. (11).

The main steps used in the shape estimation and false-positive contact detection are shown in Algorithm 1.

We prepared three different object shapes, as shown in Fig. 4. The contact points were set on the edges at intervals of 0.01 [m], as shown in Fig. 5(a) and (c), while they were set on the circumference at an interval of 3[deg] in Fig. 5(b). The internal and external points were randomly set in the given region $Q:=\{(x_{1},x_{2})\mid(-3\leq x_{1}\leq 3,-3\leq x_{2}\leq 3)\}$. To consider the false-positive contact data, we replaced the external points with a pair of outliers of contact and internal points with a 2$\%$ chance. The outlier of the internal point was randomly set within a certain distance 0.1 [m] from the outlier of the contact point.

We performed 50 simulations for each object shape. The parameters used in the simulations are listed in Table I. The parameters used in the GPIS methods were set by trial and error. The test data were set on a grid with an interval of 0.02 [m].

One of the 50 simulations is shown in Fig. 6. As shown in the figure, the object shapes could be roughly captured using the GPIS. At the same time, however, surfaces were incorrectly estimated around a few of the outliers, located sufficiently far from the object. The predicted shape potential values (colored contours) show that the mean values became negative around the outliers, which indicates that the predictive distributions were greatly influenced by the outliers.

In contrast, a surface was not incorrectly estimated around the outliers when using the robust GPIS, as shown in Fig. 6. Considering the uncertainty values, those values around the outliers became remarkably large compared with those around the normal data. As a result, the predicted shape potential values around the outliers were sufficiently greater than zero, indicating that the outliers had little influence on the predicted mean values.

To evaluate the accuracy of shape estimation using the GPIS methods, we introduce metrics for determining the shape estimation error (i.e., the difference between the estimated shape and the true shape). Using Eqs. (2) and (9), we selected the test point $i=\{1,\cdots,n_{s}\}$ whose mean value $\mu(\textit{{x}}_{i})$ is almost equal to zero ($-0.01<\mu(\textit{{x}}_{i})<0.01$), where $\textit{{x}}_{i}$ is the position of the test point $i$. After computing the minimum distance $d_{i}$ between the test point $i$ and the surface of the true shape, we computed the mean error of the distance $d_{i}$, defined by

The results obtained with the t-test for the 50 simulations used to compare the robust GPIS with the GPIS are listed in Table II. These results reveal significant differences between the robust GPIS and the GPIS in terms of the shape estimation errors, since the p-values are all less than 0.01. Therefore, for all the prepared objects, our algorithm produced smaller shape errors than the GPIS.

This subsection shows the clear detection of false-positive contact data when applying our algorithm. With the robust GPIS, the uncertainty of the data around the outliers became remarkably large compared with those around the normal data, as shown in Fig. 6.

Here we introduce metrics for evaluating the false-positive contact detection. While the robust GPIS can estimate the uncertainty of data, the GPIS cannot directly do so. Therefore, we regard the predicted variance at each observed data as the uncertainty of the data. To define the clarity of the false-positive contact detection for each outlier $o$, we define the metrics given by

where $u_{o}$, $\mathcal{R}_{o}$ and $n_{o}$ represent the uncertainty of the data $o$, the neighborhood region within certain distance $d_{o}$ from the outlier $o$, and the number of the normal data within the region $\mathcal{R}_{o}$, respectively. In the simulations, we set $d_{o}=0.50$ [m], which is equal to the length and width of the quadcopter used in the experiment.

Similar to the evaluation of the shape estimation error, we performed a t-test for the 50 simulations by comparing the robust GPIS with the GPIS. The results listed in Table III reveal significant differences between the robust GPIS and the GPIS in terms of false-positive contact detection, since the p-values are all less than 0.01.

Therefore, for all the prepared objects, our algorithm could detect false-positive contacts more clearly than the GPIS.

The position of the quadcopter was observed using an OptiTrack Prime 17W motion-capture system (Natural Point, Inc., Corvallis, OR) operating at 200 Hz. In the experiment, the ground-truth shape of the 3D object is obtained by placing markers at the vertexes of the polygons that compose the object surfaces. Moreover, the object shape is considered in the ground-fixed coordinate system. Therefore, translation and rotation were not considered when comparing the estimated shape with the true object shape.

The acceleration and attitude were measured using mounted sensors operating at 200 Hz. The quadcopter was controlled using a personal computer (PC) with an 8-core Intel Core i7 (2.80 GHz) processor, 32 GB of RAM, and a joystick (Extreme 3D Pro, Logitech, Inc., Lausanne, Switzerland). The measurement data and control signals were exchanged between the PC and the quadcopter via WiFi communication.

The exploration region $Q$ is given by

where $x_{1}$, $x_{2}$, and $x_{3}$ represent the longitudinal, transverse, and vertical axes, respectively.

We controlled the quadcopter using the joystick, which allowed us to collect as much contact data as possible in $Q$ at 25 Hz while collecting non-contact data as uniformly as possible at 0.5 Hz. The limitations imposed by the battery capacity forced us, for each experiment, to perform five flights of 8 min each.

To validate the effectiveness of our algorithm, we generated some ground truth outliers by applying a gust of wind in some regions. We conducted two flights with artificial pertubations and prepared two observed data sets combined with the other four flights without artificial pertubations. The initial values for the hyperparameters used with the GPIS methods are listed in Table IV.

This subsection introduces omni-directional contact detection based on the acceleration of the quadcopter. Based on the measured acceleration, the shape potential value is obtained using

where ${}^{B}\hat{\textit{{a}}}_{\textit{i}}$ and $a_{0}$ represent the acceleration of the robot in the body-fixed coordinate system $B$ and the positive threshold to detect the transition state from non-contact to contact, respectively.

The flowchart of the contact detection is shown in Fig. 7. If $y_{i}$ is equal to zero and its true value $y_{i}^{\ast}$ is equal to one, the data $i$ is the false-positive contact data. If $y_{i}$ is equal to one and $y_{i}^{\ast}$ is equal to zero, the data $i$ is the false-negative contact data. The occurence of these data depend on the the threshold $a_{0}$. If $a_{0}$ is set too low, false-positive contact detection can occur easily since sudden acceleration and deceleration can be detected as false-positive contacts. In the opposite case, false-negative contact detection can occur. In this paper, we focus on the false-positive contacts since the false-negative contacts have much less influence on the false-positive contacts. We searched for the minimum value of $a_{0}$ by trial and error, considering that the number of false-positive contacts is as small as possible.

Furthermore, the outward normal vector on the surface is estimated by

where ${}^{E}\hat{\textit{{a}}}_{i}$ represents the acceleration of the robot in the ground fixed coordinate system $E$. Acceleration with mounted sensors is observed in the body-fixed coordinate system $B$, such that ${}^{E}\hat{\textit{{a}}}$ in Eq. (14) is converted from ${}^{E}\hat{\textit{{a}}}_{i}=\textit{{R}}(\hat{\phi})\textit{{R}}(\hat{\theta})\textit{{R}}(\hat{\psi})^{B}\hat{\textit{{a}}}_{i}$, where R, $\hat{\phi}$, $\hat{\theta}$, and $\hat{\psi}$ represent the rotation matrix, and the measured values of the roll, pitch, and yaw angle, respectively.

Using the center of gravity of the robot and the estimated normal vector, we approximated a contact position $\textit{{x}}_{c}$, as shown in Fig. 8. Consider a circumscribed cylinder to the quadcopter with the radius $r$ and the height $h$. If one of the planar components of the estimated vector $\hat{\textit{{n}}}_{i}$ is largest, the contact position is calculated by $\textit{{x}}_{c_{i}}=\textit{{x}}_{i}-r\hat{\textit{{n}}}_{i}$. Otherwise, the contact position is calculated by $\textit{{x}}_{c_{i}}=\textit{{x}}_{i}-h\hat{\textit{{n}}}_{i}$.

Because the internal points of the object cannot be obtained directly from the contact, we estimate the position of the internal point $\textit{{x}}_{in}$, as shown in Fig. 8. Only if $y(\textit{{x}})=\rm{0}$, the position of the internal point and its potential value are calculated as $\textit{{x}}_{in}=\textit{{x}}_{c}-d_{in}\hat{\textit{{n}}}(\textit{{x}})$ and $y(\textit{{x}}_{in})=-1$, where $d_{in}$ represents the thickness parameter determined by the minimum thickness of the object. The training data was produced by merging the contact, external, and internal points while the test data was set on a grid with an interval of 0.05 [m].

The results of shape estimation when applying the GPIS methods are shown in Fig. 9.

As shown in Fig. 9(a), the shape of the 3D construction could be roughly captured when applying GPIS. However, surfaces were generated at points sufficiently far from the construction. The predicted mean values of the shape potential function were all zero around the outliers. In contrast, the robust GPIS was able to avoid the incorrect generation of surfaces around the outliers, as shown in Fig. 9(b). This result indicates that the predicted mean values of the shape potential function were sufficiently greater than zero around the outliers.

Furthermore, the shape estimation errors computed by Eq. (12) when applying the GPIS methods are listed in Table V. The results indicate that the use of the robust GPIS incurred smaller errors than the GPIS.

This subsection focuses on the results of false-positive contact detection when applying the GPIS methods.

The distribution of the uncertainty of data, shown in Fig. 9(a), indicates that the uncertainty around the outliers was as small as that around the normal data with the application of GPIS. In contrast, we can confirm that the uncertainty around the outliers in Fig. 9(b) could be distinguished very clearly when our algorithm was applied.

To evaluate the false-positive contact detection of the GPIS methods, we evaluated false-positive contact detection for each outlier using Eq. (13). In the experiment, we regarded any detected contact points which were 0.5 meters or more from the object as being outliers. This value was equivalent to the length and width of the quadcopter.

The mean values for eight outliers in the two experiments are listed in Table VI. The results show that the mean value obtained when applying the GPIS were almost equal to one, which indicates that the GPIS could not distinguish between the outliers and the normal data. In contrast, the mean values were much smaller than one, which indicates that our algorithm could very clearly distinguish the outliers from the normal data.

In this subsection, we discuss the usefulness of the false-positive contact detection. Consider active object exploration [35, 36, 37, 38] with false-positive contact data. In this problem, the outliers might degrade any prediction, resulting in inefficient exploration since robots might excessively explore some areas around the incorrect surfaces. Moreover, false-negative contacts could degrade the efficiency of the object exploration although they were not considered in the present study. If contacts were not detected at the surface, the GPIS methods could predict that no surface exists around the false-negative contacts. As a result, those locations were difficult to sample when applying Bayesian optimization [39]. To deal with this issue, we can remove the outliers based on our false-positive contact detection. To apply our algorithm to active object exploration, we should reduce the amount of observed data using a sparse Gaussian process regression methods as described in [40, 41].

Our future work will aim to improve the contact localization in our experimental system. In a more complex scenario such as some concave formations, multiple contacts might occur at different locations on the quadcopter. In such a situation, contacts are detected around the inside part of the corners because the current system estimates only one contact position at any one time using the resultant force of multiple contacts. As a result, the estimated shape around the corner tends to be rounded. To overcome this issue, more precise contact localization should be implemented using the methods described in [42, 43]. In addition, we should extend our contact detection to handle multiple contacts, as proposed by Sommer et al. [44].

In this study, we assumed that the position of the quadcopter is accurate. However, in real environments, it is necessary to consider the uncertainty of localization in the shape estimation. To address this issue, we aim to eliminate the assumption by combining the GP model with the input noise proposed by Girard et al. [45].

It would be interesting to combine our contact detection with sensor modalities other than an accelerometer. Moreover, we could explore applications other than shape estimation when using vision sensors.