CloudCompare——点云平面拟合

本文由CSDN点云侠原创，原文链接。爬虫网站自重。博客长期更新，本文最新更新时间为：2023年8月6日。

1.平面拟合

  设拟合出的平面方程为：
$$ax+by+cz+d=0\text{\hspace{1em}(1)}$$
约束条件为：$$a^2+b^2+c^2=1\text{\hspace{1em}(2)}$$
可以得到平面参数 $a、b、c、d$。此时，要使获得的拟合平面是最佳的，就是使点到该平面的距离的平方和最小，即满足：
$$e=\sum _{i=1}^n{d}_i^2\to min\text{\hspace{1em}(3)}$$
式中，$d_i$是点云数据中的任一点$p_i(x_i,y_i,z_i)$到这个平面的距离$d_i=|a{x}_i+b{y}_i+c{z}_i+d|$。要使$e\to min$，可以用$SVD$矩阵分解得到。
  推导过程如下：
所有点的平均坐标为$(\bar{x}.\bar{y},\bar{z})$,则：
$$a\bar{x}+b\bar{y}+c\bar{z}+d=0\text{\hspace{1em}(4)}$$
式(1)与式(4)相减得：
$$a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z})=0\text{\hspace{1em}(5)}$$
假设矩阵：
A = [ x 1 − x ˉ y 1 − y ˉ z 1 − z ˉ x 2 − x ˉ y 2 − y ˉ z 2 − z ˉ x 3 − x ˉ y 3 − y ˉ z 3 − z ˉ ⋯ x n − x ˉ y n − y ˉ z n − z ˉ ] A=\left[

$$\begin{matrix} x_1-\bar{x} & y_1-\bar{y} & z_1-\bar{z} \\ x_2-\bar{x} & y_2-\bar{y} & z_2-\bar{z} \\ x_3-\bar{x} & y_3-\bar{y} & z_3-\bar{z} \\ & \cdots & \\ x_n-\bar{x} & y_n-\bar{y} & z_n-\bar{z} \\ \end{matrix}$$ \right] A= ​x1​−xˉx2​−xˉx3​−xˉxn​−xˉ​y1​−yˉ​y2​−yˉ​y3​−yˉ​⋯yn​−yˉ​​z1​−zˉz2​−zˉz3​−zˉzn​−zˉ​ ​
列矩阵：
$$X=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$$
则式(5)等价于：
$$AX=0\text{\hspace{1em}(6)}$$
理想情况下所有点都在平面上,式(6)成立；实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为：
$$min||AX||\text{\hspace{1em}(7)}$$
约束条件为：

$$||X||=1\text{\hspace{1em}(8)}$$
若$A$可做奇异值分解：
$$A=UD{V}^T\text{\hspace{1em}(9)}$$
则：
$$||AX||=||UD{V}^{T}X||=||D{V}^{T}X||\text{\hspace{1em}(10)}$$
其中：$V^{T}X$为列矩阵，并且：
$$||V^{T}X||=||X||=1\text{\hspace{1em}(11)}$$
因为$D$的对角元素为奇异值，假设最后一个对角元素为最小奇异值，则当且仅当：
V T X = [ 0 0 0 ⋯ 1 ] (12) V^TX=\left[

$$\begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix}$$ \right]\tag{12} VTX= ​000⋯1​ ​(12)
时，式(10)可以取得最小值，即式(7)成立。此时：
$$X=V\left[\begin{array}{c}0\\ 0\\ 0\\ \cdots \\ 1\end{array}\right]=\left[\begin{array}{ccccc}{v}_1& v_2& v_3& \cdots & v_n\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 0\\ \cdots \\ 1\end{array}\right]\text{\hspace{1em}(13)}$$
目标函数(7)在约束条件(8)下的最优解为：
$$X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3})\text{\hspace{1em}(14)}$$
所以，$e$的最小值就是矩阵$A$的最小特征值，对应的特征向量为平面参数$a、b、c$ ，利用质心可求得$d$。

2.参考文献

[1]安超,赵文政,刘银华.面向车身虚拟匹配的非均匀点云数据配准算法[J].机械设计与研究,2021,37(04):129-134.DOI:10.13952/j.cnki.jofmdr.2021.0151.

3.操作流程

  通过Tools > Fit > Plane菜单访问。该工具在点云上拟合一个平面，并输出各种信息，如拟合均数、平面法线，甚至地质倾角和倾角方向值。

在控制台中，将输出以下信息:

• 平面拟合的RMSE
• 平面法向量(法向方向默认为指向Z坐标轴的正方向)
• 倾角和倾角方向
• 一个能够使拟合平面调整为水平的4x4变换矩阵

注意：

• 拟合出的平面会被添加到DB树中，作为点云的子节点
• 拟合出来的是一种“三角网格”。因此，可以使用Tools > Distances > Cloud/Mesh dist计算它们之间的距离

4.完整操作

5.算法源码

void MainWindow::doComputePlaneOrientation(bool fitFacet)
{
	if (!haveSelection())
		return;

	double maxEdgeLength = 0.0;
	if (fitFacet)
	{
		bool ok = true;
		static double s_polygonMaxEdgeLength = 0.0;
		maxEdgeLength = QInputDialog::getDouble(this, "Fit facet", "Max edge length (0 = no limit)", s_polygonMaxEdgeLength, 0, 1.0e9, 8, &ok);
		if (!ok)
			return;
		s_polygonMaxEdgeLength = maxEdgeLength;
	}

	ccHObject::Container selectedEntities = getSelectedEntities(); //warning, getSelectedEntites may change during this loop!
	bool firstEntity = true;
	
	for (ccHObject *entity : selectedEntities) 
	{
		ccShiftedObject* shifted = nullptr;
		CCLib::GenericIndexedCloudPersist* cloud = nullptr;

		if (entity->isKindOf(CC_TYPES::POLY_LINE))
		{
			ccPolyline* poly = ccHObjectCaster::ToPolyline(entity);
			cloud = static_cast<CCLib::GenericIndexedCloudPersist*>(poly);
			shifted = poly;
		}
		else
		{
			ccGenericPointCloud* gencloud = ccHObjectCaster::ToGenericPointCloud(entity);
			if (gencloud)
			{
				cloud = static_cast<CCLib::GenericIndexedCloudPersist*>(gencloud);
				shifted = gencloud;
			}
		}

		if (cloud)
		{
			double rms = 0.0;
			CCVector3 C;
			CCVector3 N;

			ccHObject* plane = nullptr;
			if (fitFacet)
			{
				ccFacet* facet = ccFacet::Create(cloud, static_cast<PointCoordinateType>(maxEdgeLength));
				if (facet)
				{
					plane = static_cast<ccHObject*>(facet);
					N = facet->getNormal();
					C = facet->getCenter();
					rms = facet->getRMS();

					//manually copy shift & scale info!
					if (shifted)
					{
						ccPolyline* contour = facet->getContour();
						if (contour)
						{
							contour->setGlobalScale(shifted->getGlobalScale());
							contour->setGlobalShift(shifted->getGlobalShift());
						}
					}
				}
			}
			else
			{
				ccPlane* pPlane = ccPlane::Fit(cloud, &rms);
				if (pPlane)
				{
					plane = static_cast<ccHObject*>(pPlane);
					N = pPlane->getNormal();
					C = *CCLib::Neighbourhood(cloud).getGravityCenter();
					pPlane->enableStippling(true);
				}
			}

			//as all information appears in Console...
			forceConsoleDisplay();

			if (plane)
			{
				ccConsole::Print(QString("[Orientation] Entity '%1'").arg(entity->getName()));
				ccConsole::Print("\t- plane fitting RMS: %f", rms);

				//We always consider the normal with a positive 'Z' by default!
				if (N.z < 0.0)
					N *= -1.0;
				ccConsole::Print("\t- normal: (%f,%f,%f)", N.x, N.y, N.z);
				//we compute strike & dip by the way
				PointCoordinateType dip = 0.0f;
				PointCoordinateType dipDir = 0.0f;
				ccNormalVectors::ConvertNormalToDipAndDipDir(N, dip, dipDir);
				QString dipAndDipDirStr = ccNormalVectors::ConvertDipAndDipDirToString(dip, dipDir);
				ccConsole::Print(QString("\t- %1").arg(dipAndDipDirStr));

				//hack: output the transformation matrix that would make this normal points towards +Z
				ccGLMatrix makeZPosMatrix = ccGLMatrix::FromToRotation(N, CCVector3(0, 0, PC_ONE));
				CCVector3 Gt = C;
				makeZPosMatrix.applyRotation(Gt);
				makeZPosMatrix.setTranslation(C-Gt);
				ccConsole::Print("[Orientation] A matrix that would make this plane horizontal (normal towards Z+) is:");
				ccConsole::Print(makeZPosMatrix.toString(12,' ')); //full precision
				ccConsole::Print("[Orientation] You can copy this matrix values (CTRL+C) and paste them in the 'Apply transformation tool' dialog");

				plane->setName(dipAndDipDirStr);
				plane->applyGLTransformation_recursive(); //not yet in DB
				plane->setVisible(true);
				plane->setSelectionBehavior(ccHObject::SELECTION_FIT_BBOX);

				entity->addChild(plane);
				plane->setDisplay(entity->getDisplay());
				plane->prepareDisplayForRefresh_recursive();
				addToDB(plane);

				if (firstEntity)
				{
					m_ccRoot->unselectAllEntities();
					m_ccRoot->selectEntity(plane);
				}
			}
			else
			{
				ccConsole::Warning(QString("Failed to fit a plane/facet on entity '%1'").arg(entity->getName()));
			}
		}
	}

	refreshAll();
	updateUI();
}

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

6.相关代码

[1] PCL 最小二乘平面拟合(SVD法)
[2] PCL 使用RANSAC拟合平面
[3] PCL RANSAC分割多个平面
[4] PCL 中实现平面模型分割
[5] Open3D 最小二乘拟合平面(SVD法)
[6] Open3D——RANSAC三维点云平面拟合
[7] Open3D 使用RANSAC分割平面
[8] matlab RANSAC拟合平面
[9] matlab 最小二乘拟合平面（SVD法）