CloudCompare——点云平面拟合

点云侠

已于 2023-08-14 19:40:48 修改

阅读量1.6w

点赞数 17

分类专栏： CloudCompare 文章标签：平面开发语言算法计算机视觉矩阵线性代数

于 2021-12-07 08:11:14 首次发布

本文链接：https://blog.csdn.net/qq_36686437/article/details/121725546

版权

CloudCompare 专栏收录该内容

65 篇文章 984 订阅

订阅专栏

1.平面拟合

设拟合出的平面方程为：
$ax+by+cz+d=0\tag{1}$
约束条件为： $a^2+b^2+c^2=1\tag{2}$
可以得到平面参数 $a 、 b 、 c 、 d$ 。此时，要使获得的拟合平面是最佳的，就是使点到该平面的距离的平方和最小，即满足：
$e=\sum_{i=1}^nd_i^2\rightarrow min\tag{3}$
式中， $d_i$ 是点云数据中的任一点 $p_i(x_i,y_i,z_i)$ 到这个平面的距离 $d_i=|ax_i+by_i+cz_i+d|$ 。要使 $e\rightarrow min$ ，可以用 $S V D$ 矩阵分解得到。
推导过程如下：
所有点的平均坐标为 $(\bar{x}.\bar{y},\bar{z})$ ,则：
$a\bar{x}+b\bar{y}+c\bar{z}+d=0\tag{4}$
式(1)与式(4)相减得：
$a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z})=0\tag{5}$
假设矩阵：
$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]$
列矩阵：
$X=\left[ \begin{matrix} a\\ b \\ c \\ \end{matrix} \right]$
则式(5)等价于：
$AX=0\tag{6}$
理想情况下所有点都在平面上,式(6)成立；实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为：
$min||AX||\tag{7}$
约束条件为：

$||X||=1\tag{8}$
若 $A$ 可做奇异值分解：
$A=UDV^T\tag{9}$
则：
$||AX||=||UDV^TX||=||DV^TX||\tag{10}$
其中： $V^TX$ 为列矩阵，并且：
$||V^TX||=||X||=1\tag{11}$
因为 $D$ 的对角元素为奇异值，假设最后一个对角元素为最小奇异值，则当且仅当：
$V^TX=\left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix} \right]\tag{12}$
时，式(10)可以取得最小值，即式(7)成立。此时：
$X=V\left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix} \right] =\left[ \begin{matrix} v_1 & v_2 & v_3 & \cdots& v_n \\ \end{matrix} \right] \left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix} \right]\tag{13}$
目标函数(7)在约束条件(8)下的最优解为：
$X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3})\tag{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();
}