点云拟合球体

球体"/>

点云拟合球体

前言：在很多情况下，需要根据点云来拟合球体，本博文主要介绍各种方法的拟合情况及优缺点，希望对各位小伙伴有所帮助！

如果我的分享对你有帮助，记得点赞+关注，鼓励我以下吧！

我将收获到的：

        1.使用vtk中的接口vtkFitImplicitFunction进行球拟合算法

        2.四点求解球

        3.使用最小二乘法Matlab算法拟合球，输入为四个或以上的点（推荐）

目录

1. vtkFitImplicitFunction进行球拟合

2. 四点求解球

3. 最小二乘法Matlab求解球拟合

 4. 最小二乘法Matlab求解球拟合（带约束）

1. vtkFitImplicitFunction进行球拟合

缺点：需要输入待拟合球的半径和结构，即在已知最优解的情况下求解。

vtkMath::RandomSeed(4355412); // for test result consistencydouble radius = 1.0;
vtkSmartPointer<vtkBoundedPointSource> points =vtkSmartPointer<vtkBoundedPointSource>::New();
points->SetNumberOfPoints(1000000);
points->SetBounds(-2.0, 2.0, -2.0, 2.0, -2.0, 2.0);
points->Update();vtkSmartPointer<vtkSphere> sphere =vtkSmartPointer<vtkSphere>::New();
sphere->SetRadius(radius*2);vtkSmartPointer<vtkFitImplicitFunction> fit =vtkSmartPointer<vtkFitImplicitFunction>::New();
fit->SetInputConnection(points->GetOutputPort());
fit->SetImplicitFunction(sphere);
fit->SetThreshold(.01);
fit->Update();
std::cout << fit->GetOutput()->GetNumberOfPoints() << " out of "<< points->GetNumberOfPoints() << " points are within "<< fit->GetThreshold() << " of the implicit function" << std::endl;vtkSmartPointer<vtkSphereSource> sphereSource =vtkSmartPointer<vtkSphereSource>::New();
sphereSource->SetRadius(radius * .05);vtkSmartPointer<vtkGlyph3D> glyph3D =vtkSmartPointer<vtkGlyph3D>::New();
glyph3D->SetInputConnection(fit->GetOutputPort());
glyph3D->SetSourceConnection(sphereSource->GetOutputPort());
glyph3D->ScalingOff();
glyph3D->Update();vtkSmartPointer<vtkPolyDataMapper> glyph3DMapper =vtkSmartPointer<vtkPolyDataMapper>::New();
glyph3DMapper->SetInputConnection(glyph3D->GetOutputPort());vtkSmartPointer<vtkActor> glyph3DActor =vtkSmartPointer<vtkActor>::New();
glyph3DActor->SetMapper(glyph3DMapper);
glyph3DActor->GetProperty()->SetColor(0.8900, 0.8100, 0.3400);m_viewer->renderWindow()->GetRenderers()->GetFirstRenderer()->AddActor(glyph3DActor);
m_viewer->renderWindow()->Render();

2. 四点求解球

缺点：要求输入的四点为精确球上的点；否则计算错误。

void fitSphere(vtkPoints* points, double center[3], double& radius) {double p1[3];double p2[3];double p3[3];double p4[3];points->GetPoint(0, p1);points->GetPoint(1, p2);points->GetPoint(2, p3);points->GetPoint(3, p4);double a = p1[0] - p2[0], b = p1[1] - p2[1], c = p1[2] - p2[2];double a1 = p3[0] - p4[0], b1 = p3[1] - p4[1], c1 = p3[2] - p3[2];double a2 = p2[0] - p3[0], b2 = p2[1] - p3[1], c2 = p2[2] - p3[2];double D = a * b1 * c2 + a2 * b * c1 + c * a1 * b2 - (a2 * b1 * c + a1 * b * c2 + a * b2 * c1);if (D == 0){return;}double A = p1[0] * p1[0] - p2[0] * p2[0];double B = p1[1] * p1[1] - p2[1] * p2[1];double C = p1[2] * p1[2] - p2[2] * p2[2];double A1 = p3[0] * p3[0] - p4[0] * p4[0];double B1 = p3[1] * p3[1] - p4[1] * p4[1];double C1 = p3[2] * p3[2] - p4[2] * p4[2];double A2 = p2[0] * p2[0] - p3[0] * p3[0];double B2 = p2[1] * p2[1] - p3[1] * p3[1];double C2 = p2[2] * p2[2] - p3[2] * p3[2];double P = (A + B + C) / 2;double Q = (A1 + B1 + C1) / 2;double R = (A2 + B2 + C2) / 2;double Dx = P * b1 * c2 + b * c1 * R + c * Q * b2 - (c * b1 * R + P * c1 * b2 + Q * b * c2);double Dy = a * Q * c2 + P * c1 * a2 + c * a1 * R - (c * Q * a2 + a * c1 * R + c2 * P * a1);double Dz = a * b1 * R + b * Q * a2 + P * a1 * b2 - (a2 * b1 * P + a * Q * b2 + R * b * a1);center[0] = Dx / D;center[1] = Dy / D;center[2] = Dz / D;radius = sqrt((p1[0] - center[0]) * (p1[0] - center[0]) +(p1[1] - center[1]) * (p1[1] - center[1]) +(p1[2] - center[2]) * (p1[2] - center[2]));}

3. 最小二乘法Matlab求解球拟合

优点：对四个或以上的点进行球拟合，拟合精度高 

参考链接：利用最小二乘法拟合空间圆（球） - 知乎 (zhihu)

clear all
%确认圆心和半径
o = [13.85 23.45 8.94];
R = mean(o);
%获得一系列测量点
P = [];
nP = [];
for i = 1:100vec = [randn(1,1) randn(1,1) randn(1,1)];vec = vec/norm(vec) + 0.01*[randn(1,1) randn(1,1) randn(1,1)];P   = [ P; o+R*vec; ];nP  = [nP;norm(P(i,:)-o)];
end%由P生成A（包括Ax，Ay，Az，Ad）和ex = P(:,1);y = P(:,2);z = P(:,3);
Ax = -2*x;
Ay = -2*y;
Az = -2*z;
Ad =  0*P(:,1) + 1;A = [ Ax Ay Az Ad];e = -x.^2 -y.^2 -z.^2;
%求解abcd, Ax=eX = inv(A'*A)*A'*e;a = X(1)b = X(2)c = X(3)r2 = a^2 + b^2 + c^2 - X(4)r  = r2^0.5
%验证结果
err = [a b c r] - [ o R] %结果接近0，说明结果正确
%绘图验证
hold on
plot3(P(:,1),P(:,2),P(:,3),'.')
plot3(o(1),o(2),o(3),'o')
plot3(a,b,c,'*')

 4. 最小二乘法Matlab求解球拟合（带约束）

优点：对四个或以上的点进行球拟合，拟合精度高 

参考链接：最小二乘法球面拟合(附完整代码)_最小二乘法拟合球面_Marc Pony的博客-CSDN博客

function [ center, r, fittingError ] = sphere_fitting( points, pointCount, crossStartAndEndPointFlag )
%{
Function: sphere_fitting
Description: 球面拟合
Input: 三维点points, 点个数pointCount, 是否经过起点/终点标志位crossStartAndEndPointFlag
Output: 球心center, 半径r, 拟合误差fittingError
Author: Marc Pony(marc_pony@163)
球面方程:(x - a)^2 + (y - b)^2  + (z - c)^2 = r^2  ->  -2*x*a - 2*y*b - 2*z*c +1*(a^2 + b^2 + c^2 - r^2) = -x^2 - y^2 - z^2
%}
if crossStartAndEndPointFlag == 0x = points(:, 1);y = points(:, 2);z = points(:, 3);A = [-2 * x, -2 * y, -2 * z, ones(size(x))];B = -x.^2 - y.^2 - z.^2;temp = A \ B;a = temp(1);b = temp(2);c = temp(3);d = temp(4);
elsex0 = points(1, 1);y0 = points(1, 2);z0 = points(1, 3);xn = points(pointCount, 1);yn = points(pointCount, 2);zn = points(pointCount, 3);x = points(2 : pointCount - 1, 1);y = points(2 : pointCount - 1, 2);z = points(2 : pointCount - 1, 3);EPS = 1.0e-4;if abs(x0 - xn) < EPS && abs(y0 - yn) < EPS && abs(z0 - zn) < EPS %起点与终点重合A = [2 * (x0 - x), 2 * (y0 - y), 2 * (z0 - z)];B = x0^2 + y0^2 + z0^2 - x.^2 - y.^2 - z.^2 ;temp = A \ B;a = temp(1);b = temp(2);c = temp(3);d = -x0^2 - y0^2 - z0^2 + 2.0 * a * x0 + 2.0 * b * y0 + 2.0 * c * z0;elseif abs(x0 - xn) > EPSA = [2 * x * (y0 - yn) / (x0 - xn) - 2 * y - 2 * (xn * y0 - x0 * yn) / (x0 - xn), 2 * x * (z0 - zn) / (x0 - xn) - 2 * z - 2 * (xn * z0 - x0 * zn) / (x0 - xn)];B = -x.^2 - y.^2 - z.^2 - x * (xn^2 + yn^2 + zn^2 - x0^2 - y0^2 - z0^2) / (x0 - xn) + (x0 * (xn^2 + yn^2 + zn^2) - xn * (x0^2 + y0^2 + z0^2)) / (x0 - xn);temp = A \ B;b = temp(1);c = temp(2);P = -x0^2 - y0^2 - z0^2 + 2 * y0 * b + 2 * z0 * c;Q = -xn^2 - yn^2 - zn^2 + 2 * yn * b + 2 * zn * c;a = -(P - Q) / (2 * (x0 - xn));d = -(P * xn - Q * x0) / (x0 - xn);elseif abs(y0 - yn) > EPSA = [2 * y * (x0 - xn) / (y0 - yn) - 2 * x - 2 * (yn * x0 - y0 * xn) / (y0 - yn), 2 * y * (z0 - zn) / (y0 - yn) - 2 * z - 2 * (yn * z0 - y0 * zn) / (y0 - yn)];B = -y.^2 - x.^2 - z.^2 - y * (yn^2 + xn^2 + zn^2 - y0^2 - x0^2 - z0^2) / (y0 - yn) + (y0 * (yn^2 + xn^2 + zn^2) - yn * (y0^2 + x0^2 + z0^2)) / (y0 - yn);temp = A \ B;a = temp(1);c = temp(2);P = -y0^2 - x0^2 - z0^2 + 2 * x0 * a + 2 * z0 * c;Q = -yn^2 - xn^2 - zn^2 + 2 * xn * a + 2 * zn * c;b = -(P - Q) / (2 * (y0 - yn));d = -(P * yn - Q * y0) / (y0 - yn);elseA = [2 * z * (y0 - yn) / (z0 - zn) - 2 * y - 2 * (zn * y0 - z0 * yn) / (z0 - zn), 2 * z * (x0 - xn) / (z0 - zn) - 2 * x - 2 * (zn * x0 - z0 * xn) / (z0 - zn)];B = -z.^2 - y.^2 - x.^2 - z * (zn^2 + yn^2 + xn^2 - z0^2 - y0^2 - x0^2) / (z0 - zn) + (z0 * (zn^2 + yn^2 + xn^2) - zn * (z0^2 + y0^2 + x0^2)) / (z0 - zn);temp = A \ B;b = temp(1);a = temp(2);P = -z0^2 - y0^2 - x0^2 + 2 * y0 * b + 2 * x0 * a;Q = -zn^2 - yn^2 - xn^2 + 2 * yn * b + 2 * xn * a;c = -(P - Q) / (2 * (z0 - zn));d = -(P * zn - Q * z0) / (z0 - zn);endend
endr = sqrt(a^2 + b^2 + c^2 - d);
center(1) = a;
center(2) = b;
center(3) = c;
fittingError = sqrt((points(:, 1) - center(1)).^2 + (points(:, 2) - center(2)).^2+ (points(:, 3) - center(3)).^2) - r;end

测试代码

clc
clear
close all%% 生成带误差的三维点
err = 0.1;
pointCount = 100;
R = 50;
theta = 2*pi*rand(pointCount, 1);
phi = pi*rand(pointCount, 1);
deltaR = -err + 2 * err * rand(pointCount, 1);
x = (R + deltaR) .* sin(phi) .* cos(theta);
y = (R + deltaR) .* sin(phi) .* sin(theta);
z = (R + deltaR) .* cos(phi);
plot3(x, y, z, 'ro')
hold on%% 球面最小二乘拟合
crossStartAndEndPointFlag = 0; %0:不经过给定起点与终点;  1:精确经过给定起点与终点
[ center, r, fittingError ] = sphere_fitting( [x, y, z], pointCount, crossStartAndEndPointFlag )
maxFittingError = max(abs(fittingError))
[xx, yy, zz] = sphere(50);
xx = r * xx + center(1);
yy = r * yy + center(2);
zz = r * zz + center(3);
h = surf(xx, yy, zz);
set(h, 'FaceAlpha', 0.2, 'MeshStyle', 'none')
axis equal

 小结：测试下来，采用最小二乘法拟合的精度较高

C++的最小二乘法拟合代码请详见：球拟合+最小二乘法+三维建模或点云重建资源-CSDN文库

也可以联系小易获取哟！

如果我的分享对你有帮助，记得点赞+关注，鼓励我以下吧！