Fitting cone/cylinder of known dimensions to 3D surface data

I'm looking to fit a cone/cylinder of known dimensions to 3D surface data.

As a test, I created a cone using the parametric equations and downsample it to get some 3D surface data (see photo).

I need to fit to this data a cone whose dimensions are fixed but its orientation and origin are variable (so total 6 unknowns). These 6 parameters need to be determined (in the least square sense) based on the available data.

What would be the best approach to perform this?

I am using the following but so far have not had good luck with it.

1. A function that uses fsolve to get the cone surface from an implicit form of the cone equation given the 6 parameters.
2. lsqcurvefit that calls the above function and compares with the 3D data to optimize the 6 parameters.

Thanks!

[Matt J]

How are you generating your initial guess? Also, will your surface samples always include the vertex of the cone, like in your illustration?

[Matt J]

Also, I assume this is a right circular cone? I think a more general cone would have more parameters than 6.

[J. Alex Lee]

I like this outline of strategy from an old post that seems to be related: https://www.mathworks.com/matlabcentral/answers/296946-how-can-i-fit-a-3d-cone-at-varying-tilts-through-set-of-data-points-using-the-optimization-toolbox?s_tid=srchtitle

i guess in general you should not use fsolve (which is for root finding) but rather lsqcurvefit or lsqnonlin, but how do you get a good initial guess for these iterative solutions? do you need to have constraints on your parameters to make it a constrained optimization?

[AP]

Matt: I have not done anything specific to generate the initial guess. In this test case, I am providing a guess that is not too far from the original values. No, the 3D data points would not include the vertex (see example below). Its more like a part of the surface somewhere between the vertex and the base of the cone.

Yes, this is a right circular cone. I came across this answer (https://www.mathworks.com/matlabcentral/answers/109764-fitting-implicit-function-to-data) from you and I will see if that works for me.

Alex: Thanks for the link, I will check it out. I'm using fsolve because the cone surface (z) is related to the (x,y) coordinates through an implicit equation. Is there a better way?

[J. Alex Lee]

I don't think your response on the reasoning for using fsolve makes sense. When you say x, y, z coordinates are related implicitly, i think "minimize the function F(xData,yData,zData), where the surface of the cone is defined as 0=F(x,y,z)". Unless your data points were exactly on the surface, fsolve would fail. Alternatively, if your data points were exactly on the surface a bad/naive minimizer may fail.

Answer 1

[Matt J]

I've recently added a cone-fitting routine to this FIle Exchange package,

https://www.mathworks.com/matlabcentral/fileexchange/87584-object-oriented-tools-for-fitting-conics-and-quadrics

I still consider it a beta-version for now, but it's performed well so far in a number of tests. Here is an example of usage,

vertex=[1,2,3].';  %ground truth vertex coordinates
cone_angle=20;     %ground truth axis-to-surface angle
yaw=20; pitch=-45; %ground truth yaw/pitch orientation (degrees)


theta=[0:40:200];
h=linspace(5,10,31);

sig=0.01; %noise sigma
xyz = rightcircularconeFit.xyzsim(vertex,cone_angle,[yaw,pitch],...
                                  theta,h,sig);  %noisy surface samples

fobj=rightcircularconeFit(xyz),  %perform the fit

%Visualize the fit
fobj.plot() 
xlabel X; ylabel Y; zlabel Z;
axis vis3d

The results of the fit are,

fobj = 

  rightcircularconeFit with properties:

        vertex: [1.0001 1.9994 2.9979]
    cone_angle: 19.9902
        height: 10.0113
           yaw: 20.0104
         pitch: -45.0145

[Matt J]

Note: this is not a least squares fit, but if you really need a least squares fit, this should at least give you a good initial guess.

[AP]

Thank you very much for sharing this; it works great with the points that I created!

However, when I use the experimental data (see attached), I am getting very large numbers: cone_angle = 8.5e-7; height = 1.2e+9, yaw = 9, pitch = -6. This looks more like a planar fit to the data.

Since I know the dimensions of the cone (cone_angle = 7), could the code be modified to restrict the search space? Using something like the fminsearchbnd (https://www.mathworks.com/matlabcentral/fileexchange/8277-fminsearchbnd-fminsearchcon) instead of the fminsearch.

[Matt J]

SInce you have the Optimization Toolbox, the code below might help solve assuming an a priori known cone_angle. It's hard to predict whether my initialization strategy will extend well to your other data sets. But are you sure the cone angle isn't around 17 degrees for the last data set, rather than 7 degrees? With 17 degrees, I obtain a much better fit (EDIT: because there was a bug in the code which I have now corrected). If not, it's possible that you simply haven't enough surface samples to fit unambiguously.

load xyz

KnownAngle=7; %ground truth axis-to-surface angle

efit=ellipsoidalFit(xyz);
p0=[mean(xyz,2).',efit.ypr(1:2)]; %initiliazation strategy

fun=@(p) resid(p,xyz, KnownAngle);
options=optimoptions('lsqnonlin', 'OptimalityTol', 1e-12, 'StepTol',1e-12, 'FunctionTol',1e-12);
p=lsqnonlin(fun,p0,[],[],options);

 vertex=p(1:3);
 yp=p(4:5);
 [d1,d2,d3]=sph2cart(p(4)*pi/180,-p(5)*pi/180,1);
 fittedHeight = max( [d1,d2,d3]*(xyz-vertex(:)));


fobj=rightcircularconeFit.groundtruth(xyz,vertex,KnownAngle,fittedHeight,yp)
plot(fobj,{'FaceAlpha',0.6})


function fval=resid(p,xyz, KnownAngle)

      v=p(1:3); v=v(:);
      [d1,d2,d3]=sph2cart(p(4)*pi/180,-p(5)*pi/180,1);
      
      Zt=(xyz-v).';

      U=cosd(KnownAngle)*vecnorm(Zt,2,2);
      
      fval=Zt*[d1;d2;d3]-U;
 
end

[AP]

Thank you!

Is it possible to also provide the height of the cone (known = 460 mm) along with the cone_angle (known = 7 degree)? Your search resulted in a shorter-length larger-cone_angle cone instead of a longer-length smaller-cone_angle cone.

[Matt J]

Which version of the code are we talking about? Most recently, I gave you a version that lets you pre-specify an a priori known cone_angle. You should be finding that for smaller pre-specified cone_angles, the estimated vertex gets further from the data points. The section of the cone fit containing both the vertex and the data points will therefore be longer/taller.

Regardless, what constraints would a known height create between the samples and the other parameters? Are you guaranteed that some of your samples will lie at the circular rim of the cone's flat face?

[AP]

Yes, I was referring to the last code where you specified the 17 degree cone angle.

There are some points that I know exist along the base of the cone. So the height between the vertex and these points could be restricted by the known height input.

In the end, there are only 6 unkown parameters - vertex location (3) and cone axis orientation (3).

[Matt J]

In the end, there are only 6 unkown parameters - vertex location (3) and cone axis orientation (3).

That is in fact 5 parameters. The axis orientation only has 2 degrees of freedom.

[AP]

Got it, that makes sense since the vertex constraints the axis.

[Matt J]

Here is a version that constrains the height.

load xyz

KnownAngle=7;   % known  axis-to-surface angle
KnownHeight=460;

efit=ellipsoidalFit(xyz);
p0=[mean(xyz,2).',efit.ypr(1:2)]; %initiliazation strategy


%%First pass
fun=@(p) resid(p,xyz, KnownAngle);
con=@(p) nonlcon(p,xyz,KnownHeight);
options=optimoptions('fmincon', 'OptimalityTol', 1e-12, 'StepTol',1e-12, 'FunctionTol',1e-12);
p1=fmincon(fun,p0,[],[],[],[],[],[],con,options);

    vertex=p1(1:3);
    [~,index]=max(vecnorm(xyz-vertex(:),2,1));

%%Second pass - differentiable    
con=@(p) nonlcon(p,xyz,KnownHeight,index);
p=fmincon(fun,p1,[],[],[],[],[],[],con,options);

    vertex=p(1:3);
    yp=p(4:5);


fobj=rightcircularconeFit.groundtruth(xyz,vertex,KnownAngle,KnownHeight,yp)
plot(fobj,{'FaceAlpha',0.6})

function fval=resid(p,xyz, KnownAngle)

      v=p(1:3); v=v(:);
      [d1,d2,d3]=sph2cart(p(4)*pi/180,-p(5)*pi/180,1);
      
      Zt=(xyz-v).';

      U=cosd(KnownAngle)*vecnorm(Zt,2,2);
      
      fval=norm(Zt*[d1;d2;d3]-U).^2;
 
end

function [c,ceq]=nonlcon(p,xyz,KnownHeight,index)

      c=[];

      v=p(1:3); v=v(:);
      [d1,d2,d3]=sph2cart(p(4)*pi/180,-p(5)*pi/180,1);

      if nargin<4
        Zt=(xyz-v).';
        ceq=max(Zt*[d1;d2;d3])-KnownHeight; %non-differentiable, but still works
      else
          
         Zt=(xyz(:,index)-v).';
        
         ceq=Zt*[d1;d2;d3]-KnownHeight; %Differentiable!!! (but doesn't change much)
      end
end

[AP]

Great, thank you! Could you please share a link/article which describes the algorithm used here?

[Matt J]

I don't know of any prior sources, but if you find one, I'd be grateful to know about it.

[AP]

Sounds good!

Thank you very much for taking time to make the surface-fitting FEX package and for modifying it to cater to my problem.

Answer 2

[Matt J]

If you know where the given surface samples would be located on an unrotated/untranslated version of the cone/cylinder, then the rotation and translation can be calculated using absor (Download).

Answer 3

[Matt J]

I am providing a guess that is not too far from the original values.

If so, it sounds like there is an error in your calculation of the objective function and/or of the Jacobian, if you are providing it. One thing you should check is whether if you initialize the iterations with the exact original values, if lsqnonlin recognizes it as a solution, terminating in zero iterations with residuals=0.