FITCIRCLE3D Fit a 3D circle to a set of points. [FITTEDCIRCLE, CIRCLENORMAL] = fitCircle3d(PTS) Example % points on a 2d circle with noise nop = randi([5 50],1,1); radius = randi([5 25],1,1); points2d = circleToPolygon([0 0 radius], nop); points2d(1,:) = []; points2d = points2d + rand(size(points2d)); points2d(:,3)=rand(length(nop),1); % apply random rotation and translation [theta, phi] = randomAngle3d; theta = rad2deg(theta); phi = rad2deg(phi); tfm = eulerAnglesToRotation3d(phi, theta, 0); trans = randi([-250 250],3,1); tfm(1:3,4)=trans; points3d = awgn(transformPoint3d(points2d, tfm),1); % fit 3d circle [fittedCircle, circleNormal] = fitCircle3d(points3d); % plot 3d points and 3d circle figure('Color','w'); hold on; axis equal tight; view(3); xlabel('X');ylabel('Y');zlabel('Z'); drawPoint3d(points3d) drawCircle3d(fittedCircle, 'k') drawVector3d(fittedCircle(1:3), circleNormal*fittedCircle(4)) See also circle3dOrigin, circle3dPosition, circle3dPoint, intersectPlaneSphere drawCircle3d, drawCircleArc3d, drawEllipse3d ------ Authors: oqilipo, David Legland created: 2017-05-09
0001 function [fittedCircle, circleNormal] = fitCircle3d(pts) 0002 %FITCIRCLE3D Fit a 3D circle to a set of points. 0003 % 0004 % [FITTEDCIRCLE, CIRCLENORMAL] = fitCircle3d(PTS) 0005 % 0006 % Example 0007 % % points on a 2d circle with noise 0008 % nop = randi([5 50],1,1); 0009 % radius = randi([5 25],1,1); 0010 % points2d = circleToPolygon([0 0 radius], nop); 0011 % points2d(1,:) = []; 0012 % points2d = points2d + rand(size(points2d)); 0013 % points2d(:,3)=rand(length(nop),1); 0014 % % apply random rotation and translation 0015 % [theta, phi] = randomAngle3d; 0016 % theta = rad2deg(theta); 0017 % phi = rad2deg(phi); 0018 % tfm = eulerAnglesToRotation3d(phi, theta, 0); 0019 % trans = randi([-250 250],3,1); 0020 % tfm(1:3,4)=trans; 0021 % points3d = awgn(transformPoint3d(points2d, tfm),1); 0022 % % fit 3d circle 0023 % [fittedCircle, circleNormal] = fitCircle3d(points3d); 0024 % % plot 3d points and 3d circle 0025 % figure('Color','w'); hold on; axis equal tight; view(3); 0026 % xlabel('X');ylabel('Y');zlabel('Z'); 0027 % drawPoint3d(points3d) 0028 % drawCircle3d(fittedCircle, 'k') 0029 % drawVector3d(fittedCircle(1:3), circleNormal*fittedCircle(4)) 0030 % 0031 % See also 0032 % circle3dOrigin, circle3dPosition, circle3dPoint, intersectPlaneSphere 0033 % drawCircle3d, drawCircleArc3d, drawEllipse3d 0034 % 0035 % ------ 0036 % Authors: oqilipo, David Legland 0037 % created: 2017-05-09 0038 0039 % Mean of all points 0040 meanPoint = mean(pts,1); 0041 0042 % Center points by subtracting the meanPoint 0043 centeredPoints = pts - repmat(meanPoint,size(pts,1),1); 0044 0045 % Project 3D data to a plane 0046 [~,~,V]=svd(centeredPoints); 0047 tfmPoints = transformPoint3d(centeredPoints, V'); 0048 0049 % Fit a circle to the points in the xy-plane 0050 circleParamter = CircleFitByTaubin(tfmPoints(:,1:2)); 0051 center2d = circleParamter(1:2); 0052 radius=circleParamter(3); 0053 center3d = transformPoint3d([center2d, 0], [inv(V'), meanPoint']); 0054 circleNormal = V(:,3)'; 0055 [theta, phi, ~] = cart2sph2(circleNormal); 0056 fittedCircle = [center3d radius rad2deg(theta) rad2deg(phi) 0]; 0057 0058 end 0059 0060 % Circle Fit (Taubin method) 0061 % version 1.0 (2.24 KB) by Nikolai Chernov 0062 % http://www.mathworks.com/matlabcentral/fileexchange/22678 0063 function Par = CircleFitByTaubin(XY) 0064 0065 %-------------------------------------------------------------------------- 0066 % 0067 % Circle fit by Taubin 0068 % G. Taubin, "Estimation Of Planar Curves, Surfaces And Nonplanar 0069 % Space Curves Defined By Implicit Equations, With 0070 % Applications To Edge And Range Image Segmentation", 0071 % IEEE Trans. PAMI, Vol. 13, pages 1115-1138, (1991) 0072 % 0073 % Input: XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2) 0074 % 0075 % Output: Par = [a b R] is the fitting circle: 0076 % center (a,b) and radius R 0077 % 0078 % Note: this fit does not use built-in matrix functions (except "mean"), 0079 % so it can be easily programmed in any programming language 0080 % 0081 %-------------------------------------------------------------------------- 0082 0083 n = size(XY,1); % number of data points 0084 0085 centroid = mean(XY); % the centroid of the data set 0086 0087 % computing moments (note: all moments will be normed, i.e. divided by n) 0088 0089 Mxx = 0; Myy = 0; Mxy = 0; Mxz = 0; Myz = 0; Mzz = 0; 0090 0091 for i=1:n 0092 Xi = XY(i,1) - centroid(1); % centering data 0093 Yi = XY(i,2) - centroid(2); % centering data 0094 Zi = Xi*Xi + Yi*Yi; 0095 Mxy = Mxy + Xi*Yi; 0096 Mxx = Mxx + Xi*Xi; 0097 Myy = Myy + Yi*Yi; 0098 Mxz = Mxz + Xi*Zi; 0099 Myz = Myz + Yi*Zi; 0100 Mzz = Mzz + Zi*Zi; 0101 end 0102 0103 Mxx = Mxx/n; 0104 Myy = Myy/n; 0105 Mxy = Mxy/n; 0106 Mxz = Mxz/n; 0107 Myz = Myz/n; 0108 Mzz = Mzz/n; 0109 0110 % computing the coefficients of the characteristic polynomial 0111 0112 Mz = Mxx + Myy; 0113 Cov_xy = Mxx*Myy - Mxy*Mxy; 0114 A3 = 4*Mz; 0115 A2 = -3*Mz*Mz - Mzz; 0116 A1 = Mzz*Mz + 4*Cov_xy*Mz - Mxz*Mxz - Myz*Myz - Mz*Mz*Mz; 0117 A0 = Mxz*Mxz*Myy + Myz*Myz*Mxx - Mzz*Cov_xy - 2*Mxz*Myz*Mxy + Mz*Mz*Cov_xy; 0118 A22 = A2 + A2; 0119 A33 = A3 + A3 + A3; 0120 0121 xnew = 0; 0122 ynew = 1e+20; 0123 epsilon = 1e-12; 0124 IterMax = 20; 0125 0126 % Newton's method starting at x=0 0127 0128 for iter=1:IterMax 0129 yold = ynew; 0130 ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3)); 0131 if abs(ynew) > abs(yold) 0132 disp('Newton-Taubin goes wrong direction: |ynew| > |yold|'); 0133 xnew = 0; 0134 break; 0135 end 0136 Dy = A1 + xnew*(A22 + xnew*A33); 0137 xold = xnew; 0138 xnew = xold - ynew/Dy; 0139 if (abs((xnew-xold)/xnew) < epsilon), break, end 0140 if (iter >= IterMax) 0141 disp('Newton-Taubin will not converge'); 0142 xnew = 0; 0143 end 0144 if (xnew<0.) 0145 fprintf(1,'Newton-Taubin negative root: x=%f\n',xnew); 0146 xnew = 0; 0147 end 0148 end 0149 0150 % computing the circle parameters 0151 0152 DET = xnew*xnew - xnew*Mz + Cov_xy; 0153 Center = [Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy]/DET/2; 0154 0155 Par = [Center+centroid , sqrt(Center*Center'+Mz)]; 0156 0157 end % CircleFitByTaubin 0158