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

Question

AP 2021-6-10

1
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/853345-fitting-cone-cylinder-of-known-dimensions-to-3d-surface-data

评论： AP 2021-6-15

采纳的回答： Matt J

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.

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

Thanks!

5 个评论
显示 3更早的评论隐藏 3更早的评论

AP 2021-6-11

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 2021-6-11

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 2021-6-12

1
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/853345-fitting-cone-cylinder-of-known-dimensions-to-3d-surface-data#answer_723440

编辑：Matt J 2021-6-13

在 MATLAB Online 中打开

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

12 个评论
显示 10更早的评论隐藏 10更早的评论

Matt J 2021-6-14

编辑：Matt J 2021-6-15

在 MATLAB Online 中打开

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

Matt J 2021-6-15

编辑：Matt J 2021-6-15

在 MATLAB Online 中打开

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

Matt J 2021-6-15

编辑：Matt J 2021-6-15

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

AP 2021-6-15

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 2021-6-14

1
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/853345-fitting-cone-cylinder-of-known-dimensions-to-3d-surface-data#answer_724630

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

Answer 3

Matt J 2021-6-11

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/853345-fitting-cone-cylinder-of-known-dimensions-to-3d-surface-data#answer_722870

编辑：Matt J 2021-6-11

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.