样条 拟合 python

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");
}

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