前排瞌睡杂物堆

LKH 求存，探路，躬行，旁通，振兴，延续。
第二阶段，启动！

0%

点云三维重建 PolyFit算法解析与改进（四）

发表于 更新于 分类于
本文字数： 11k 阅读时长 ≈ 20 分钟

摘要：
点云法向量估计；区域增长聚类。

法向量估计

上一节中，输入数据经底部填充后可视作完整的建筑物点云。到了这一节，目标是把点云按某种特征聚类分割为各个区域，方便面片模型的构造，从而以更抽象、简化的方法来描述实体。
从整体粗略地看，常规建筑的轮廓是方方正正、横平竖直的，它的表面结构可拆分成若干个不同大小和形状的面状区域，也就是说，在多数情况下可以用单个平面来近似描述建筑的局部表面，用它来表示建筑上一小块区域的朝向。那为了能准确地将建筑物分割为数个呈面状的区域，常见方法是按平面模型来对点云进行多次随机采样一致性或进行区域增长。
点云底部填充

从这一块开始，后面的大部分内容都是对PolyFit算法的照搬，官方的调用示例可以去看下这个页面[14]，它的源码主要封装在compute_confidences和hypothesis这两个文件中。我为了方便调试和理解，把其中用到的摘抄到自己的程序里，这里面熟悉的功能我会捋一下代码逻辑，不太懂的部分我就跳了。
继续顺着上一节的预处理往后讲，接下来要计算点的法向量并分割点云区域。法向量是点的一种特征值，定义上，对穿过点的拟合曲面，它在该点的垂直向量称为点的法向量，由于用作曲面拟合的近邻点数和拟合公式是可变的，法向量并不是一个确定值，所以法向量的计算过程也叫估计点的法向量。

下图是源自[15]的法向量概念图，感觉他讲得挺明白的，法向量的概念可以抽象成对函数在某一点上的微分。
法向量概念图

以前用GPT和这篇知乎回答[16]记下法向量估计的步骤，可参考一二。
法向量估计步骤

代码实现

代码实现在RegionSegProcess的EstimateNormals()。调用CGAL的PCA法向量估计，先定义个std::pair<Kernel::Point_3, Kernel::Vector_3>的二元对象，前者存点坐标后者存法向量。邻域点的个数用_num_kSearch存储，定义在头文件的私有变量那，默认初值是18，感觉估算得太慢/不需要太平缓的值，就【Alt+F12】速览定义把值改小些。
由此得到的法线方向虽然有正有负，不影响后面的聚类。

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
void RegionSegProcess::EstimateNormals(const PointCloud & cloud, PointCloud & cloud_normal)
{
	//1、转格式，cloud -> list_normal
	std::list<PointVectorPair> list_normal;//二元列表_法向量
	for (int i = 0; i < cloud.size(); ++i)
	{
		PointVectorPair pair_temp;
		pair_temp.first = cloud.point(i);
		list_normal.push_back(pair_temp);
	}

	
	//2、法向量计算
	//使用固定邻近点来计算法线，pca_estimate_normals()需要一个范围的点，以及属性映射来访问每个点的位置和法线
	CGAL::pca_estimate_normals<CGAL::Parallel_if_available_tag>(list_normal,
		_num_kSearch,
		CGAL::parameters::point_map(CGAL::First_of_pair_property_map<PointVectorPair>())
		.normal_map(CGAL::Second_of_pair_property_map<PointVectorPair>()));

	///输出测试_list格式的法向量
	//for (const PointVectorPair& pair : list_normal)
	//	std::cout << pair.first << "  " << pair.second << std::endl;


#if 0
	//3、可选操作：法线定向。//后续用不上
	//需要包含#include <CGAL/mst_orient_normals.h>
	//mst_orient_normals()需要一系列点，以及属性映射来访问每个点的位置和法线。
	std::list<PointVectorPair>::iterator unoriented_points_begin =
		CGAL::mst_orient_normals(list_normal, _num_kSearch,
			CGAL::parameters::point_map(CGAL::First_of_pair_property_map<PointVectorPair>())
			.normal_map(CGAL::Second_of_pair_property_map<PointVectorPair>()));
	//删除具有无方向法线的点，计划调用期望定向法线的重建算法
	list_normal.erase(unoriented_points_begin, list_normal.end());
#endif //3、法线定向，后续用不上


	//4、转格式，list_normal -> cloud_normal
	cloud_normal.clear();
	cloud_normal.add_normal_map();
	for (const PointVectorPair& pair : list_normal)
		cloud_normal.insert(pair.first, pair.second);

}

代码实现_常用的法向量估计

再记录一些常用的法向量估计方法：
（1）特征值分解，再取最小特征值的特征向量作为点云的法向量。注意此处的d值为点云质心到坐标原点的距离，返回的Eigen::Vector4f内存储了穿过质心的平面的系数，前三项即为法向量值。Eigen实现。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
Eigen::Vector4f GeometryCal::FittingPlane(const std::vector<Eigen::Vector3f>& points)
{
	//检查输入点有效性
	if (points.size() < 3)
		return Eigen::Vector4f(-999, -999, -999, -999);

	Eigen::MatrixXf matrix(points.size(), 3);
	for (int i = 0; i < points.size(); ++i)
		matrix.row(i) = points[i].transpose();

	Eigen::Vector3f center = matrix.colwise().mean();//计算质心
	Eigen::MatrixXf mat_center = matrix.rowwise() - center.transpose();//点矩阵去中心化
	Eigen::Matrix3f covariance = (mat_center.transpose() * mat_center) / points.size();//协方差矩阵
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);//对协方差矩阵进行特征值分解
	Eigen::Vector3f normal = solver.eigenvectors().col(0);//最小特征值对应的特征向量
	float d = -normal.dot(center);//平面系数

	return Eigen::Vector4f(normal(0), normal(1), normal(2), d);//平面系数
}

（2）PCL调用NormalEstimationOMP所构造的方法。cloud_pcl是类型为pcl::PointXYZ的输入点云，normals为估计的法向量。
需要在顶部引用：

1
2
#include <pcl/search/kdtree.h>//kd树
#include <pcl/features/normal_3d_omp.h>//法向量估计加速
1
2
3
4
5
6
7
8
9
10
//计算法向量
int k = 30;//近邻点数
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud_pcl);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
ne.setSearchMethod(tree);
ne.setKSearch(k);
ne.setNumberOfThreads(10);//设置的多线程数量
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
ne.compute(*normals);

（3）加权最小二乘计算平面系数。先用树结构获取邻近点下标及平方距离，按平面方程构造矩阵方程。比较特别的是里面的权重阵，根据平方距离给一个反距离权重，范围在（0，1]，示例中设置1e-2范围内的邻近点权重固定为1，超过该范围外的点离搜索点越远权重越小。
planes存储搜索点的拟合平面系数，前三项为估计的法向量，第四项为搜索点到全局坐标系原点的距离。

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
#pragma omp parallel for
	for (int i_p = 0; i_p < cloud.size(); ++i_p)
	{
		pcl::PointXYZ p_search;
		p_search.getVector3fMap() = cloud[i_p].point;

		std::vector<int> indices;
		std::vector<float> distances;
		tree.nearestKSearch(p_search, k, indices, distances);

		//构建邻域点数据
		Eigen::MatrixXf X(indices.size(), 3);//平面方程的参数阵
		Eigen::VectorXf Z(indices.size());//高度阵
		Eigen::VectorXf W(indices.size());//权重阵
		for (int i = 0; i < indices.size(); ++i)
		{
			int idx = indices[i];
			const Eigen::Vector3f& p_neighbor = cloud[idx].point;
			float x = p_neighbor.x() - p_search.x;
			float y = p_neighbor.y() - p_search.y;
			float z = p_neighbor.z();//用偏移后的x,y描述邻域点相对于中心点的局部分布特性，不需要减去p_search.z

			//填充平面方程矩阵z = ax + by + c
			X(i, 0) = x;
			X(i, 1) = y;
			X(i, 2) = 1.0f;
			Z(i) = z;

			W(i) = (distances[i] < 1e-4) ? 1.0f : (1e-4 / distances[i]);//按平方距离计算反距离权重
		}

		//加权最小二乘法拟合//权重对角阵、权重修正后的参数阵、目标值
		Eigen::MatrixXf W_diag = W.asDiagonal();
		Eigen::MatrixXf XtWX = X.transpose() * W_diag * X;//Xt * W * X
		Eigen::VectorXf XtWZ = X.transpose() * W_diag * Z;//Xt * W * Z
		//解方程，拟合平面系数[a, b, c, d]
		Eigen::VectorXf coeffs = XtWX.ldlt().solve(XtWZ);
		float d = -(coeffs(0) * p_search.x + coeffs(1) * p_search.y + coeffs(2) * p_search.z);//平面系数//注：求该值不需要法向量归一化
		//if (XtWX.ldlt().info() != Eigen::Success)
		//	std::cout << "奇异矩阵" << std::endl;

		planes[i_p] = Eigen::Vector4f(coeffs(0), coeffs(1), coeffs(2), d);
	}

这是邻域点数为18的法线可视化结果，一般设置的邻近点个数越多，估算的法线朝向越趋向一致，换言之是更容易聚类出大块的区域，但也可能会让原本不同朝向的区域聚合到一起，理论上要按点密度取个合适的值。
法向量可视化

区域聚类

有了法向量数据，开始对点云进行区域增长聚类。PolyFit代码里用的是区域增长方法（论文中用的是RANSAC区域聚类），效果不错我直接照搬。该方法基于种子点逐步扩展的思想，通过迭代合并具有相似属性的邻近信息（如法向量、颜色或强度）来形成连续区域。这里聚类依赖的属性信息就是点的法向量，它的聚类步骤猜测如下：
（1）创建聚类Q。
（2）按某规则选取一个种子点，近邻搜索最近的k个点；求每个近邻点与种子点的法向量夹角，夹角小于阈值的点纳入到聚类中；
（3）将种子点标记为已处理，从聚类中选取未标记点为新的种子点，重复（2）；
（4）聚类中的所有点已被标记，按最小二乘法对点云拟合平面，聚类中点到平面的距离小于阈值的剔除；
（5）若聚类Q的点数小于阈值，移除Q。重复（1）到（4）直至所有点都被标记过。

代码实现

具体的代码实现在RegionSegProcess的RegionGrowing()。区域增长聚类后能得到一个二维容器regions，表示各个聚类区域内点的索引。为了方便可视化显示，给cloud_seg附加颜色和分区序号的属性，通过遍历regions中的索引值从cloud_normal中拷贝相应的点，并赋予同一区域的点相同颜色。CGAL的聚类方法还能导出所有未满足聚类条件的点的索引，我把这一类点return了。

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
PointCloud RegionSegProcess::RegionGrowing(const PointCloud & cloud_normal, PointCloud & cloud_seg)
{
	//1、创建类的实例
	Neighbor_query neighbor_query(
		cloud_normal,
		_num_kSearch,
		cloud_normal.point_map());//k近邻搜索邻域查询的实例
	//Neighbor_query neighbor_query(
	//	cloud_normal,
	//	search_sphere_radius);//半径搜索邻域查询的实例//易报错，弃用
	Region_type region_type(
		cloud_normal,
		_max_distance_point2plane, _max_accepted_angle, _min_region_pointNum,
		cloud_normal.point_map(), cloud_normal.normal_map());//区域类型的实例
	Region_growing region_growing(
		cloud_normal,
		neighbor_query, region_type);//区域增长的实例


	//2、区域增长分割算法
	CGAL::Timer timer;//时间计数的实例
	timer.start();
	std::vector<std::vector<std::size_t>> regions;
	region_growing.detect(std::back_inserter(regions));//算法运行
	timer.stop();
	std::cout << "有 " << regions.size() << " 个分割区域已生成，运行耗时 " << timer.time() << " 秒" << std::endl;


	//3、根据region中的区域索引，拷贝cloud_normal至cloud_seg中，附上随机颜色
	cloud_seg.clear();
	cloud_seg.add_normal_map();//为cloud_seg添加normal属性
	cloud_seg.add_property_map<int>("region_map").first;//为cloud_seg添加区域索引属性
	cloud_seg.add_property_map<unsigned char>("red").first;//为cloud_seg添加color属性
	cloud_seg.add_property_map<unsigned char>("green").first;
	cloud_seg.add_property_map<unsigned char>("blue").first;

	for (int i = 0; i < regions.size(); ++i)
	{
		//设置随机颜色
		const unsigned char r = static_cast<unsigned char>(std::rand() % 256);
		const unsigned char g = static_cast<unsigned char>(std::rand() % 256);
		const unsigned char b = static_cast<unsigned char>(std::rand() % 256);

		for (const auto& index : regions[i])
		{
			PointCloud cloud_piece;
			cloud_piece.add_normal_map();
			const auto& key = *(cloud_normal.begin() + index);//根据regions[i]建立对cloud_normal的索引
			cloud_piece.insert(cloud_normal, key);
			cloud_piece.add_property_map<int>("region_map", i).first;
			cloud_piece.add_property_map<unsigned char>("red", r).first;
			cloud_piece.add_property_map<unsigned char>("green", g).first;
			cloud_piece.add_property_map<unsigned char>("blue", b).first;

			cloud_seg.insert(cloud_piece, *cloud_piece.begin());
		}
	}


	//4、统计未分割的区域
	std::vector<std::size_t> unassigned_items;//因未满足条件而未被分割点的区域
	region_growing.unassigned_items(std::back_inserter(unassigned_items));

	PointCloud cloud_unassigned;
	for (const auto index : unassigned_items)
	{
		const auto& key = *(cloud_normal.begin() + index);//索引
		const Kernel::Point_3& point = get(cloud_normal.point_map(), key);
		cloud_unassigned.insert(point);
	}

	return cloud_unassigned;
}
点云区域聚类

主函数调用

主函数run.cpp中，通过以下代码实现点云的法向量估计、区域聚类。

1
2
3
4
5
6
7
8
9
10
11
12
RegionSegProcess obj_seg;
PointCloud cloud_normal;
//obj_seg.ChangeParameter(18, 2.5, 45, 350);//参数修改
//obj_seg.ChangeParameter(12, 2, 30, 50);//参数修改
obj_seg.EstimateNormals(cloud_input, cloud_normal);//估计点云的法向量
PointCloud cloud_seg;
obj_seg.RegionGrowing(cloud_normal, cloud_seg);//区域生长分割
//弃用，原数据平移后可能因位数问题导致法向量偏移，致使重建面片偏移
//obj_pre.Coordination2Original(cloud_seg);//分割完毕，将数据平移至原始坐标
PointCloud cloud_seg_copy(cloud_seg);
obj_pre.Coordination2Original(cloud_seg_copy);//分割完毕，将数据平移至原始坐标
obj_file.SaveCloud(path_output, obj_file.GetFileName(file_path) + "_region", cloud_seg);

聚类结果受点的法向量、点到拟合平面的最大距离限制、点间的法向量夹角限制、聚类的最小点数这四个参数影响，对它们的设置一并放到RegionSegProcess的ChangeParameter()中进行。
所以最后再说说这一块的参数设置：
num_kSearch的值越小，则越少的邻近点参与到平面拟合中，导致点之间的法向量变化越大，最后聚类区域间的间隙越大，也就是建筑物墙角、屋檐这类的转角处越明显；
max_accepted_angle的效果类似，该值设置得越小，能得到越多细碎的区域。
max_distance_point2plane和min_region_pointNum我认为是去噪参数，前者去除离群点，后者剔除过于细碎的区域。

另外，如果要处理大型建筑物，建议把法线间的最大角度阈值调大，聚类点数最小值调大，从而使聚类结果中只留有大区域点云。

参考文献&引用

[1] https://3d.bk.tudelft.nl/liangliang/
[2] https://3d.bk.tudelft.nl/liangliang/publications/2017/polyfit/polyfit.html
[3] https://www.youtube.com/watch?v=_0brfDFkIkc
[4] Linfu X ,Han H ,Qing Z , et al. Combined Rule-Based and Hypothesis-Based Method for Building Model Reconstruction from Photogrammetric Point Clouds [J]. Remote Sensing, 2021, 13 (6): 1107-1107.
[5] Nan L , Wonka P .PolyFit: Polygonal Surface Reconstruction from Point Clouds[C]//International Conference on Computer Vision.IEEE, 2017.
[6] https://github.com/Kitware/CMake/releases/download/v3.29.3/cmake-3.29.3-windows-x86_64.zip
[7] https://boostorg.jfrog.io/artifactory/main/release/1.85.0/source/boost_1_85_0.7z
[8] https://github.com/CGAL/cgal/releases/download/v5.6.1/CGAL-5.6.1.zip
[9] https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.zip
[10] https://scipopt.org/download.php?fname=SCIPOptSuite-9.0.0-win64-VS15.exe
[11] https://zenodo.org/records/4390295#.Y0eIodJBxuV
[12] https://www.zhihu.com/question/277599635/answer/2149719454
[13] https://zhuanlan.zhihu.com/p/668272208
[14] https://doc.cgal.org/latest/Polygonal_surface_reconstruction/index.html#Chapter_PolygonalSurfaceReconstruction
[15] https://zhuanlan.zhihu.com/p/90858099
[16] https://zhuanlan.zhihu.com/p/668272208