In many LIDAR applications, after filtering and segmenting cloud points geometrically or semantically, we need to fit some sets of point clouds into some basic geometric models.
In this post, we compare 2 methods of fitting a given set of 3D points into a sphere.
- Least Square Fit
- Random Sample Consensus (RANSAC)
The mathematical expression of the sphere model is as follows:
(x - x₀)² + (y - y₀)² + (z - z₀)² = r²
The goal is to figure out the parameters, the sphere center (x₀, y₀, z₀) and r.
Least Square Fit
x² - 2*x₀*x + x₀² + y² - 2*y₀*y + y₀² + z² - 2*z₀*z + z₀² = r²
x² + y² + z² = 2*x*x₀ + 2*y*y₀ + 2*z*z₀ + (x₀² + y₀² + z₀² — r²)
By plugging (x, y, z) values into the equation above, we get a set of 4-variable linear equation.
let’s denote
then vector f can be seen as a linear combination of vector x, y, z
equation-1 can also be expressed in matrix multiplication form
we denote
so that we get
So far, we have formulated the problem mathematically, the next problem is how we solve the equation-7 .
Theoretically, as equation-3 shows, vector f is a linear combination of vector x, y, z. In case we have a set of perfect points of which every point falls exactly on the sphere, this equation has a solution. However, in reality, the data points may not fit perfectly onto a sphere, some may fall inside and some may fall outside of the sphere. In this case, we may work out a least-squares solution c’, which minimizes the Euclidean 2-norm error||f - Ac’||
Let’s generate a set of noisy data points of a sphere by calling the create_noisy_sphere_points with the ground truth parameters, then call the lstsq_sphere_fitting to solve the parameters of the sphere and visualize how this least square sphere fitting method works out.
visualize the 3D points to check out the outcome of least square fitting
Random Sample Consensus
The Random Sample Consensus (RANSAC) algorithm is an iterative method that is often used to estimate parameters of a mathematical model from a set of data containing outliers.
The Wikipedia page of the RANSAC algorithm outlines how RANSAC works as follows:
The input to the RANSAC algorithm is a set of observed data values, a way of fitting some kind of model to the observations, and some confidence parameters. RANSAC achieves its goal by repeating the following steps:
1. Select a random subset of the original data. Call this subset the hypothetical inliers.
2. A model is fitted to the set of hypothetical inliers.
3. All other data are then tested against the fitted model. Those points that fit the estimated model well, according to some model-specific loss function, are considered as part of the consensus set.
4. The estimated model is reasonably good if sufficiently many points have been classified as part of the consensus set.
5. Afterwards, the model may be improved by reestimating it using all members of the consensus set.
We can use the RANSAC implementation of the PCL library to fit our noisy testing data into a sphere.
However, different from Least Square Method, RANSAC algorithm requires setting problem-specific thresholds. In this tutorial, we are supposed to set the distance threshold (threshold value to determine data points that are fit well by model).
We use the following testing data, the blue points depicted in Figure 1, to illustrate how the distance threshold affects sphere parameter estimation.
with the distance threshold set to 1.0, the estimated model coefficients are:
center.x is: -0.074, center.y is: 0.51, center.z is: 0.095
sphere radius is: 0.98
With the distance threshold set to 0.1, the estimated model coefficients are:
center.x is: 0.054, center.y is: 0.006, center.z is: -0.001
sphere radius is: 0.983
With the distance threshold set to 0.01, the estimated model coefficients are:
center.x is: -1.85e+13, center.y is: -9.83e+12, center.z is: 1.08
sphere radius is: 2.1e+13
As you see, by setting a large threshold 1.0, the estimated sphere center is a bit off. When the distance threshold is set to a small value of 0.01, the sphere parameters are way off the ground truth. A good threshold distance turns out to be 0.1.
The prerequisites of building and executing this c++ code above are:
- install PCL library
- copy paste the CMakeLists.txt file
In order to test the robustness of the RANSAC algorithm, we use 5000 points of the sphere (blue points in Figure 2, 3, 4) and different level noise (red points in Figure 2, 3, 4) to perform the robustness test.
1000 noisy points :
(center.x : 1.1e-05, center.y : -1.1e-05, center.z : 1.7e-06, radius : 1)
3000 noisy points :
(center.x : 0.0, center.y : 6.7e-08, center.z : -1.7e-08, radius : 1)
5000 noisy points :
(center.x : 3.2e-08, center.y : 1.0e-08, center.z : -3.0e-08, radius : 1)
7000 noisy points :
(center.x : 0.0, center.y : 0.0, center.z : -0.003, radius : 0.997)
Random Sample Consensus algorithm turns out robust to noise/outliers.
Discussion
The advantage of RANSAC is its robustness to outliers.
The disadvantage of RANSAC is the settings of problem-specific thresholds. As it’s iterative, it doesn’t have a definitive computing time upper bound.
The advantage of the Least Square Method is that it doesn’t require any threshold tuning. Besides, it has a computing time upper bound.
