本文由CSDN点云侠原创,原文链接。爬虫网站自重。博客长期更新,本文最新更新时间为:2023年8月6日。
1.平面拟合
设拟合出的平面方程为:
a
x
+
b
y
+
c
z
+
d
=
0
(1)
ax+by+cz+d=0\tag{1}
ax+by+cz+d=0(1)
约束条件为:
a
2
+
b
2
+
c
2
=
1
(2)
a^2+b^2+c^2=1\tag{2}
a2+b2+c2=1(2)
可以得到平面参数
a
、
b
、
c
、
d
a、b、c、d
a、b、c、d。此时,要使获得的拟合平面是最佳的,就是使点到该平面的距离的平方和最小,即满足:
e
=
∑
i
=
1
n
d
i
2
→
m
i
n
(3)
e=\sum_{i=1}^nd_i^2\rightarrow min\tag{3}
e=i=1∑ndi2→min(3)
式中,
d
i
d_i
di是点云数据中的任一点
p
i
(
x
i
,
y
i
,
z
i
)
p_i(x_i,y_i,z_i)
pi(xi,yi,zi)到这个平面的距离
d
i
=
∣
a
x
i
+
b
y
i
+
c
z
i
+
d
∣
d_i=|ax_i+by_i+cz_i+d|
di=∣axi+byi+czi+d∣。要使
e
→
m
i
n
e\rightarrow min
e→min,可以用
S
V
D
SVD
SVD矩阵分解得到。
推导过程如下:
所有点的平均坐标为
(
x
ˉ
.
y
ˉ
,
z
ˉ
)
(\bar{x}.\bar{y},\bar{z})
(xˉ.yˉ,zˉ),则:
a
x
ˉ
+
b
y
ˉ
+
c
z
ˉ
+
d
=
0
(4)
a\bar{x}+b\bar{y}+c\bar{z}+d=0\tag{4}
axˉ+byˉ+czˉ+d=0(4)
式(1)与式(4)相减得:
a
(
x
i
−
x
ˉ
)
+
b
(
y
i
−
y
ˉ
)
+
c
(
z
i
−
z
ˉ
)
=
0
(5)
a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z})=0\tag{5}
a(xi−xˉ)+b(yi−yˉ)+c(zi−zˉ)=0(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
=
[
a
b
c
]
X=\left[ \begin{matrix} a\\ b \\ c \\ \end{matrix} \right]
X=
abc
则式(5)等价于:
A
X
=
0
(6)
AX=0\tag{6}
AX=0(6)
理想情况下所有点都在平面上,式(6)成立;实际情况下有部分点在平面外,拟合的目的为平面距离所有点的距离之和尽量小,所以目标函数为:
m
i
n
∣
∣
A
X
∣
∣
(7)
min||AX||\tag{7}
min∣∣AX∣∣(7)
约束条件为:
∣
∣
X
∣
∣
=
1
(8)
||X||=1\tag{8}
∣∣X∣∣=1(8)
若
A
A
A可做奇异值分解:
A
=
U
D
V
T
(9)
A=UDV^T\tag{9}
A=UDVT(9)
则:
∣
∣
A
X
∣
∣
=
∣
∣
U
D
V
T
X
∣
∣
=
∣
∣
D
V
T
X
∣
∣
(10)
||AX||=||UDV^TX||=||DV^TX||\tag{10}
∣∣AX∣∣=∣∣UDVTX∣∣=∣∣DVTX∣∣(10)
其中:
V
T
X
V^TX
VTX为列矩阵,并且:
∣
∣
V
T
X
∣
∣
=
∣
∣
X
∣
∣
=
1
(11)
||V^TX||=||X||=1\tag{11}
∣∣VTX∣∣=∣∣X∣∣=1(11)
因为
D
D
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
[
0
0
0
⋯
1
]
=
[
v
1
v
2
v
3
⋯
v
n
]
[
0
0
0
⋯
1
]
(13)
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}
X=V
000⋯1
=[v1v2v3⋯vn]
000⋯1
(13)
目标函数(7)在约束条件(8)下的最优解为:
X
=
(
a
,
b
,
c
)
=
(
v
n
,
1
,
v
n
,
2
,
v
n
,
3
)
(14)
X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3})\tag{14}
X=(a,b,c)=(vn,1,vn,2,vn,3)(14)
所以,
e
e
e的最小值就是矩阵
A
A
A的最小特征值,对应的特征向量为平面参数
a
、
b
、
c
a、b、c
a、b、c ,利用质心可求得
d
d
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();
}
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法)