3d视觉——3.平面提取方式(open3d/python/cpp)

这里记录了常用的平面提取方式，主要介绍平面在3d点云中的价值和意义，以及常用平面提取的场景和方式，主要包括最常ranscan算法和最小二乘算法，并附上了python/cpp代码的实现方式。

场景和问题

在点云处理过程中，存在大量的平面结构，如地面、墙面，这些平面可以作为基本特征使用，又比如工件的外表，可以作为测量的基本元素。
所以，需要有提取平面的方式，获取点云平面。

常用算法

常用的方式为ranscan，法向量过滤+最小二乘。
ranscan是最常用的平面提取算法
法向量过滤+最小二乘是更精确的平面提取算法。

ranscan

ranscan算法，即随机采样一致性算法，是一种迭代算法，用于从点云中提取平面。

算法原理

这个算法的泛用性很强，可以用于提取任意形状的几何特征，而不仅仅是平面，这里根据其应用场景，只讨论简化的平面提取原理。
1.模型假设：平面方程ax+by+cz+d=0，
2.随机采样点云中不共面3d点，这三个点可以确定一个平面，解出平面参数a,b,c,d。
3.计算点云中所有点到该平面的距离，设置阈值，距离小于阈值的点为平面点，距离大于阈值的点为非平面点。
4.统计平面内的点数
5.多次重复步骤2-4，每次随机采样3d点，计算距离，统计平面内的点数，选取点数最多的平面作为最终结果。
这个算法有一个特点，就是只要迭代次数多，就可以稳定的得到

算法特点

这个算法是平面提取中最常用的一种
使用一个基本的假设，即平面内的点数最多，平面外的点数最少，只要迭代次数多，所以可以稳定得到平面。
这带来了一个优点，就是鲁棒性强，在复杂场景中只要有平面，基本都能获得。
但是也带来了一个缺点，就是这个算法含有随机性，每次得到的平面都会有轻微不同，只能是大致的一个平面。

代码实现

# ranscan平面提取
plane_model, inliers = down_pcd.segment_plane(distance_threshold=10, 
                                                ransac_n=3,
                                                num_iterations=1000)
[a, b, c, d] = plane_model
print(f"平面模型: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
inlier_cloud = down_pcd.select_by_index(inliers)
o3d.io.write_point_cloud("pointcloud_plane.pcd", inlier_cloud)
print("点云保存到pointcloud_plane.pcd")

其中，
distance_threshold：点到平面的最大距离（内点判定阈值）
ransac_n：为每次随机采样点数（拟合平面需要3个点）
num_iterations：迭代次数

    // 1. RANSAC平面提取
    auto segment_result = down_pcd.SegmentPlane(
        10.0,    // distance_threshold
        3,       // ransac_n
        1000     // num_iterations
    );

    // 2. 获取平面模型参数和内点索引
    Eigen::Vector4d plane_model = segment_result.first;
    std::vector<size_t> inliers = segment_result.second;

    double a = plane_model(0);
    double b = plane_model(1);
    double c = plane_model(2);
    double d = plane_model(3);

    // 3. 打印平面方程 (C++中格式化输出需要 <iomanip> 头文件)
    std::cout << std::fixed << std::setprecision(2);
    std::cout << "平面模型: " << a << "x + " << b << "y + " << c << "z + " << d << " = 0" << std::endl;

    // 4. 根据索引提取内点点云
    auto inlier_cloud = down_pcd.SelectByIndex(inliers);

    // 5. 保存点云
    open3d::io::WritePointCloud("pointcloud_plane.pcd", *inlier_cloud, true, true);
    std::cout << "点云保存到pointcloud_plane.pcd" << std::endl;

最小二乘

最小二乘法是一种数学方法，用于在给定的数据中找到最佳拟合的平面。

算法原理

最小二乘也是一种迭代算法，但是和ranscan算法不同，ranscan算法是随机采样，而最小二乘算法是全局搜索。
1.模型假设：平面方程ax+by+cz+d=0，
2.计算点云中所有点到该平面的距离，计算距离的平方和，得到目标函数。
3.求取最目标函数值，对目标函数求导，得到导数为0的方程，解出a,b,c,d。
4.得到平面参数a,b,c,d。
这个算法在求解最小值时可以使用拉格朗日乘子法，处理后可以等效为特征值分解，而且更方便快速，所以是点云处理中常用的算法。
1.计算质心
2.去中心化
3.计算协方差矩阵
4.计算特征值和特征向量
5.选取特征值最小的特征向量，即为法向量
6.计算d，得到平面参数a,b,c,d。

算法特点

算法特点

这个算法的假设是处理的点云基本已经形成了平面，对点云进行全局搜索，能得到唯一的平面。
这个特点解决了ranscan算法的随机性问题，算是主要优点。
但是这个算法的缺点也很明显，就是鲁棒性差，如果点云中存在噪声，或者点云没有形成平面，那么这个算法就会失效。
所以需要配合其他算法，先提取点云平面，再进行最小二乘。
常用的方式是使用法向量过滤，先提取点云平面，再进行最小二乘。

代码实现

import numpy as np

def fit_plane_pca(points: np.ndarray):
    """
    用 PCA / SVD 拟合平面（完全确定）
    返回：法向量、d、质心
    """
    centroid = points.mean(axis=0)
    centered = points - centroid

    _, _, Vt = np.linalg.svd(centered, full_matrices=False)

    normal = Vt[2, :]

    d = -np.dot(normal, centroid)

    return normal, d, centroid

/**
   * @brief 使用 PCA / SVD 拟合平面
   */
  std::vector<double> fit_plane_pca(
      const std::vector<Eigen::Vector3d> &points)
  {
    const std::size_t N = points.size();

    // ---------- 1. 计算质心 ----------
    Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
    for (const auto &p : points)
    {
      centroid += p;
    }
    centroid /= static_cast<double>(N);

    // ---------- 2. 去质心 ----------
    Eigen::MatrixXd centered(N, 3);
    for (std::size_t i = 0; i < N; ++i)
    {
      centered.row(i) = (points[i] - centroid).transpose();
    }

    // ---------- 3. SVD ----------
    Eigen::BDCSVD<Eigen::MatrixXd> svd(
        centered, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::Vector3d normal =
        svd.matrixV().transpose().row(2).transpose();

    // ---------- 4. 保证法向量 z >= 0 ----------
    if (normal.z() < 0)
    {
      normal = -normal;
    }

    // ---------- 5. 平面参数 ----------
    double a = normal.x();
    double b = normal.y();
    double c = normal.z();
    double d = -normal.dot(centroid);

    return {a, b, c, d};
  }

总结

ranscan算法和最小二乘算法是两种常用的平面提取算法
ranscan算法是随机采样，鲁棒性强，但是随机性导致结果不稳定
最小二乘算法是全局搜索，结果稳定，但是鲁棒性差，需要配合其他算法使用
在对精度要求不高的场景中，ranscan算法是首选，比如导航
而在对精度要求高的场景中，最小二乘算法是首选，比如工业测量。