PCL——B样条曲线曲面拟合_b样条曲面拟合

B样条曲线
样条曲线，是B-样条基函数的线性组合，是贝塞尔曲线的一般化。
给定n+1个控制点，P0,P1, …, Pn以及一个节点向量U = { u0,u1, …, um }, p 次B-样条曲线由这些控制点和节点向量U 定义，设Ni,p(u)是第i个 p次B-样条基函数，则p 次B-样条曲线的公式为

设P0、P02、P2是一条抛物线上顺序三个不同的点。过P0和P2点的两切线交于P1点，在P02点的切线交P0P1和P2P1于P01和P11


引入比值为参数t则

将前两个式子代入到第三个式子得到：

二次Bezier曲线P02可以定义为分别由前两个顶点(P0,P1)和后两个顶点(P1,P2)决定的一次Bezier曲线的线性组合
由此递推到n次Bezier曲线，可得到递推式：

什么是节点向量？
设U 是m + 1个非递减数的集合，u0 <=u1 <= u2 <= … <= um。ui称为节点（knots）, 集合U 称为节点向量（knot vector）
因为可能存在一个节点出现多次，也叫多重节点。所以节点向量的维度不等于控制点的个数。

B-样条基函数
基函数的次数p，第i个p次B-样条基函数，写为Ni,p(u)，递归定义如下：

此处规定了0/0=0。 式中p表示样条的幂次,下标i表示B样条的序号。该递推公式表明,任意p次样条可由两个相邻的p-1次样条的线性组合构成。计算Ni,p(u)，可以依据三角计算格式

B样条曲线曲面重建的原理
基于B样条曲线的曲面重建实质上就是根据控制点进行B样条曲面拟合。B 样条曲面是由多条 B 样条曲线在 u、v 两个方向上多次构建形成的，由（m+1)×（n+1）个控制点构成一张控制网格，两个方向的参数节点矢量分别为U=[u0,u1…um+k+1]和V=[v0,v1…vm+l+1]。B样条曲面的方程的定义如下

Pi,j是控制点集Pi,j（i=0,1…m;j=0,1…n），也就是三维点云中的点集（局部）。Ni,k(u)和Nj,l(v)是B样条曲面基函数。

拟合的过程很简单，拟合后求曲面方程系数的步骤：
1)固定 j，对于Pi,j（i=0,1…m;j=0,1…n）沿u 方向分别求出 n+1条参数曲线的控制顶点。
2)固定 i，对于Pi,j（i=0,1…m;j=0,1…n）沿v 方向分别求出 m+1条参数曲线的控制顶点。
PCL中基于B样条曲线的曲面重建
算法流程
1，使用主成分分析(PCA)初始化B样条曲面。这假设点云有两个主要的方向，即它大致是平面的。
2，对初始化后的B样条曲面进行拟合和迭代优化。
3，用圆来对B样条曲线初始化。这里我们假设点云是紧凑的，即没有分离的聚类。
4，对初始化后的B样条曲线进行拟合。
5，对拟合得到的B样条曲面进行三角化，得到最终的曲面模型。
PCL实现
// parameters
unsigned order (3);
unsigned refinement (5);
unsigned iterations (10);
unsigned mesh_resolution (256);

pcl::on_nurbs::FittingSurface::Parameter params;
params.interior_smoothness = 0.2;
params.interior_weight = 1.0;
params.boundary_smoothness = 0.2;
params.boundary_weight = 0.0;
Order ——B样条曲面的多项式阶
Refinemen——是求精迭代的次数，其中每插入一个迭代控制点，b样条曲面的每个参数方向上的控制点大约翻倍。
Iterations——是优化完成后执行的迭代数量。
mesh_resolutio——每个参数方向上的顶点数，用于b样条曲面的三角剖分。
interior_smoothness——内部表面的光滑度
interior_weight——用于表面内部优化的权重。
boundary_smoothness——表面边界的平滑度
boundary_weight——表面边界优化的权重
B样条拟合曲面的相关参数解读：
pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
curve_params.addCPsAccuracy = 5e-2;//曲线的支持区域到最近数据点的距离必须低于该值，否则将插入控制点。
curve_params.addCPsIteration = 3;//内部迭代没有插入控制点。
curve_params.maxCPs = 200;//控制点的最大总数
curve_params.accuracy = 1e-3;//曲线的拟合精度
curve_params.iterations = 100;//迭代次数

curve_params.param.closest_point_resolution = 0;//每个支持区域内的控制点数量
curve_params.param.closest_point_weight = 1.0;//曲线拟合到最近点的权值
curve_params.param.closest_point_sigma2 = 0.1;//最近点的阈值
curve_params.param.interior_sigma2 = 0.00001;//内点的阈值
curve_params.param.smooth_concavity = 1.0;//导致曲线向内弯曲的值(0 =不弯曲;< 0向内弯曲;> 0向外弯曲)
curve_params.param.smoothness = 1.0;//平滑项的权值
拟合B样条曲面结果：


附上代码：

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>
#include <pcl/console/parse.h>
using namespace pcl::console;
typedef pcl::PointXYZ Point;

void
PointCloud2Vector3d(pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data);

void
visualizeCurve(ON_NurbsCurve &curve,
	ON_NurbsSurface &surface,
	pcl::visualization::PCLVisualizer &viewer);

int
main(int argc, char *argv[])
{
	std::string pcd_file, file_3dm;

	if (argc < 2)
	{
		printf("\nUsage: pcl_example_nurbs_fitting_surface pcd<PointXYZ>-in-file -o 3 -rn 4 -in 10 -mr 128 -td 1\n\n");
		exit(0);
	}
	pcd_file = argv[1];
	//file_3dm = argv[2];

	pcl::visualization::PCLVisualizer viewer("点云库PCL学习教程第二版-B样条曲面拟合点云数据");
	viewer.setBackgroundColor(255, 255, 255);
	viewer.setSize(800, 600);

	// ############################################################################
	// load point cloud

	printf("  loading %s\n", pcd_file.c_str());
	pcl::PointCloud<Point>::Ptr cloud(new pcl::PointCloud<Point>);
	pcl::PCLPointCloud2 cloud2;
	pcl::on_nurbs::NurbsDataSurface data;

	if (pcl::io::loadPCDFile(pcd_file, cloud2) == -1)
		throw std::runtime_error("  PCD file not found.");

	fromPCLPointCloud2(cloud2, *cloud);
	PointCloud2Vector3d(cloud, data.interior);
	pcl::visualization::PointCloudColorHandlerCustom<Point> handler(cloud, 0, 255, 0);
	viewer.addPointCloud<Point>(cloud, handler, "cloud_cylinder");
	printf("  %lu points in data set\n", cloud->size());

	// ############################################################################
	// fit B-spline surface

	// parameters
	unsigned order(3);
	unsigned refinement(4);
	unsigned iterations(10);
	unsigned mesh_resolution(128);
	bool two_dim = true;
	parse_argument(argc, argv, "-o", order);
	parse_argument(argc, argv, "-rn", refinement);
	parse_argument(argc, argv, "-in", iterations);
	parse_argument(argc, argv, "-mr", mesh_resolution);
	parse_argument(argc, argv, "-td", two_dim);
	pcl::on_nurbs::FittingSurface::Parameter params;
	params.interior_smoothness = 0.2;
	params.interior_weight = 1.0;
	params.boundary_smoothness = 0.2;
	params.boundary_weight = 0.0;

	// initialize
	printf("  surface fitting ...\n");
	ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox(order, &data);
	pcl::on_nurbs::FittingSurface fit(&data, nurbs);
	//  fit.setQuiet (false); // enable/disable debug output

	// mesh for visualization
	pcl::PolygonMesh mesh;
	pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	std::vector<pcl::Vertices> mesh_vertices;
	std::string mesh_id = "mesh_nurbs";
	pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh(fit.m_nurbs, mesh, mesh_resolution);
	viewer.addPolygonMesh(mesh, mesh_id);
	std::cout << "Before refine" << endl;
	viewer.spinOnce(3000);
	// surface refinement
	for (unsigned i = 0; i < refinement; i++)
	{
		fit.refine(0);
		if (two_dim)fit.refine(1);
		fit.assemble(params);
		fit.solve();
		pcl::on_nurbs::Triangulation::convertSurface2Vertices(fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
		viewer.updatePolygonMesh<pcl::PointXYZ>(mesh_cloud, mesh_vertices, mesh_id);
		viewer.spinOnce(3000);
		std::cout << "refine: " << i << endl;
	}
	// surface fitting with final refinement level
	for (unsigned i = 0; i < iterations; i++)
	{
		fit.assemble(params);
		fit.solve();
		pcl::on_nurbs::Triangulation::convertSurface2Vertices(fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
		viewer.updatePolygonMesh<pcl::PointXYZ>(mesh_cloud, mesh_vertices, mesh_id);
		viewer.spinOnce(3000);
		std::cout << "iterations: " << i << endl;
	}
	// ############################################################################
	// fit B-spline curve

	// parameters
	pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
	curve_params.addCPsAccuracy = 5e-2;
	curve_params.addCPsIteration = 3;
	curve_params.maxCPs = 200;
	curve_params.accuracy = 1;
	curve_params.iterations = 100;

	curve_params.param.closest_point_resolution = 0;
	curve_params.param.closest_point_weight = 1.0;
	curve_params.param.closest_point_sigma2 = 0.1;
	curve_params.param.interior_sigma2 = 0.00001;
	curve_params.param.smooth_concavity = 1.0;
	curve_params.param.smoothness = 1.0;

	// initialisation (circular)
	printf("  curve fitting ...\n");
	pcl::on_nurbs::NurbsDataCurve2d curve_data;
	curve_data.interior = data.interior_param;
	curve_data.interior_weight_function.push_back(true);
	ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D(order, curve_data.interior);
	// curve fitting
	pcl::on_nurbs::FittingCurve2dASDM curve_fit(&curve_data, curve_nurbs);
	// curve_fit.setQuiet (false); // enable/disable debug output
	curve_fit.fitting(curve_params);
	visualizeCurve(curve_fit.m_nurbs, fit.m_nurbs, viewer);

	// ############################################################################
	// triangulation of trimmed surface

	printf("  triangulate trimmed surface ...\n");
	viewer.removePolygonMesh(mesh_id);
	pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh(fit.m_nurbs, curve_fit.m_nurbs, mesh,
		mesh_resolution);
	viewer.addPolygonMesh(mesh, mesh_id);


	// save trimmed B-spline surface
	/*if ( fit.m_nurbs.IsValid() )
	{
	  ONX_Model model;
	  ONX_Model_Object& surf = model.m_object_table.AppendNew();
	  surf.m_object = new ON_NurbsSurface(fit.m_nurbs);
	  surf.m_bDeleteObject = true;
	  surf.m_attributes.m_layer_index = 1;
	  surf.m_attributes.m_name = "surface";
	  ONX_Model_Object& curv = model.m_object_table.AppendNew();
	  curv.m_object = new ON_NurbsCurve(curve_fit.m_nurbs);
	  curv.m_bDeleteObject = true;
	  curv.m_attributes.m_layer_index = 2;
	  curv.m_attributes.m_name = "trimming curve";
	  model.Write(file_3dm.c_str());
	  printf("  model saved: %s\n", file_3dm.c_str());
	}*/

	printf("  ... done.\n");

	viewer.spin();
	return 0;
}

void
PointCloud2Vector3d(pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
{
	for (unsigned i = 0; i < cloud->size(); i++)
	{
		Point &p = cloud->at(i);
		if (!pcl_isnan(p.x) && !pcl_isnan(p.y) && !pcl_isnan(p.z))
			data.push_back(Eigen::Vector3d(p.x, p.y, p.z));
	}
}

void
visualizeCurve(ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::on_nurbs::Triangulation::convertCurve2PointCloud(curve, surface, curve_cloud, 4);
	for (std::size_t i = 0; i < curve_cloud->size() - 1; i++)
	{
		pcl::PointXYZRGB &p1 = curve_cloud->at(i);
		pcl::PointXYZRGB &p2 = curve_cloud->at(i + 1);
		std::ostringstream os;
		os << "line" << i;
		viewer.removeShape(os.str());
		viewer.addLine<pcl::PointXYZRGB>(p1, p2, 1.0, 0.0, 0.0, os.str());
	}

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps(new pcl::PointCloud<pcl::PointXYZRGB>);
	for (int i = 0; i < curve.CVCount(); i++)
	{
		ON_3dPoint p1;
		curve.GetCV(i, p1);

		double pnt[3];
		surface.Evaluate(p1.x, p1.y, 0, 3, pnt);
		pcl::PointXYZRGB p2;
		p2.x = float(pnt[0]);
		p2.y = float(pnt[1]);
		p2.z = float(pnt[2]);

		p2.r = 255;
		p2.g = 0;
		p2.b = 0;

		curve_cps->push_back(p2);
	}
	viewer.removePointCloud("cloud_cps");
	viewer.addPointCloud(curve_cps, "cloud_cps");
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224

命令参数：C:\\Users\\Administrator\\Desktop\\ConsoleApplication1\\13.pcd -rn 4 -in 10