PCL Developers blog

A. Huaman Quispe

project:Fitting segmented pointclouds to superquadrics online
mentor:Marco Gutierrez

About me

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. sn_se

About the project

Our goal is to provide PCL with a module to fit superellipsoids to segmented, one-view pointclouds in an online manner.

Recent status updates

3. Some real results! (a.k.a. Not hacks!)
Monday, August 11, 2014

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:

  1. Connect your Kinect to your PC :0}
  2. A snapshot of a pointcloud (captured by OpenNI grabber) is taken (demo.cpp)
  3. The pointcloud captured is segmented (table + objects) in clusters (demo.cpp)
  4. The clusters (one-view) are mirrored based on [Bohg2011] (tabletop_symmetry/mindGapper.cpp)
  5. The mirrored clouds are then fitted to a SQ (SQ_fitter_test1.cpp)


  1. Using mirrored pointclouds helps substantially to the fitting process.
  2. I have implemented the optimization process using two different libraries: levmar and ceres. While ceres have to be installed from a external repo, levmar can just be added to the code (sq_fitting/levmar). It only depends on lapack and it is lightweight. I have to test how both compare in processing times. The results I am attaching were obtained using the levmar optimization process.
  3. If you noticed, the milk carton was not fitted. I got an spurious result there given that the mirroring code did not work well with that object.
  4. Fitting times (still in debug mode, not optimized and with warning printouts here and there) is < 2 seconds in my old laptop.
  5. You might notice that for one of the objects (the blue water container) the fitting produces a SQ longer than needed (cross the table). How to limit it to be shorter?
  6. For objects like the tuna can, in which the “bigger axis” (assumed to be Z) is horizontal, the fitting is not exactly as it should be (the revolution axis should be UP, but it is horizontal. How to deal with objects that are wider than taller?

Things to figure out:

  1. Levmar or Ceres? So far I am more inclined towards levmar since it is smaller and - other than lapack, which should be installed on most Linux machines - it can be modified. I know Ceres is fairly popular but I am not sure if we need all that power.
  2. Figure out the “wide” objects issue.
  3. Improve my mirror code for cases like the milk carton with bad viewing angles.
[Bohg2011]Bohg, Jeannette, et al. “Mind the gap-robotic grasping under incomplete observation.” Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011.
2. Fitting superquadrics (a.k.a. The Horror of Box Constrained Non-Linear Optimization)
Friday, June 20, 2014

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):

(1)\left( \left(\dfrac{x}{a}\right)^{\frac{2}{\epsilon_{2}}} + \left(\dfrac{y}{b}\right)^{\frac{2}{\epsilon_{2}}} \right) ^{\frac{\epsilon_{2}}{\epsilon_1} } + \left(\dfrac{z}{c}\right)^{\frac{2}{\epsilon_{1}}} = 1

We will call the expression to the left F(x,y,z). A 3D point((x_{i},y_{i},z_{i})) will belong to the canonical superquadric defined by the parameters (a,b,c,\epsilon_{1}, \epsilon_{2}) if F(x_{i},y_{i},z_{i}) = 1. To have a general superquadric, we must consider the translation and rotation terms, hence the general equation (1) has the following form:

(2)F(x,y,z) =  \left[ \left(\dfrac{ n_{x}x + n_{y}y + n_{z}z -t_{x}n_{x}-t_{y}n_{y} - t_{z}n_{z} }{a}\right)^{\frac{2}{\epsilon_{2}}} + \left(\dfrac{ o_{x}x + o_{y}y + o_{z}z -t_{x}n_{x}-t_{y}o_{y} - t_{z}o_{z} }{b}\right)^{\frac{2}{\epsilon_{2}}} \right] ^{\frac{\epsilon_{2}}{\epsilon_1} } + \left(\dfrac{ a_{x}x + a_{y}y + a_{z}z -t_{x}a_{x}-t_{y}a_{y} - t_{z}a_{z} }{c}\right)^{\frac{2}{\epsilon_{1}}} = 1

where \mathbf{t} is the translation of the superellipsoid center with respect to a given global frame and (\mathbf{n},\mathbf{o},\mathbf{a}) 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 F(.) as a function of both the point being evaluated and the superellipsoid parameters being used: F(x,\Lambda) where \Lambda = (a,b,c,\epsilon_{1}, \epsilon_{2},t_{x}, t_{y}, t_{z}, \psi, \theta, \gamma)

In order to find the superellipsoid that best fit a given full pointcloud (composed by 3D points \mathbf{x}_{i} with i \in [1,k], we need to minimize the error between each point and equation (2). In its most basic form, we could try to minimize this equation:

\min_{k} \sum_{k=0}^{n} \left( F(\mathbf{x}; \Lambda) - 1 \right ) ^{2}

Some wise people suggested a couple of modifications to the basic version above and came up with this:

\min_{k} \sum_{k=0}^{n} \left( \sqrt{abc}F^{\epsilon_{1}}(\mathbf{x}; \Lambda) - 1 \right ) ^{2}

the \sqrt{abc} factor makes sure that the superellipsoid obtained is the smallest possible. The additional exponent \epsilon_{1} 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:

  • Our goal is to solve a Non-Linear Square problem.
  • We have 11 parameters to find
  • Our parameters are bounded, which means that they have upper and lower limits. This constraints the type of algorithms we can use to optimize our solution.
  • The more dense the pointcloud is, the more factors will be considered in the equation above.

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:

  • Canonical superellipsoids with no noise.
  • General superellipsoids (rotation and translation added) no noise.
  • General superellipsoids with noise (up to 5% of the length of the smallest principal axis).

Let’s start with the initial test cases:

Case 0: Parameters used
Parameter Sphere Ellipsoid Cylinder Box
a 0.25 0.04 0.05 0.025
b 0.25 0.08 0.05 0.08
c 0.25 0.06 0.1 0.18
e1 1 0.75 0.25 0.1
e2 1 0.75 1.0 0.1

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 \epsilon_{1} and 1.0 for \epsilon_{2} (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
Parameter Sphere Ellipsoid Cylinder Box
a 0.247 0.039 0.0499 0.025
b 0.247 0.079 0.049 0.079
c 0.247 0.059 0.099 0.179
e1 0.99 0.753 0.271 0.10
e2 0.99 0.753 0.97 0.13

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 a,b,c,\epsilon_{1},\epsilon_{2} remain constant so we omit to repeat them).

Case 1: Parameters
Parameter Sphere Ellipsoid Cylinder Box
x 0.5 -0.6 -0.4 -0.1
y 0.8 0.2 0.7 0.3
z 0.0 0.0 0.3 0.5
roll 0.0 0.2 0.6 0.0
pitch 0.0 0.5 0.9 0.0
yaw 0.3 0.3 0.8 0.8

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
Parameter Sphere Ellipsoid Cylinder Box
x 0.5 -0.6 -0.4 -0.1
y 0.8 0.199 0.69 0.3
z 0.0 0.0 0.29 0.49
roll 0.0 0.199 -0.117 0.0
pitch 0.0 0.49 1.02 0.0
yaw 0.0 0.29 -0.055 -2.34

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 [-\delta,\delta] was randomly applied to each point in the pointcloud.

Case 2: Parameters
Parameter Sphere Ellipsoid Cylinder Box
\delta 0.01 0.002 0.0025 0.0015
Percentage 4% 5% 5% 6%

The pointclouds are shown in the following figure:


The final parameters are shown below:

Case 2: Results
Parameter Sphere Ellipsoid Cylinder Box
a 0.24 0.039 0.049 0.025
b 0.245 0.079 0.049 0.08
c 0.249 0.0601 0.102 0.19
e1 1 0.76 0.35 0.33
e2 0.91 0.71 0.92 0.1
x 0.49 -0.59 -0.39 -0.1
y 0.8 0.19 0.70 0.3
z 0.0 0.0 0.30 0.49
roll 0.0 0.19 0.95 0.0
pitch 0.0 0.50 0.47 0.0
yaw -2.8 0.30 1.34 -2.3

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:

  • Rotation final values are the ones with the biggest errors.
  • Noise levels that exceed 5% are not acceptable (the fitting values \epsilon_{1} and \epsilon_2 vary significantly, altering the shape of the object. The other parameters keep being reasonably accurate.
  • We are not using any loss function to alleviate the effect of the outliers. Here that did not prove particularly important, but when we do experiments with data that is not synthetically obtained (real pointclouds of objects) it will probably matter.
  • A crucial requirement for a good fit is the initialization of the Z axis of the object (revolution axis).
[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.
1. Uniform sampling for superquadrics
Friday, June 20, 2014

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:

  • Synthetic data: To test our superquadric fitting algorithm (next section) we will start using as a baseline canonical superquadrics with some added noise. Once a fitting algorithm works with these complete pointclouds, we will be able to proceed with more complex shapes (that are not necessarily superellipsoids but close enough to attempt to fith them).
  • Debugging purposes: Visualization of our results.
  • Beautiful math: The math of this part is simple, yet elegant. If you want to know the details, peek the code (linked below) or read the paper mentioned later in this entry.

From last time, you might remember that the Superellipsoids can be expressed with the explicit equations:

x = a{\cos(\theta)}^{\epsilon_{1}}{\cos(\gamma)}^{\epsilon_{2}} \\
y = b{\cos(\theta)}^{\epsilon_{1}}{\sin(\gamma)}^{\epsilon_{2}} \\
z = c{\sin(\theta)}^{\epsilon_{1}}

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 \theta and \gamma 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 \theta and \gamma would work well for high values of \epsilon_{1} and \epsilon_{2} (where high means closer to 1 than to 0). For lower values (such as for the box and cylinder cases where \epsilon_{1}=0.1) 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).
0. Whetting your appetite: Why superquadrics?
Thursday, June 19, 2014

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:

(1)\left( \left(\dfrac{x}{a}\right)^{\frac{2}{\epsilon_{2}}} + \left(\dfrac{y}{b}\right)^{\frac{2}{\epsilon_{2}}} \right) ^{\frac{\epsilon_{2}}{\epsilon_1} } + \left(\dfrac{z}{c}\right)^{\frac{2}{\epsilon_{1}}} = 1

As it can be seen, superellipsoids in their canonical form can be expressed by 5 parameters:

  • a,b,c: Scaling factors along principal axes
  • \epsilon_{1} : Shape factor of the superellipsoid cross section in a plane orthogonal to XY containing the axis Z.
  • \epsilon_{2} : Shape factor of the superellipsoid cross section in a plane parallel to XY.

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 (x,y,z,\rho,\psi,\theta))

Expression (1) shows the implicit equation of the superellipsoids. Its parametric solution can be expressed as:

x = a{\cos(\theta)}^{\epsilon_{1}}{\cos(\gamma)}^{\epsilon_{2}} \\
y = b{\cos(\theta)}^{\epsilon_{1}}{\sin(\gamma)}^{\epsilon_{2}} \\
z = c{\sin(\theta)}^{\epsilon_{1}}

with \theta \in [-\phi/2, \phi/2] and \gamma \in [-\phi,\phi]. In our next post we will learn how to generate pointclouds for superellipsoids (which is not as simple as just sampling \theta and \gamma! Stay tuned :).

[Barr81]Barr, Alan H. “Superquadrics and angle-preserving transformations.” IEEE Computer graphics and Applications 1.1 (1981): 11-23.