The clusters (one-view) are mirrored based on [Bohg2011] (tabletop_symmetry/mindGapper.cpp)

The mirrored clouds are then fitted to a SQ (SQ_fitter_test1.cpp)

Observations:

Using mirrored pointclouds helps substantially to the fitting process.

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.

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.

Fitting times (still in debug mode, not optimized and with warning printouts here and there) is < 2 seconds in my old laptop.

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?

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:

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.

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)

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:

(2)

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:

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

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 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 was randomly applied to each point in the pointcloud.

Case 2: Parameters

Parameter

Sphere

Ellipsoid

Cylinder

Box

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 and 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).

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:

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.

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)

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

: Scaling factors along principal axes

: Shape factor of the superellipsoid cross section in a plane orthogonal to XY containing the axis Z.

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

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