Fitting a plane to many points in 3D
当提及在给定 n 个 3D 点，用它们来进行平面拟合，你通常会想到哪些方式呢？在这篇文章中，将推导出一些简单的、数值上稳定的非迭代方法。参考1 2
1. 直接求解法
首先，如果你在网上寻找答案，你会得到包括对协方差矩阵进行奇异值分解以找到最小特征值的特征向量的答案。然而，事实证明，这使事情变得更加复杂。因此，让我们从最基础的直接求解法开始。
一个平面一般由法向量 $n = [a, b, c]^T$ 和距离 $d$ 来描述，因此对于平面上的点 $p = [x, y, z]^T，n \cdot p + d = 0$ 。我们可以写成这样： $$ ax + by + cz + d = 0 \tag{1.1} \label{1.1} $$ 然而，请注意，这个问题是过定 (overdetermined) – 即，解空间（一个平面）是三维的，但上述描述使用了四个参数值。因此，让我们首先通过限制解空间来去除一个分量。我们通过任意指定 $c=1$ 来做到这一点，也就是说，平面法线的 $z$ 分量总是 $1$（注意，法线的长度不需要是1）。如果你认为这是一个有潜在问题的假设，你是对的 – 我们将在后面再讨论这个问题。现在，让我们来定义一下： $$ \begin{matrix} ax + by + z + d = 0 \newline \therefore \newline ax + by + d = -z \end{matrix} \tag{1.2} \label{1.2} $$ 那么可以通过式 $\eqref{1.3}$ 来求解 $a, b, d$ ： $$ \begin{bmatrix} x_0 & y_0 & 1 \newline x_1 & y_1 & 1 \newline & \cdots \newline x_n & y_n &1 \end{bmatrix} \begin{bmatrix} a \newline b \newline d \end{bmatrix} = \begin{bmatrix} -z_0 \newline -z_1 \newline \cdots \newline -z_n \end{bmatrix} \tag{1.3} \label{1.3} $$ 接下来，我们把这个矩阵，转置，并从左边乘以，进行线性最小二乘法：
$$ \begin{bmatrix} x_0 & x_1 & \cdots & x_n \newline y_0 & y_2 & \cdots & y_n \newline 1 & 1 & \cdots &1 \end{bmatrix} \begin{bmatrix} x_0 & y_0 & 1 \newline x_1 & y_1 & 1 \newline & \cdots \newline x_n & y_n &1 \end{bmatrix} \begin{bmatrix} a \newline b \newline d \end{bmatrix} = \begin{bmatrix} x_0 & x_1 & \cdots & x_n \newline y_0 & y_2 & \cdots & y_n \newline 1 & 1 & \cdots &1 \end{bmatrix} \begin{bmatrix} -z_0 \newline -z_1 \newline \cdots \newline -z_n \end{bmatrix} \tag{1.4} \label{1.4} $$ 化简归类得： $$ \begin{bmatrix} \sum x_i x_i & \sum x_i y_i & \sum x_i \newline \sum y_i x_i & \sum y_i y_i & \sum y_i \newline \sum x_i & \sum y_i & N \end{bmatrix} \begin{bmatrix} a \newline b \newline d \end{bmatrix} = - \begin{bmatrix} \sum x_i z_i \newline \sum y_i z_i \newline \sum z_i \end{bmatrix} \tag{1.5} \label{1.5} $$ 其中 N 为点的数量。现在，聪明的优化部分来了：让我们把上面的 $x,y,z$ 定义为相对于点云的中心点（平均值）。现在 $\sum x = \sum y = \sum z = 0$ ，所以我们可以简化为： $$ \begin{bmatrix} \sum x_i x_i & \sum x_i y_i & 0 \newline \sum y_i x_i & \sum y_i y_i & 0 \newline 0 & 0 & N \end{bmatrix} \begin{bmatrix} a \newline b \newline d \end{bmatrix} = - \begin{bmatrix} \sum x_i z_i \newline \sum y_i z_i \newline 0 \end{bmatrix} \tag{1.6} \label{1.6} $$ 从最后一行（$N \cdot d = 0$）我们可以得出结论：$d = 0$。这意味着，如果所有的点都是相对于点云的中心点，那么平面就会通过原点。换句话说：平面总是穿过输入点的平均值。我们现在可以摆脱一个维度了： $$ \begin{bmatrix} \sum x_i x_i & \sum x_i y_i \newline \sum y_i x_i & \sum y_i y_i \end{bmatrix} \begin{bmatrix} a \newline b \end{bmatrix} = - \begin{bmatrix} \sum x_i z_i \newline \sum y_i z_i \end{bmatrix} \tag{1.7} \label{1.7} $$ 根据克莱默法则 (Cramer’s rule) 得： $$ \begin{matrix} D = \sum xx \times \sum yy - \sum xy \times \sum xy \newline a = (\sum yz \times \sum xy - \sum xz \times \sum yy) / D \newline b = (\sum xy \times \sum xz - \sum xx \times \sum yz) / D \newline n = [a, b, 1]^T \end{matrix} \tag{1.8} \label{1.8} $$ 我们可以通过将 $n$ 与 $D$ 相乘来简化这个问题（无论如何我们都需要对 $n$ 进行归一化处理），这样就可以得到： $$ \begin{matrix} D = \sum xx \times \sum yy - \sum xy \times \sum xy \newline a = \sum yz \times \sum xy - \sum xz \times \sum yy \newline b = \sum xy \times \sum xz - \sum xx \times \sum yz \newline n = [a, b, D]^T \end{matrix} \tag{1.9} \label{1.9} $$ 就这样了! 但请记住我们的假设：平面法线的 $z$ 分量是不为零的。如果它是零呢？那么可以证明，上面的行列式 $D$ 变成了零，我们有除以零。即使它不完全是零，但接近零，我们仍然会得到不好的条件，从而得到不好的结果。那么我们能做什么呢？好吧，如果这些点确实跨越了一个平面，那么法线的分量中至少有一个必须是非零的。因此，让我们对哪一个分量是非零的三个独立假设进行上述计算。然后，我们简单地选取行列式最大的那个。
注意：这个方法将最小化垂直于主轴的残差的平方，而不是垂直于平面的残差。如果残差很小（即你的点都靠近结果平面），那么这个方法可能就足够了。然而，如果你的点比较分散，那么这种方法可能不是最好的拟合。
C++代码点击查看
Plane plane_from_points(const std::vector<Eigen::Vector3f>& points) {
if (points.size() < 3) throw std::runtime_error("Not enough points to calculate plane");
Eigen::Vector3f sum(0, 0, 0);
for (auto& point : points) sum += point;
Eigen::Vector3f centroid = sum / float(points.size());
double xx = 0, xy = 0, xz =0, yy = 0, yz = 0, zz = 0;
for (auto& point : points) {
Eigen::Vector3f temp = point - centroid;
xx += temp.x() * temp.x();
xy += temp.x() * temp.y();
xz += temp.x() * temp.z();
yy += temp.y() * temp.y();
yz += temp.y() * temp.z();
zz += temp.z() * temp.z();
}
double det_x = yy*zz - yz*yz;
double det_y = xx*zz - xz*xz;
double det_z = xx*yy - xy*xy;
double det_max = std::max({det_x, det_y, det_z});
if (det_max <= 0) return {0, 0, 0, 0};
Eigen::Vector3f dir;
if (det_max == det_x) {
float a = static_cast<float>((xz*yz - xy*zz) / det_x);
float b = static_cast<float>((xy*yz - xz*yy) / det_x);
dir = Eigen::Vector3f(1, a, b);
} else if (det_max == det_y) {
float a = static_cast<float>((yz*xz - xy*zz) / det_y);
float b = static_cast<float>((xy*xz - yz*xx) / det_y);
dir = Eigen::Vector3f(a, 1, b);
} else {
float a = static_cast<float>((yz*xy - xz*yy) / det_z);
float b = static_cast<float>((xz*xy - yz*xx) / det_z);
dir = Eigen::Vector3f(a, b, 1);
}
dir = dir.normalized();
float d = -dir.transpose() * centroid;
return {dir.x(), dir.y(), dir.z(), d};
}
2. SVD 分解法
一个平面一般由法向量 $n = [a, b, c]^T$ 和距离 $d$ 来描述， $$ ax + by + cz + d = 0 \tag{2.1} \label{2.1} $$ 约束条件为： $$ a^2 + b^2 + c^2 = 1 \tag{2.2} \label{2.2} $$ 为了使得拟合的平面是最佳的（即，求解最优的平面参数 $a, b, c, d$），对于 n 个点云数据，就是使得所有点云到该平面的距离平方和最小，即满足： $$ e = \sum_{i=1}^n d_i^2 \to \min \tag{2.3} \label{2.3} $$ 式中，$d_i = \lvert ax_i + by_i + cz_i + d \rvert$ 是点云数据中任意一点 $p_i = [x_i, y_i, z_i]^T$ 到这个平面的距离。要使得 $e \to \min$ ，可以使用 SVD 矩阵分解得到。
设所有点的平均坐标为 $[\bar x , \bar y , \bar z]^T$ ，则： $$ a \bar x + b \bar y + c \bar z + d = 0 \tag{2.4} \label{2.4} $$ 式 $\eqref{2.1} - \eqref{2.4}$ 得： $$ a(x_i - \bar x) + b (y_i - \bar y) + c (z_i - \bar z) = 0 \tag{2.5} \label{2.5} $$ 整合可得：
$$ \underbrace{\begin{bmatrix} x_1 - \bar x & y_1 - \bar y & z_1 - \bar z \newline x_2 - \bar x & y_2 - \bar y & z_2 - \bar z \newline & \cdots \newline x_n - \bar x & y_n - \bar y & z_n - \bar z \end{bmatrix}} _A \underbrace{\begin{bmatrix} a \newline b \newline c \end{bmatrix}} _X = 0 \tag{2.6} \label{2.6} $$
理想情况下所有点都在平面上,式 $\eqref{2.6}$ 成立，实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为： $$ \min \lVert AX \rVert \quad s.t. \lVert X \rVert = 1 \tag{2.7} \label{2.7} $$ 对 $A$ 做奇异值分解： $$ A = UDV^T \tag{2.8} \label{2.8} $$ 则： $$ \lVert AX \rVert = \lVert UDV^TX \rVert = \lVert DV^TX \rVert \tag{2.9} \label{2.9} $$ 其中，$V^T X$ 为列矩阵，并且 $$ \lVert V^T X \rVert = \lVert X \rVert = 1 \tag{2.10} \label{2.10} $$ 因为 $D$ 的对角元素为奇异值，假设最后一个对角元素为最小奇异值，则当且仅当： $$ V^T X = \begin{bmatrix} 0 \newline 0 \newline \cdots \newline 1 \end{bmatrix} \tag{2.11} \label{2.11} $$ 时，式 $\eqref{2.9}$ 可以取得最小值，即式 $\eqref{2.7}$ 成立了。此时有： $$ X = V \begin{bmatrix} 0 \newline 0 \newline \cdots \newline 1 \end{bmatrix} = \begin{bmatrix} v_1 & v_2 & \cdots & v_n \end{bmatrix}\begin{bmatrix} 0 \newline 0 \newline \cdots \newline 1 \end{bmatrix} \tag{2.12} \label{2.12} $$ 因此，$X = (a, b, c) = (v_{n,1}, v_{n,2}, v_{n,3})$ 。所以， $e$ 的最小值就是矩阵 $A$ 的最小特征值，对应的特征向量为平面参数 $a, b, c$，利用质心可求得 $d$ 。
C++代码点击查看
Plane plane_from_points(const Eigen::MatrixXd& points) {
// 1、计算质心
Eigen::RowVector3d centroid = points.colwise().mean();
// 2、去质心
Eigen::MatrixXd demean = points;
demean.rowwise() -= centroid;
// 3、SVD分解求解协方差矩阵的特征值特征向量
Eigen::JacobiSVD<Eigen::MatrixXd> svd(demean, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Matrix3d V = svd.matrixV();
Eigen::MatrixXd U = svd.matrixU();
Eigen::Matrix3d S = U.inverse() * demean * V.transpose().inverse();
// 5、平面的法向量a,b,c
Eigen::RowVector3d normal;
normal << V(0,2), V(1,2), V(2,2);
// 6、原点到平面的距离d
double d = -normal * centroid.transpose();
// 7、获取拟合平面的参数a,b,c,d和质心x,y,z。
return {normal, d, centroid};
}
3. 拉格朗日乘子法
一个平面一般由法向量 $n = [a, b, c]^T$ 和距离 $d$ 来描述， $$ ax + by + cz + d = 0 \tag{3.1} \label{3.1} $$ 约束条件为： $$ a^2 + b^2 + c^2 = 1 \tag{3.2} \label{3.2} $$ 为了使得拟合的平面是最佳的（即，求解最优的平面参数 $a, b, c, d$），对于 n 个点云数据，就是使得所有点云到该平面的距离平方和最小，即满足： $$ e = \sum_{i=1}^n d_i^2 \to \min \tag{3.3} \label{3.3} $$ 式中，$d_i = \lvert ax_i + by_i + cz_i + d \rvert$ 是点云数据中任意一点 $p_i = [x_i, y_i, z_i]^T$ 到这个平面的距离。要使得 $e \to \min$ ，可以使用拉格朗日乘子法3求解极值，得到函数：
$$ f = e - \lambda (a^2 + b^2 + c^2 - 1) \tag{3.4} \label{3.4} $$ 先将式 $\eqref{3.4}$ 两边对 $d$ 求偏导，并令偏导数为零，得到： $$ d = \frac 1n (a \sum_{i=1}^n x_i + b \sum_{i=1}^n y_i + c \sum_{i=1}^n z_i) \tag{3.5} \label{3.5} $$ 令 $\bar x = \sum_{i = 0}^n x_i / n,\ \bar y = \sum_{i = 0}^n y_i / n,\ \bar z = \sum_{i = 0}^n z_i / n,\ \Delta x_i = x_i - \bar x,\ \Delta y_i = y_i - \bar y,\ \Delta z_i = z_i - \bar z$ ，质心为 $\bar p (\bar x, \bar y, \bar z)$ ，则： $$ d_i = \lvert a \Delta x_i + b \Delta y_i + c \Delta z_i \rvert \tag{3.6} \label{3.6} $$ 再对式 $\eqref{3.4}$ 两边对 $a, b, c$ 求偏导，得： $$ \begin{align*} 2 \sum_{i = 1}^n (a \Delta x_i + b \Delta y_i + c \Delta z_i) \Delta x_i - 2\lambda a &= 0 \newline 2 \sum_{i = 1}^n (a \Delta x_i + b \Delta y_i + c \Delta z_i) \Delta y_i - 2\lambda b &= 0 \newline 2 \sum_{i = 1}^n (a \Delta x_i + b \Delta y_i + c \Delta z_i) \Delta z_i - 2\lambda c &= 0 \end{align*} $$ 将上述方程组构成特征值方程： $$ \begin{matrix} A {\rm x} = \lambda {\rm x} \newline A = \begin{bmatrix} \sum_{i=1}^n \Delta x_i \Delta x_i & \sum_{i=1}^n \Delta x_i \Delta y_i & \sum_{i=1}^n \Delta x_i \Delta z_i \newline \sum_{i=1}^n \Delta x_i \Delta y_i & \sum_{i=1}^n \Delta y_i \Delta y_i & \sum_{i=1}^n \Delta y_i \Delta z_i \newline \sum_{i=1}^n \Delta x_i \Delta z_i & \sum_{i=1}^n \Delta y_i \Delta z_i & \sum_{i=1}^n \Delta z_i \Delta z_i \newline \end{bmatrix} , {\rm x} = \begin{bmatrix} a \newline b \newline c \end{bmatrix} \end{matrix} \tag{3.7} \label{3.7} $$ 那么，求解平面参数 $a, b, c$ 就是求解矩阵的特征值和特征向量，又因为 $A$ 是 3 阶实对称矩阵，其特征值求解公式为： $$ \lambda = \frac{(A{\rm x})^T \cdot {\rm x}}{{\rm x}^T \cdot {\rm x}} \quad {\rm x} \neq 0 \tag{3.8} \label{3.8} $$ 在约束条件 $a^2 + b^2 + c^2 = 1$ 下，可得 $\lambda = \sum_{i = 1}^n (a \Delta x_i + b \Delta y_i +c \Delta z_i)^2 = \sum_{i=1}^n d_i^2 = e$ ，所以，$e$ 的最小值就是矩阵 $A$ 的最小特征值，对应的特征向量为平面参数 $a, b, c$ ，利用质心可以求得 $d$ 。
C++代码点击查看
Plane plane_from_points(const std::vector<Eigen::Vector3f>& points) {
if (points.size() < 3) throw std::runtime_error("Not enough points to calculate plane");
Eigen::Vector3f sum(0, 0, 0);
for (auto& point : points) sum += point;
Eigen::Vector3f centroid = sum / float(points.size());
double xx = 0, xy = 0, xz =0, yy = 0, yz = 0, zz = 0;
for (auto& point : points) {
Eigen::Vector3f temp = point - centroid;
xx += temp.x() * temp.x();
xy += temp.x() * temp.y();
xz += temp.x() * temp.z();
yy += temp.y() * temp.y();
yz += temp.y() * temp.z();
zz += temp.z() * temp.z();
}
Eigen::Matrix3d A;
A << xx, xy, xz, xy, yy, yz, xz, yz, zz;
Eigen::EigenSolver<Eigen::Matrix3d> es(A);
Eigen::Matrix3d D = es.pseudoEigenvalueMatrix();
Eigen::Matrix3d V = es.pseudoEigenvectors();
Eigen::Vector3f dir;
dir << (float)V(0,2), (float)V(1,2), (float)V(2,2);
float d = -dir.transpose() * centroid;
return {dir.x(), dir.y(), dir.z(), d};
}