PCL教程指南-判断点的邻域范围是否为平面

PCL教程指南-判断点的邻域范围是否为平面

• 用于过滤平面结构或者说判断是否点邻域范围是否为平面
• 两种方法：
  • 在某点邻域范围内进行RANSAC
  • OBB包围盒坐标值差距
• 以下给出两种方法的核心代码

RANSAC判断邻域范围是否为平面

 /**
     * sample plane
     */
//pointIdxNKNSearch 为计算点的邻域点索引
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud,pointIdxNKNSearch));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
    ransac.setDistanceThreshold (0.000000001);
    ransac.setMaxIterations(1);
    ransac.computeModel();
    std::vector<int> inliers;

    ransac.getInliers(inliers);
    cout<<inliers.size()<<endl;

OBB包围盒判断邻域范围是否为平面

  /**
     * eigen OBB plane
     */

    int  sum=pointIdxNKNSearch.size();
    Eigen::Matrix<float,3,Eigen::Dynamic> kMatrix;
    kMatrix.resize(3,sum);
	//点云坐标转矩阵形式
    for (int i = 0; i < pointIdxNKNSearch.size(); ++i) {
        kMatrix.col(i)=cloud->points[pointIdxNKNSearch[i]].getVector3fMap();
    }
    //各坐标维度均值
    Eigen::MatrixXf meanval=kMatrix.rowwise().mean();
    kMatrix.colwise()-=(Eigen::VectorXf)meanval;
	//协方差矩阵
    Eigen::Matrix<float,3,3> covMatrix;
    covMatrix=(kMatrix*kMatrix.transpose())/sum;
	//特征值特征向量
    Eigen::EigenSolver<Eigen::Matrix<float,3,3>> eigen_solver(covMatrix,true);
    Eigen::Matrix<float,3,3> eigenVectors=eigen_solver.eigenvectors().real();
    Eigen::Matrix<float,3,1> eigenValues =eigen_solver.pseudoEigenvalueMatrix().diagonal();
    //特征向量按特征值排序
    sortEigenVectorByValues(eigenValues, eigenVectors);
    //转换坐标后的OBB包围盒矩阵形式
    Eigen::Matrix<float,Eigen::Dynamic,3> obbMatrix;
    obbMatrix=kMatrix.transpose()*eigenVectors;
    //判断最小维度距离
    double minv=(obbMatrix.colwise().maxCoeff()-obbMatrix.colwise().minCoeff()).minCoeff();
cout<<minv<<endl;



///特征向量按特征值排序  （排序方法参考https://blog.csdn.net/qq_24380063/article/details/104958703）
      void sortEigenVectorByValues(Eigen::Matrix<float,3, 1>& eigenValues, Eigen::Matrix<float,3,3>& eigenVectors) {
	std::vector<std::tuple<double, Matrix<float, 3,1 >>> eigenValueAndVector;
	int size = static_cast<int>(eigenValues.size());

	eigenValueAndVector.reserve(size);
	//向量元组形式存储 特征值-特征向量对
	for (int i = 0; i < size; ++i)
		eigenValueAndVector.push_back(std::tuple<double, Matrix<float, 3,1 >>(eigenValues[i], eigenVectors.col(i)));

	// 按特征值按从大到小排序
	std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
		[&](const std::tuple<double, Matrix<float, 3,1 >>& a, const std::tuple<double, Matrix<float, 3,1 >>& b) -> bool {
		return std::get<0>(a) > std::get<0>(b);
		});
	//特征向量排好序后，交换特征向量
	for (int i = 0; i < size; ++i) {
		eigenVectors.col(i).swap(std::get<1>(eigenValueAndVector[i]));
	}
}