Fitting sphere to quantized depth information

B. Paláncz and  B. Molnár

Department of Photogrammetry and Geoinformtics,
Budapest University of Technology and Economy
1521 Budapest, Hungary

Abstract

This notebook presents different techniques to estimate radius and position of a sphere in case of quantized depth information obtained from low resolution sensors like Microsoft Kinect XBOX. First algebraic, geometric and directional least squares estimations were applied to the quantized data directly. Then two techniques Self-Organizing Map (SOM) and RANdom SAmple Consensus (RANSAC) were employed as preprocessing methods to smooth and reduce quantized data. To solve the resulted nonlinear algebraic systems Gröbner basis with Gauss-Jacobi method as global method as well as Newton method with pseudoinverse and direct minimization as local methods applying the result of the algebraic method as initial guess have been used.  In order to decrease the computation time parallel computation on multi-core machine could be utilized . According to this case study all of these methods can be accepted from engineering point of view, although the geometrical approach with initial condition based on the solution of the algebraic method was proved to be the most effective.

1 - Introduction

For understanding the geometry of an object as a function of its depth values relative to the sensor is vital to have both a smooth and accurate estimate of the depth map. Due to the limitations of the depth sensing technology used by inexpensive devices, a significant loss incurs when capturing depth information. Unlike depth measuring technologies, like laser based LIDAR or range finding cameras, theses sensors contains many missing data and is incapable of measuring depth for shiny objects or objects on certain orientation. On the flip side, traditional depth capturing devices cost in the order of a several thousand dollars, while low resolution sensors costs around $100-200. These sensors mainly are purposed for close distance human pose recognition, like Microsoft’s Kinect and is operated in an environment with a conspicuous foreground (human) and background (room), a clean and smooth depth map is not necessary.
 However, if the ability of such sensors is to be extended to a more general setting such as the inclusion in consumer laptops for human-computer interaction, robotics for robot motion planning, or graphics for environment reconstruction both smooth and complete depth information are essential.and it could significantly enhance the accuracy of object detection.

2 - Problem definition

In our example the measurement has been carried out with Microsoft Kinect XBOX see Fig. 1 Microsoft’s Kinect contains a diverse set of sensors, most notably a depth camera based on PrimeSense’s infrared structured light technology. With a proper calibration of its color and depth cameras, the Kinect can capture detailed color point clouds at up to 30 frames per  second. This capability uniquely positions the Kinect for use in fields such as robotics, natural user interfaces, and three-dimensional mapping, see i.e. Draelos (2012)

Fig .1  Microsoft Kinect XBOX

This device provides 2D RGB color image and the RGB data representing the depth - the distance of the object - in 11 bits (0..2048). 
This low resolution causes discontinuities effect, which can be even 10 cm above 4 m object distance, and is about  2 cm in case when the object distance is less than 2.5 m. In our case a sphere object,  having radius R = 0.152 m was placed in the real world position x = 0, y = 0 and the object distance z = 3 m. Since the intensity thresholds resolution is low, one may have quantized levels caused by round-off process.  Fig.2. represents the quantized depth data points simulated as synthetic data for illustration.

Fig .2  Simulated synthetic quantized depth data points in case of a sphere from 3 different view points

Real measurements suffer not only from quantization but from random errors, too. Let us load a real measured data set adopted from Molnár, et al. (2012).

dataQ=Import["E:\\f_03_05.dat"]/1000;

The number of the data points is,

n=Length[dataQ]

The cloud of measured points are shown in Fig 3.

p1=ListPointPlot3D[dataQ,PlotStyle->Directive[Blue,PointSize[0.006]],Axes->False,Boxed->False,PlotRange->{{-0.2,0.2},{-0.2,0.2},{2.8,3.2}},BoxRatios->{1,1,1}];
GraphicsGrid[{{p1,p1}}];

Fig . 3 Real measured data points from different view points

There are three basic approaches for estimating the position (a, b, c) and  the radius  (R)  of a sphere from data point clouds, namely algebraic, geometric and directional least squares estimate i.e. Shah (2006).

3 - Algebraic least squares estimation when  R is unknown

In this case , we want to fit given n points to a sphere such that the sum of the squared algebraic distance is minimized, Then the local error is defined as

(Subscript[δ, i])=((((Subscript[x, i])-a))^2)+((((Subscript[y, i])-b))^2)+((((Subscript[z, i])-c))^2)-((R )^2)

where ((Subscript[x, i]), (Subscript[y, i]), (Subscript[z, i])) are the coordinates of the measured  point. First let us suppose that the position as well as the radius of the sphere are unknown.

The parameters to be estimated  are R, a, b and c. Let us introduce a new parameter,

d=(a^2)+(b^2)+(c^2)-(R^2)

Then  (Subscript[δ, i])  can be expressed as

(Subscript[δ, i])=(- 2(Subscript[x, i]),- 2(Subscript[y, i]),-2 (Subscript[z, i]), 1)(({{a}, {b}, {c}, {d}}))+(((Subscript[x, i])^2)+((Subscript[y, i])^2)+((Subscript[z, i])^2))

Consequently the parameter estimation problem is  linear, namely  one should solve an overdetermined system. In matrix form

M p = h

where

M=(({{-2 (Subscript[x, 1]), -2 (Subscript[y, 1]), - 2 (Subscript[z, 1]), 1}, {., ., ., .}, {-2 (Subscript[x, i]), -2 (Subscript[y, i]), - 2 (Subscript[z, i]), 1}, {., ., ., .}, {-2 (Subscript[x, n]), -2 (Subscript[y, n]), - 2 (Subscript[z, n]), 1}})), p = (({{a}, {b}, {c}, {d}}))  and  h=(({{-( ((Subscript[x, 1])^2)+((Subscript[y, 1])^2)+((Subscript[z, 1])^2))}, {.}, {-( ((Subscript[x, i])^2)+((Subscript[y, i])^2)+((Subscript[z, i])^2))}, {.}, {-( ((Subscript[x, n])^2)+((Subscript[y, n])^2)+((Subscript[z, n])^2))}}))

Then the solution in least square sense

(({{a}, {b}, {c}, {d}}))= (ℳ^(-1)) h

where (ℳ^(-1)) is the pseudoinverse of  M.

The  attractive feature of the algebraic parameter estimation is that its formulation results in a linear least squares problem which has non-iterative closed -form solution. Therefore the algebraic distance based fitting can be used as a quick way to calculate approximate parameter values without requiring initial guess.

First we create the M matrix.

M=Map[{-2 #[[1]],-2 #[[2]], -2 #[[3]], 1}&,dataQ];

Dimensions[M]

The right hand side vector is

h=Map[-( ((#[[1]])^2)+ ((#[[2]])^2)+ ((#[[3]])^2))&,dataQ];

Then we can compute the parameters a, b, c and d,

{a, b, c, d}=PseudoInverse[M].h

and the estimated radius of the sphere is

R=(Sqrt[(a^2)+(b^2)+(c^2)-d])

Since we want to use these parameter values as initial values for other methods, we assigned them to the following variables,

a0=a;b0=b;c0=c;d0=d;R0=R;

Let us display this estimated sphere with the data points

p2=Graphics3D[{Specularity[White,30],Orange,Opacity[0.1],Sphere[{a,b,c},R ]/.{a->a0,b->b0,c->c0,R->R0}},PlotRange->{{-0.2,0.2},{-0.2,0.2},{2.8,3.2}},Axes->False,BoxRatios->{1,1,1}];

Show[{p1,p2}]

Fig. 4 The measured data with the estimated  object

The disadvantages of this method that are its sensitivity for noise, outliers and partial occlusion, namely when the data points cover only a part of the surface of the sphere.

4 - Algebraic least squares estimation when R is known

Sometimes the radius of sphere is known i.e. in case of calibration, and we want to find only the position of the sphere. Therefore there are  3 parameters a, b and c to be estimated. Now we have an overdetermined polynomial system to be solved,

((((Subscript[x, i])-a))^2)+((((Subscript[y, i])-b))^2)+((((Subscript[z, i])-c))^2)-((R )^2)= 0       i= 1, 2, ...n

The equations can be easily generated:

Clear[a,b,c]; R=0.152;

the prototype of the equation,

G=(((x-a))^2)+(((y-b))^2)+(((z-c))^2)-(R^2);

Then the system can be created,

eqs=Map[G/.{x->#[[1]],y->#[[2]],z->#[[3]]}&,dataQ];

For example the first equation is,

eqs[[1]]

The number of the equations is,

Length[eqs]

The solution of this problem is not unique since the reduced Gröbner basis for the determined polynomial subsystems (n = 3) are second order polynomials, see Appendix 1A. To solve the overdetermined system one can use local or global techniques. The local methods need initial guess for the parameter values, which can be the solution of the linear problem. One of the most effective local methods is the Extended Newton method. The Extended Newton method can handle nonlinear overdetermined systems since using pseudoinverse of the Jacobian, see Awange et al (2011).

NewtonExtended[f_List,x_List,x0_List,eps_:(10^(-12)),n_:100]:=With[{jac=N[Outer[D,f,x]]},FixedPointList[(#+PseudoInverse[jac/.Thread[x->N[#]]].(-f/.Thread[x->N[#]]))&,N[x0],n,SameTest->(Sqrt[Abs[(#1-#2).(#1-#2)]]<eps&)]]//Last

Let us employ the result of the algebraic solution as initial guess

NewtonExtended[eqs,{a,b,c},{a0,b0,c0}]

Another local method is based on the direct minimization of the squares of the residuals of the equations,

ρ = (∑_(i=1)^n)((Subscript[δ, i])^2)=(∑_(i=1)^n)(((((((Subscript[x, i])-a))^2)+((((Subscript[y, i])-b))^2)+((((Subscript[z, i])-c))^2)-R ))^2)

Now we employ local minimization with the initial guess again computed for the linear problem. The objective function is

W=Apply[Plus,Map[((#)^2)&,eqs]];

Then

FindMinimum[W,{{a,a0},{b,b0},{c,c0}}]

The solution of the two methods is the same, but the computation time of  the local minimization is somewhat shortest.

Although as we mentioned, the reduced Gröbner basis of the determined system (n = 3) can be computed in symbolic form represented by second order polynomials, the high number of the subsystems, namely

Binomial[n,3]

hinders the realization of the Gauss-Jacobi combinatorial computation as a global method. However, later we shall demonstrate, that if one can represent the cloud of points with considerable less points, for example via SOM (Self Organizing Map), then the symbolic solution is reasonable alternative of the local methods.

5 - Geometric fitting when R is unknown

In this case, we want to minimize the sum of squared geometric distances to the point set. Then the local error is defined as

(Subscript[δ, i])=(Sqrt[((((Subscript[x, i])-a))^2)+((((Subscript[y, i])-b))^2)+((((Subscript[z, i])-c))^2)])-R

Now we have an overdetermined nonlinear equation system to be solved for the parameter estimation problem even in case of  unknown R. However in this case, the solution of the system is unique, since the Gröbner basis of the determined subsystems are first order polynomials.  In general the use of  geometric distance results in better solution compared to the algebraic distance. This is especially true in the presence of noise and small outliers. Similarly to the algebraic approach, local methods like Extended Newton method as well as direct local minimization  can be employed. 

Let us create the system of the equations. First we reset a, b, c and R to be symbolic variables again,

Clear[a,b,c,R]

The prototype of the equation,

G=(Sqrt[(((x-a))^2)+(((y-b))^2)+(((z-c))^2)])-R;

Then the system can be created as,

eqs=Map[G/.{x->#[[1]],y->#[[2]],z->#[[3]]}&,dataQ];

For example the first equation is

eqs[[1]]

Let us employ the Extended Newton method with the result of the algebraic solution as initial guess

NewtonExtended[eqs,{a,b,c,R},{a0,b0,c0,R0}]

Indeed, the geometric approach provides better estimation than algebraic one, however it needs a good initial guess.

The other possible solution is minimization of  the squares of the residuals of the equations,

ρ = (∑_(i=1)^n) ((Subscript[δ, i])^2)=(∑_(i=1)^n)((((Sqrt[((((Subscript[x, i])-a))^2)+((((Subscript[y, i])-b))^2)+((((Subscript[z, i])-c))^2)])-R ))^2)

Now again we can employ local minimization with the initial guess  computed from the linear problem,

W=Apply[Plus,Map[((#)^2)&,eqs]];

FindMinimum[W,{{a,a0},{b,b0},{c,c0},{R,R0}}]

Again  this computation required less time than the Newton  method.

6 - Geometric fitting when  R is known

Again one can use the Extended Newton method,

NewtonExtended[eqs/.R->0.152,{a,b,c},{a0,b0,c0}]

The  solution with the direct local minimization

FindMinimum[W/.R->0.152,{{a,a0},{b,b0},{c,c0}}]

In this case the nonlinear system has more solutions since the Gröbner basis of the determined subsystems are second order polynomials, see Appendix. A.2 Generally speaking, the algebraic as well as the geometric approach with known radius are always lead to nonlinear problems, where in case of geometric model the solution is not unique.

7 - Directional Fitting

For directional fitting, the error function is based on the distance between the measured point (Subscript[P, i]) = P{(Subscript[x, i]), (Subscript[y, i]) , (Subscript[z, i])} and its projection onto the surface of the  sphere closer to the instrument along the direction of (Subscript[P, i]), see Fig. 5

Fig. 5 Comparison of the geometric and directional fitting

In Cartesian coordinates, the expressions for (Subscript[r, i]), (Subscript[p, i]) and (Subscript[q, i]) (see Fig. 5) can be written as, see i.e.  Franaszek et al  (2009)

(Subscript[r, i])=(Sqrt[((((Subscript[x, c])-(Subscript[x, i])))^2)+((((Subscript[y, c])-(Subscript[y, i])))^2)+((((Subscript[z, c])-(Subscript[z, i])))^2)])

(Subscript[p, i]) = a (Subscript[u, i])+b (Subscript[v, i])+c (Subscript[w, i])

and

(Subscript[q, i])=(Sqrt[((((Subscript[v, i])c-(Subscript[w, i])b))^2)+((((Subscript[w, i]) a-(Subscript[u, i])c))^2)+((((Subscript[u, i])b-(Subscript[v, i])a))^2)])

where

(Subscript[u, i])=((Subscript[x, i])/(Subscript[r, i])), (Subscript[v, i])=((Subscript[y, i])/(Subscript[r, i]))     and      (Subscript[w, i])=((Subscript[z, i])/(Subscript[r, i]))

The system of the nonlinear equations are

((((Subscript[p, i])- (Sqrt[(R^2)-((Subscript[q, i])^2)])-(Subscript[r, i])))^2)=0   if   (Subscript[q, i])<R

((((Subscript[p, i])-(Subscript[r, i])))^2)+((((Subscript[q, i])- R))^2)   = 0   if  (Subscript[q, i]) ≥ R

In our case the camera is in the origo,

(Subscript[x, c])=0,  (Subscript[y, c])=0   and      (Subscript[z, c])=0

This algorithm can be summarize in the following module,

Res[a_,b_,c_,R_,data_]:=Module[{xc=0,yc=0,zc=0,RR,P,Q,Z1,Z2,Z},
RR=Map[Norm[{xc,yc,zc}-#]&,data];
dataQN=MapThread[#1/#2&,{data,RR}];
P=Map[{a,b,c}.#&,dataQN];
Q=Map[(√((((#[[2]] c-#[[3]] b))^2)+(((#[[3]] a-#[[1]] c))^2)+(((#[[1]] b-#[[2]] a))^2)))&,dataQN];
Z1=MapThread[(((#1-(Sqrt[(R^2)-((#2)^2)])-#3))^2)&,{P,Q,RR}];
Z2=MapThread[(((#1-#3))^2)+(((#2-R))^2)&,{P,Q,RR}];
Z=MapThread[If[#1<R,#2,#3]&,{Q,Z1,Z2}]];

Now, let us create the nonlinear system of the problem,

Eqs=Res[a,b,c,R,dataQ];

The number of equations are

Length[Eqs]

For example the first equation is,

Eqs[[1]]

The Extended Newton method can not handle these equations properly because the derivatives can not compute symbolically, therefore local minimization is used.

The objective function is the sum of the residual of the equations,

W= Apply[Plus,Eqs];

Then the local minimization is employed with a derivative free method,

FindMinimum[W,{{a,a0},{b,b0},{c,c0},{R,R0}},Method->"PrincipalAxis"]

Now, let us assume that R is known. Then we have the same equation system where R = 0.152.

Then the local minimization gives

FindMinimum[W/.R->0.152,{{a,a0},{b,b0},{c,c0}},Method->"PrincipalAxis"]

In the following  sections we shall demonstrate two methods - the application of  Self-Organizing Map (SOM) and  that of  RANdom SAmple Consensus (RANSAC) -t which can reduce the data points and open the way for the application of the Gauss-Jacobi combinatoric solution.

8 - Application Self-Organizing Map for reducing data

The Self-Organizing Map (SOM) is a neural network algorithm, which uses a competitive learning technique to train itself in an unsupervised manner. SOMs are different from other artificial neural networks in the sense that they use a neighborhood function to preserve the topological properties of the input space and they have been used to create an ordered representation of multi-dimensional data which simplifies complexity and reveals meaningful relationships. 
In the last decade a lot of research has been done to develop surface reconstruction methods. A more recent approach to the problem of surface reconstruction is that of learning based methods. Learning algorithm are able to process very large and/or noisy data, such as point clouds obtained from 3D scanners and have been used to construct surfaces. Following this approach some studies have been employed SOM and their variants for surface reconstruction. SOM is suitable for this problem because it can form topological maps replicate the distribution of input data. In our case this mapping occurs from 3D space to 2D one i.e. DalleMole et al (2010)                                                                                                                       

Therefore we try to represent this cloud of data points by considerably less points using Kohonen-map with a lattice of  SOM (Self Organizing Map) of size 5 × 5 code-book vectors with symmetrical neighborhood.

<<NeuralNetworks`

{som,fitrecord}=UnsupervisedNetFit[dataQ,25,100,SOM->{5,5},Neighbor->Symmetric];

Fig. 6 Convergence of the Kohonen neural network

The 25 representing code-book vectors are

dataSOM=som[[1]];

Let us display the code-book vectors with the sphere to be estimated, see Fig.7.

p3=ListPointPlot3D[dataSOM,PlotStyle->Directive[Red],BoxRatios->{1,1,1}];

Show[{p2,p3}]

Fig .7 The code-book vectors with the exact target object

At this time the application of Gauss-Jacobi algorithm as a global method to solve polynomial systems  is reasonable, since now the number of the subsets is “only”

Binomial[25,4]

when R is unknown, and

Binomial[25,3]

when R is known.

But before to do that, let us compute the parameters from these code-book vectors as input data in order with the earlier methods to see how precise result can be expected at all with reduced data set. First we consider geometric fitting with unknown and with known radius.

The parameters when R is unknown:

Clear[a,b,c,R]

Then the system can be created

eqs=Map[G/.{x->#[[1]],y->#[[2]],z->#[[3]]}&,dataSOM];

Let us employ the Extended Newton method with the result of the algebraic solution as initial guess

NewtonExtended[eqs,{a,b,c,R},{a0,b0,c0,R0}]

Then for known radius, we get

NewtonExtended[eqs/.R->0.152,{a,b,c},{a0,b0,c0}]

It means that the code-book vectors  provide a quite good representation of the point cloud. 

Let us compute the parameters from the directional approach, too. Creating the nonlinear system of the problem for R is unknown,

Eqs=Res[a,b,c,R,dataSOM];

The number of equations are

Length[Eqs]

For example the first equation is,

Eqs[[1]]

Again let us employ local minimization

W=Apply[Plus,Eqs];

FindMinimum[W,{{a,a0},{b,b0},{c,c0},{R,R0}},Method->"PrincipalAxis"]

However, now SOM provides a smooth and well arranged data point set, therefore Newton method can be also successful,

NewtonExtended[Eqs,{a,b,c,R},{a0,b0,c0,R0}]

Now let us compute the Gröbner basis for the equation system resulted from the geometric approach, when R  is unknown.

9 - Symbolic computation of Gröbner basis for geometric fitting

Considering  4 exact noiseless measurement  points, then  prototype system for these 4 points is,

proto=Table[(G/.{x->(Subscript[η, i]),y->(Subscript[ξ, i]),z->(Subscript[χ, i])}),{i,1,4}]

One can realize, that the elimination of  R reduces the system

protoR=Take[proto,{2,4}]/.R->(Sqrt[(((-a+(Subscript[η, 1])))^2)+(((-b+(Subscript[ξ, 1])))^2)+(((-c+(Subscript[χ, 1])))^2)])

To get univariate polynomial for parameter  a let us eliminate  b and c from the basis

grba=GroebnerBasis[protoR,{a,b,c},{b,c}]//Simplify;

This basis contains

Length[grba]

polynomials. We take the first one

grba[[1]]

It turns out that this is a first order polynomial, since

ca2=Coefficient[grba[[1]],a,2]

Therefore the solution for parameter a is simple,

aG=-((Coefficient[grba[[1]],a,0])/(Coefficient[grba[[1]],a,1]))//Simplify

This means that from the coordinates of the corresponding 4 points the parameter can be directly computed. Similar expressions can be developed for the other two parameters b and c, too.

For parameter b:

grbb=GroebnerBasis[protoR,{a,b,c,R},{a,c}];

bG=-((Coefficient[grbb[[1]],b,0])/(Coefficient[grbb[[1]],b,1]))//Simplify

For parameter c:

grbc=GroebnerBasis[protoR,{a,b,c,R},{a,b}];

cG=-((Coefficient[grbc[[1]],c,0])/(Coefficient[grbc[[1]],c,1]))//Simplify

10 - Geometric fitting via Gauss-Jacobi method with SOM data

We can get a good approximation to compute the arithmetic average of the solution of the (({{25}, {4}})) = 12 650 subsets. Let us generate the indices of the equations belonging to the same subset,

index=Partition[Map[#&,Flatten[Subsets[Range[25],{4}]]],4];

For example the (10^th) subset consists of the following equations

index[[10]]

From now, we apply parallel computation to reduce computation time. The coordinates of the points of  the subsets are

dataSub=ParallelMap[Map[dataSOM[[#]]&,#]&,index];

For example

dataSub[[10]]

In order to avoid ill - posed subsets, we compute the product of the distances of every vertex of a quadrant representing a subset,

avgSide[x_]:=Apply[Times,Map[Norm[x[[#[[1]]]]-x[[#[[2]]]]]&,{{1,2},{1,3},{1,4},{2,3},{2,4},{3,4}}]]

Let us  select the only subsets for which this product is higher than 2.5× (10^(-5))

dataSubS=Parallelize[Select[dataSub,avgSide[#]>0.000025&]];

Generating a prototype for the equations of the subsets

psi=Table[({(Subscript[η, i]),(Subscript[ξ, i]),(Subscript[χ, i])}),{i,1,4}];

Substituting numerical values

dataS=ParallelMap[Flatten[MapThread[MapThread[#1->#2&,{#1,#2}]&,{psi,#}]]&,dataSubS];

Then the number of the equations to be considered is

Length[dataS]

instead of the 12 650.

Employing the result of the Gröbner basis the parameter a is

aS=ParallelMap[aG/.#&,dataS];

Then the average is

aaS=Mean[aS]

The parameters  b and c can be computed in the same way

bS=ParallelMap[bG/.#&,dataS];

bbS=Mean[bS]

and

cS=ParallelMap[cG/.#&,dataS];

ccS=Mean[cS]

Now  employing these averages of  computed parameters  to compute  the radius of the sphere

eqsR=Parallelize[Map[proto/.#1/.{a->aaS,b->bbS,c->ccS}&,dataS]];

RS=Map[#[[1]][[1]]&,eqsR];

RRS=Mean[RS]

11 - Application of  RANdom SAmple Consensus (RANSAC)

11 - 1 RANSAC algorithm

In addition we investigated the application of the RANSAC method, i.e. Fischler and Bolles (1981), which is proved to be successful for detecting outliers. The basic RANSAC algorithm is the following:

1) Pick up a model type (ℳ)
2) Input data as
     -  dataQ  - data contaminated with outliers (cardinality (dataQ) = n)
     -  s     - number of data elements required per subset
     -  𝒩           -  number of subsets draw from the data
     -  τ            -  threshold which defines if  data element, (Subscript[d, i])∈ dataQ, agrees with the model ℳ

Remarks

In general s can be the minimal number of the data which results a determined system for the unknown parameters of the model.
The number of subsets draw from the data, 𝒩 is chosen high enough to ensure that the probability p (usually set to 0.99) that at least one of the subsets of the random examples does not include an outliers. Let u represent the probability that any selected data point is an inlier and v = 1 - u the probability of observing an outlier. Then the iterations 𝒩 can be computed as

𝒩 = ((log(1-p))/(log(1-(((1-v))^s))))

3) maximalConsensusSet ← ∅
4) Iterate 𝒩 times:
            a) consensusSet ← ∅
            b) Randomly draw a subset containing s elements and estimate the parameters of the model ℳ
            c) For each data element,  (Subscript[d, i])∈ dataQ:
                if Agree ((Subscript[d, i]),ℳ, τ), consensusSet ← (Subscript[d, i])
            d) if cardinality (maximalConsensusSet) < cardinality(consensusSet),
                maximalConsensusSet ←   consensusSet
                
   5) Estimate model parameters using maximalConsensusSet

11 - 2 The model: algebraic least squares estimation when  R is unknown

In order to see how this data reduction technique can work in our case, let us select the most simple model, namely the algebraic least squares estimation when R is known.

Now, number of data elements required per subset s is

s=4;

The solution of a subsystem of size s (which is a determined system) is

M=(({{-2 (Subscript[x, 1]), -2 (Subscript[y, 1]), - 2 (Subscript[z, 1]), 1}, {., ., ., .}, {-2 (Subscript[x, i]), -2 (Subscript[y, i]), - 2 (Subscript[z, i]), 1}, {., ., ., .}, {-2 (Subscript[x, s]), -2 (Subscript[y, s]), - 2 (Subscript[z, s]), 1}})), p = (({{a}, {b}, {c}, {d}}))  and  h=(({{-( ((Subscript[x, 1])^2)+((Subscript[y, 1])^2)+((Subscript[z, 1])^2))}, {.}, {-( ((Subscript[x, i])^2)+((Subscript[y, i])^2)+((Subscript[z, i])^2))}, {.}, {-( ((Subscript[x, s])^2)+((Subscript[y, s])^2)+((Subscript[z, s])^2))}}))

and

M p = h

Remark

Although it is a determined system, subsets representing  ill-posed configuration can arise. Therefore the best way to avoid this situation to restrict the conditional number of M. Let say if this conditional number is higher than 1000 we leave out the computation of the subset and draw another one. Here we employ another less efficient method, namely we compute pseudoinverse.

11 - 3 Computation of the necessary number of the iterations

The total number of the data is,

n

Let

p=0.99;

In addition let us assume that the probability of observing outlier is

v=1/2;

Then the number of the necessary iterations is

𝒩=Round[Log[1-p]/Log[1-(((1-v))^s)]]

Remark

Higher this probability more the necessary iterations

Thinking of Gauss- Jacobi technique, which can be also employed to kick out outliers the all possible subset of  which can be drawn from dataQ

Binomial[n,s]

11 - 4 Computation of  the threshold

Let the threshold τ

τ=1;

An element  (Subscript[d, i])∈ dataQ will be accepted if

|((((Subscript[x, i])-(Subscript[a, s])))^2)+((((Subscript[y, i])-(Subscript[b, s])))^2)+((((Subscript[z, i])-(Subscript[c, s])))^2)-(((Subscript[R, s]) )^2)|< (τ^2)

Remarks

(Subscript[d, i]) is represented by the triplet ((Subscript[x, i]), (Subscript[y, i]), (Subscript[z, i])) and the parameters ((Subscript[a, s]),(Subscript[b, s]),(Subscript[c, s]),(Subscript[R, s])) computed from a subset of s elements.

11 - 5 The computation model step by step

inliers=Table[{},{𝒩}] (* the consensus sets, j = 1,2, ...𝒩*)

Let us consider a subset  (j = 1,2, ...𝒩).

j=2 (*second iteration means second subset*)

(*get four random points without replacement*)

dataS=RandomSample[dataQ,4]

(*determine inliers for a subset*)

First we compute the parameters from this subset. Let us create the M matrix.

M=Map[{-2 #[[1]],-2 #[[2]], -2 #[[3]], 1}&,dataS];M//MatrixForm

Dimensions[M]

The right hand side vector is

h=Map[-( ((#[[1]])^2)+ ((#[[2]])^2)+ ((#[[3]])^2))&,dataS];

Then we can compute the parameters a, b, c and d,

P={a, b, c, d}=PseudoInverse[M].h

and the estimated radius of the sphere is

R=(Sqrt[(a^2)+(b^2)+(c^2)-d])

Then check for all elements of dataQ (i = 1,2, ..., n)  whether it can satisfy the threshold. Let

i=3;

dataQ[[i]]

Abs[(((dataQ[[i]][[1]]-a))^2) +(((dataQ[[i]][[2]]-b))^2)+(((dataQ[[i]][[3]]-c))^2)-(R^2)]

If[Abs[(((dataQ[[i]][[1]]-a))^2) +(((dataQ[[i]][[2]]-b))^2)+(((dataQ[[i]][[3]]-c))^2)-(R^2)]<(τ^2),inliers[[j]]=Append[inliers[[j]],dataQ[[i]]]]

inliers

If this double For cycle

for j = 1, 2, ... 𝒩
  for i = 1,2,...n

is over we find out the maximal consensus set and using this set - whose size is between s and n, so mostly overdetermined - to compute the parameters.

11 - 6 The implementation of the computation model

Now let us use for threshold τ = 1.5 cm

τ=0.015;

Preparation of the sets of inliers for every iteration step - at the beginning they are empty sets:

inliers=Table[{},{𝒩}] (* the consensus sets, j = 1,2, ...𝒩*);

For[j=1,j≤𝒩,j++,
(*get four random points*)
dataS=RandomSample[dataQ,4];
(*determine model parameters*)
M=Map[{-2 #[[1]],-2 #[[2]], -2 #[[3]], 1}&,dataS];
h=Map[-( ((#[[1]])^2)+ ((#[[2]])^2)+ ((#[[3]])^2))&,dataS];
P={a, b, c, d}=PseudoInverse[M].h;
R=(Sqrt[(a^2)+(b^2)+(c^2)-d]);
(* determine inliers*)
For[i=1,i≤n,i++,
If[Abs[(((dataQ[[i]][[1]]-a))^2) +(((dataQ[[i]][[2]]-b))^2)+(((dataQ[[i]][[3]]-c))^2)-(R^2)]<(τ^2),inliers[[j]]=Append[inliers[[j]],dataQ[[i]]]];
];];

Select the consensus set which has the highest number of inliers

trimmeddata=inliers[[Ordering[inliers,-1]]][[1]];

Length[trimmeddata]

As last step we apply algebraic least squares with the maximal consensus set

M=Map[{-2 #[[1]],-2 #[[2]], -2 #[[3]], 1}&,trimmeddata];

Dimensions[M]

The right hand side vector is

h=Map[-( ((#[[1]])^2)+ ((#[[2]])^2)+ ((#[[3]])^2))&,trimmeddata];

Then we can compute the parameters a, b, c and d,

P={a, b, c, d}=PseudoInverse[M].h

and the estimated radius of the sphere is

R=(Sqrt[(a^2)+(b^2)+(c^2)-d])

The selected points - the elements of the maximal consensus set - are close to the surface of the  sphere, see Fig.8.

p4=Graphics3D[{Specularity[White,30],Orange,Opacity[0.1],Sphere[{a,b,c},R ]},PlotRange->{{-0.2,0.2},{-0.2,0.2},{2.8,3.2}},Axes->False,BoxRatios->{1,1,1}];

p5=ListPointPlot3D[trimmeddata,PlotStyle->Directive[Red],BoxRatios->{1,1,1}];

Show[{p4,p5}]

Fig 8 Elements of the maximal consensus set

It seems better result, than direct algebraic approach without RANSAC and quite close to the result of the directional approach.

Conclusions

To find the sphere parameters, different methods: algebraic,  geometrical and directional method  have been employed. The algebraic approach results a linear least square problem which does not require initial guess. The geometric as well as the directional approach lead to a nonlinear problem requires iteration with an initial guess. This initial guess can be the result of the algebraic method. One also may employ direct global minimization of the sum of the residual of the geometrical model equations, however, it may need restrictions for the search space. 

As an alternative method, Gauss-Jacobi combinatorial method based on Gröbner basis solution utilizing SOM representation of  the measured data points has been demonstrated. This method similarly to the algebraic solution does not requires initial guess. Parallel computation on multi-cores machine can be utilized decreasing the computation time considerably. The results of the different methods are in the following Tables.

Table 1

Estimated parameters employing different methods
(R is unknown)

({{Type of estimation, a[mm], b [mm], c [mm], R [mm]}, {Algebraic, -41.4, -8.5, 300.2, 140.0}, {Geometric , -41.1, -8.5, 301.2, 145.9}, {Directional , -36.7, -7.4, 300.0, 140.1}, {Geometric based on  SOM, -41.0, -7.9, 300.6, 140.0}, {Directional based on  SOM, -40.3, -9.1, 301.1, 142.8}, {Gauss-Jacobi with Gröbner based on SOM, -40.0, -11.0, 300.6, 140.0}, {RANSAC with Algebraic, -41.0, -8.4, 300.8, 140.3}})

Table 2

Estimated parameters employing different methods
(R = 152 mm)

({{Type of estimation, a[mm], b [mm], c [mm]}, {Algebraic, -40.4, -7.5, 301.8}, {Geometric , -40.7, -7.9, 302.0}, {Directional , -36.7, -7.4, 302.0}, {Geometric based on  SOM, -42.3, -9.6, 302.4}, {Directional based on  SOM, -36.9, -8.0, 302.0}})

References

Awange et al. (2011) Algebraic Geodesy and Geoinformatics, Springer, Heidelberg, Berlin.

Mark Theodore Draelos (2012) The Kinect Up Close: Modifications for Short-Range Depth Imaging, A thesis Master of Science, Electrical Engineering, Raleigh, North Carolina

Marek Franaszek, Geraldine S. Cheok, Kamel S. Saidi, Christoph Witzgall  (2009) Fitting Spheres to Range Data From 3-D Imaging Systems. IEEE T. Instrumentation and Measurement 58(10): 3544-3553 

B. Molnár, C. K. Toth,  A. Detrekõi (2012)  Accuracy Test of Microsoft Kinect for Human Morphological Measurements,  International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XXXIX-B3, 2012 XXII ISPRS Congress, 25 August - 01 September 2012, Melbourne, Australia

Tahir Rabbani Shah (2006) Automatic Reconstruction of Industrial Installations Using Point Clouds and Images
Publications on Geodesy 62, NCG Netherlands Commissie voor Geodesie Netherlands Geodetic Commission
Delft, 

Vilson L. DalleMole, Renata L.M. E. do Rego and Aluizio F.R. Araujo (2010). The Self-Organizing Approach for Surface Reconstruction from Unstructured Point Clouds, p. 167-188. in Self-Organizing Maps, George K Matsopoulos (Ed.) INTECH, Rijeka, Croatia.

Westfeld, P.; Mulsow, C.; Schulze, M. (2009): Photogrammetric calibration of range imaging sensors using intensity and range information simultaneously. Grün, A.; Kahmen H. (Eds.): Optical 3-D Measurement Techniques IX. Vol. II, pp. 129, Research Group Engineering Geodesy, Vienna University of Technology.  

Zhou Shijian, Guan Yunlan, Zhan Xinwu and Lu Tieding (2008) Robust Algorithm for fitting Sphere to 3D Point Clouds in Terrestrial Laser Scanning,The Int. Archives of Photogrammetry and Spatial Information Sci. Vol. XXXVII Part B5. Beijing. p. 519-522.

Fischler M A, Bolles R C (1981) Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography. CCommunications of theACM, 24 (6): 381-395 (document).

Appendix

A1 - Algebraic approach:  Computation of the Gröbber basis, when R is known

Would it be 3 exact noiseless measurement  points, then  prototype system for 3 points

Clear[a,b,c,R];

The prototype of the equations,

G=(((x-a))^2)+(((y-b))^2)+(((z-c))^2)-(R^2);

proto=Table[(G/.{x->(Subscript[η, i]),y->(Subscript[ξ, i]),z->(Subscript[χ, i])}),{i,1,3}]

The polynomial for a is

grba=GroebnerBasis[proto,{a,b,c},{b,c}]//Simplify;

Coefficient[grba,a,2]//Simplify

Coefficient[grba,a,3]//Simplify

It means we have a second order polynomial indicating non-unique solution.

A2 - Geometric approach:  Computation of the Gröbner basis, when R is known

Would it be 3 exact noiseless measurement  points, then  prototype system for 3 points

Clear[a,b,c,R];

The prototype of the equations,

G=(Sqrt[(((x-a))^2)+(((y-b))^2)+(((z-c))^2)])-R;

proto=Table[(G/.{x->(Subscript[η, i]),y->(Subscript[ξ, i]),z->(Subscript[χ, i])}),{i,1,3}]

The polynomial for a is

grba=GroebnerBasis[proto,{a,b,c},{b,c}]//Simplify;

Length[grba]

ca2=Coefficient[grba[[1]],a,2]//Simplify

ca3=Coefficient[grba[[1]],a,3]//Simplify

Again, we have second order polynomial indicating non-unique solution.