|project:||Fitting segmented pointclouds to superquadrics online|
|I am a GSoC participant who wants to bestow robots with the powers of Sight Beyond Sight to perceive objects in their environment and to perform useful manipulation tasks with them.|
Our goal is to provide PCL with a module to fit superellipsoids to segmented, one-view pointclouds in an online manner.
In this post I will show you some results I got yesterday of our demo code working in a slightly cluttered scenario and fitting superquadrics to household objects.
Here is the environment we wish to recognize:
Here are the fitting results:
Here a bit more of detail with the colored pointclouds:
And here the fitted superquadrics alone for a better view:
The code used for the results shown in the rest of this post can be found here .
So, how does this code work? A general overview would go like this:
Things to figure out:
|[Bohg2011]||Bohg, Jeannette, et al. “Mind the gap-robotic grasping under incomplete observation.” Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011.|
In this post we will see some initial results for fitting superquadrics to full pointclouds. Let’s quickly start with the math so you have a good idea of how the code works.
I will remind you again the superellipsoid equation (yes, I will keep bringing up the equation again and again so it stays forever on your brain):
We will call the expression to the left . A 3D point() will belong to the canonical superquadric defined by the parameters if . To have a general superquadric, we must consider the translation and rotation terms, hence the general equation (1) has the following form:
where is the translation of the superellipsoid center with respect to a given global frame and are the column vectors of the rotation matrix of the superellipsoid (again, with respect to some defined global axes). In fact, to be completely rigurous, we should express as a function of both the point being evaluated and the superellipsoid parameters being used: where
In order to find the superellipsoid that best fit a given full pointcloud (composed by 3D points with , we need to minimize the error between each point and equation (2). In its most basic form, we could try to minimize this equation:
Some wise people suggested a couple of modifications to the basic version above and came up with this:
the factor makes sure that the superellipsoid obtained is the smallest possible. The additional exponent improves the convergence time.
As of now, I will not go into more details on the math behind since I am running out of time to write this entry. However, there are a few things that you should remember of this post:
As of now, we have implemented the method described in [Duncan13] to fit segmented, full pointclouds to superellipsoids. In this post we will present some initial results obtained for 4 test cases (sphere, box, cylinder and oval shape). We tested them in 3 scenarios:
Let’s start with the initial test cases:
|Case 0: Parameters used|
The pointclouds are shown in the following figure and are also available in my repository .
The code used for the results shown in the rest of this post can be found here .
For the first test case we generated full sampled pointclouds by using the sampling code we presented in our previous post. To initialize the maximizer, we used the pointcloud’s bounding box information for the superellipsoid dimensions and global transform. For all cases we used an initial value of 0.5 for and 1.0 for (these values are in the middle of the allowed range).
Results for the fitting are shown in the following table. It can be seen that the fitting works pretty well, which is kind of expected since this is the most basic case.
|Case 0: Results|
For case 1, we modified the test pointclouds by applying a transformation to them. Details of the transformations for each case are shown below (the parameters remain constant so we omit to repeat them).
|Case 1: Parameters|
The results are shown below. We observed that the parameters that remain constant keep approximately the same fitted values as in Case 0, so we won’t repeat them in the next table.
|Case 1: Results|
We can observe that the translation values are well fitted, while the same is not the case for the rotation values. In the next post we should discuss some ideas to fix that.
Finally, we added noise to the pointclouds. The values used are shown in the following table (meaning that a uniform disturbance between was randomly applied to each point in the pointcloud.
|Case 2: Parameters|
The pointclouds are shown in the following figure:
The final parameters are shown below:
|Case 2: Results|
I should probably put a parallel table with all the original values so you can compare visually more easily. In any case, couple of observations:
|[Duncan13]||Duncan, Kester, et al. “Multi-scale superquadric fitting for efficient shape and pose recovery of unknown objects.” Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013.|
In this post I will talk about superquadric uniform sampling. You might be wondering why you should care about sampling. Well, there are 2 reasons:
From last time, you might remember that the Superellipsoids can be expressed with the explicit equations:
Now, let’s say that we want to generate pointclouds of different superellipsoids. Also by now let’s assume we only care about canonical superellipsoids (no translation and no rotation). A first idea would probably be to just sample the values of and to generate the 3D samples. Let’s see what happens if we use this simple approach:
You might notice that the results for the sphere and the oval shape are reasonably well distributed; however the results for the cylinder and the box are far from even. In general, the naive approach of uniformly sampling and would work well for high values of and (where high means closer to 1 than to 0). For lower values (such as for the box and cylinder cases where ) the performance of naively sampling the angles would degrade.
The following figure shows a more reasonable sampling:
The samples above were obtained by performing a simple technique proposed by Pilu and Fisher [PiluFisher95]. These authors noticed that in order to obtain an uniform sampling, the 3D distance between samples had to be constant; however, uniform sampling distance does not correlate with uniform angle steps.
An implementation of the algorithm proposed by Pilu and Fisher can be found in my PCL fork while the source example sampleSQ.cpp generates a pointcloud for a superellipsoid defined by user input parameters.
|[PiluFisher95]||Pilu, Maurizio, and Robert B. Fisher. “Equal-distance sampling of superellipse models.” DAI RESEARCH PAPER (1995).|
Superquadrics are a family of geometric shapes that can represent a wide array of diverse primitives using a small number of parameters. As an example, look at the figure below: The first row depicts 5 common household objects. The second row shows the superquadrics that more closely resemble them.
Superquadrics were initially introduced in the computer graphics community by Alan Barr [Barr81], but they were later adopted by the robotics community as an effective modelling tool to approximate objects shape. In general, superquadrics include superellipsoid and supertoroids, but for most practical uses, we care for only superellipsoids. These can be expressed with the following formula:
As it can be seen, superellipsoids in their canonical form can be expressed by 5 parameters:
If a general transformation is considered, then the total number of parameters required to define a superellipsoid is 11 (the 6 additional being the rotation and translation degrees of freedom )
Expression (1) shows the implicit equation of the superellipsoids. Its parametric solution can be expressed as:
with and . In our next post we will learn how to generate pointclouds for superellipsoids (which is not as simple as just sampling and ! Stay tuned :).
|[Barr81]||Barr, Alan H. “Superquadrics and angle-preserving transformations.” IEEE Computer graphics and Applications 1.1 (1981): 11-23.|