点云项目算法（1）——拟合空间的圆

由于项目需要：拟合构件的圆孔

老师说:选取孔洞周围点进行拟合圆，首先将孔所在面提取出来，然后再在该面取孔位处的点进行圆拟合。

参考相关论文之后，我的想法有两个：
1、只针对这个零件，圆孔的大小固定（有设计图），只是圆心的位置不确定。那么首先把孔洞的部分的区域点云提取出来，对点云进行投影到二维，用一个圆去判断怎样范围的点数最少就是圆孔位置
2、老师点选了一些点云，用点选出来的点云拟合圆形，可以用 PCL随机一致性RANSAC算法拟合圆形。

 
clear all;
clc;
close all;

 
%%%   含误差空间圆拟合点  %%%
M=[0.306400001049,7.283800125122,-0.833899974823
0.303600013256,7.299799919128,-0.830200016499
0.330099999905,7.292799949646,-0.831799983978
0.312299996614,7.306300163269,-0.830500006676
0.317299991846,7.278500080109,-0.838299989700
0.325800001621,7.303400039673,-0.829900026321
0.326700001955,7.281099796295,-0.831200003624
0.302300006151,7.289100170135,-0.829999983311
0.311199992895,7.279699802399,-0.837000012398
0.319200009108,7.305699825287,-0.829500019550
0.307599991560,7.302400112152,-0.829599976540
0.319700002670,7.279099941254,-0.838800013065

];
 
 
[num dim]=size(M);
 
L1=ones(num,1);
A=inv(M'*M)*M'*L1;       % 求解平面法向量
 
B=zeros((num-1)*num/2,3);
 
count=0;
for i=1:num-1
    for j=i+1:num   
        count=count+1;
        B(count,:)=M(j,:)-M(i,:);
    end    
end
 
L2=zeros((num-1)*num/2,1);
count=0;
for i=1:num-1
    for j=i+1:num
        count=count+1;
        L2(count)=(M(j,1)^2+M(j,2)^2+M(j,3)^2-M(i,1)^2-M(i,2)^2-M(i,3)^2)/2;
    end
end
 
D=zeros(4,4);
D(1:3,1:3)=(B'*B);
D(4,1:3)=A';
D(1:3,4)=A;
 
L3=[B'*L2;1]
 
C=inv(D')*(L3)   % 求解空间圆圆心坐标
 
C=C(1:3);
 
radius=0;
for i=1:num
    tmp=M(i,:)-C';
    radius=radius+sqrt(tmp(1)^2+tmp(2)^2+tmp(3)^2);
end
r=radius/num            %  空间圆拟合半径
 
figure
h1=plot3(M(:,1),M(:,2),M(:,3),'*');
%set(gca,'xlim',[11.4 11.7]);
 
 
%%%%   绘制空间圆  %%%%
n=A;
c=C;
 
theta=(0:2*pi/100:2*pi)';    %  theta角从0到2*pi
a=cross(n,[1 0 0]);          %  n与i叉乘，求取a向量
if ~any(a)                   %  如果a为零向量，将n与j叉乘
    a=cross(n,[0 1 0]);
end
b=cross(n,a);      % 求取b向量
a=a/norm(a);       % 单位化a向量
b=b/norm(b);       % 单位化b向量
 
c1=c(1)*ones(size(theta,1),1);
c2=c(2)*ones(size(theta,1),1);
c3=c(3)*ones(size(theta,1),1);
 
x=c1+r*a(1)*cos(theta)+r*b(1)*sin(theta);  % 圆上各点的x坐标
y=c2+r*a(2)*cos(theta)+r*b(2)*sin(theta);  % 圆上各点的y坐标
z=c3+r*a(3)*cos(theta)+r*b(3)*sin(theta);  % 圆上各点的z坐标
 
hold on;
h2=plot3(x,y,z,'-r');
xlabel('x轴')
ylabel('y轴')
zlabel('z轴')
legend([h1 h2],'控制点','拟合圆');
grid on

参考文献
[1]潘国荣,李怀锋.基于空间向量的空间圆形拟合检测新方法[J].大地测量与地球动力学,2010,30(04):106-108.
[1]李英硕,杨帆,袁兆奎.空间圆形拟合检测新方法[J].测绘科学,2013,38(06):147-148.
[1]田晓,李全海.基于投影点的空间圆形拟合检测新方法[J].工程勘察,2014,42(05):72-74.
https://blog.csdn.net/qq_18895311/article/details/108340276