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_modelprint(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平面提取autosegment_result=down_pcd.SegmentPlane(10.0,// distance_threshold3,// ransac_n1000// num_iterations);// 2. 获取平面模型参数和内点索引Eigen::Vector4d plane_model=segment_result.first;std::vector<size_t>inliers=segment_result.second;doublea=plane_model(0);doubleb=plane_model(1);doublec=plane_model(2);doubled=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. 根据索引提取内点点云autoinlier_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算法的随机性问题，算是主要优点。
但是这个算法的缺点也很明显，就是鲁棒性差，如果点云中存在噪声，或者点云没有形成平面，那么这个算法就会失效。
所以需要配合其他算法，先提取点云平面，再进行最小二乘。
常用的方式是使用法向量过滤，先提取点云平面，再进行最小二乘。
代码实现
importnumpyasnpdeffit_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)returnnormal,d,centroid/** * @brief 使用 PCA / SVD 拟合平面 */std::vector<double>fit_plane_pca(conststd::vector<Eigen::Vector3d>&points){conststd::size_t N=points.size();// ---------- 1. 计算质心 ----------Eigen::Vector3d centroid=Eigen::Vector3d::Zero();for(constauto&p:points){centroid+=p;}centroid/=static_cast<double>(N);// ---------- 2. 去质心 ----------Eigen::MatrixXdcentered(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. 平面参数 ----------doublea=normal.x();doubleb=normal.y();doublec=normal.z();doubled=-normal.dot(centroid);return{a,b,c,d};}总结
ranscan算法和最小二乘算法是两种常用的平面提取算法
ranscan算法是随机采样，鲁棒性强，但是随机性导致结果不稳定
最小二乘算法是全局搜索，结果稳定，但是鲁棒性差，需要配合其他算法使用
在对精度要求不高的场景中，ranscan算法是首选，比如导航
而在对精度要求高的场景中，最小二乘算法是首选，比如工业测量。