目录
 
 

  
一、算法原理
  

   

    
1、概述
    
2、拟合平面
    
3、实现过程
    
4、参考文献
   
 
  
二、代码示例
  

   

    
1、主要函数
    
2、代码实现
   
 
  
三、结果展示
 
 
 
 
一、算法原理
 
1、概述
 
  随机抽样一致性算法RANSAC(Random sample consensus)是一种迭代的方法来从一系列包含有离异值的数据中计算数学模型参数的方法。
   RANSAC算法本质上由两步组成，不断进行循环：
   (1)从输入数据中随机选出能组成数学模型的最小数目的元素，使用这些元素计算出相应模型的参数。选出的这些元素数目是能决定模型参数的

 
我有一个来自现实世界的点云,我想在它们上面拟合并计算点的曲线(！).
 
因为点在现实世界中,因此点的x,y和z之间的差异幅度非常大,当我在
 
here中使用代码时,我在matlab中遇到以下错误：
 
Warning: Rank deficient, rank = 2, tol = 7.9630e-007.
 
这意味着我的数据状况不佳.
 
我的一些数据是：
 
32512032.3900000 5401399.69000000 347.030000000000
 
32512033.1400000 5401399.79000000 346.920000000000
 
32512036.3000000 5401399.62000000 346.840000000000
 
32512037.3900000 5401399.95000000 346.870000000000
 
32512034.4800000 5401400 346.930000000000
 
32512035.6000000 5401400.05000000 346.950000000000
 
32512036.6900000 5401400.38000000 346.980000000000
 
32512037.9600000 5401400.30000000 346.910000000000
 
32512033.7600000 5401400.42000000 346.880000000000
 
32512034.8700000 5401400.48000000 346.960000000000
 
我也在matlab中使用了fit公式.
 
sf = fit( [x, y], z, 'poly23');
 
并看到同样的错误：
 
Warning: Equation is badly conditioned. Remove repeated data points
 
or try centering and scaling.
 
这种类型的点是否适合曲面或平滑曲线？

     点云法向量估计的主要思路是对K-近邻的N个点进行平面拟合（平面过N点重心），平面法向量即为所求；所以求法向量就是变相的求拟合平面。
 
下面我们用最小二乘法求k近邻点云的拟合平面：
 
 
当 ||x||＝1时，Ax=0的最小二乘解是ATA的最小特征值对应的特征向量等同于：ATA的最小特征值所对应的特征向量可使||Ax||最小。
 
结论：假设k-近邻点矩阵B，B为k*3的矩阵，则根据B拟合平面的法向量就是BTB对应的最小特征向量；
 
同理我们可以得出BTB对应的最大特征向量就是拟合平面的方向向量。（这里的前提是：B已经去中心化）
 

实际上，求一组点的拟合平面的过程，就是求其PCA的过程。因为PCA也是可以通过最小化投影距离推导出来的。即样本点到这个超平面的距离足够近。
 
PCA的推导:基于最小投影距离
 
重建：
 
                              
 
     上式中的x'其实表示原样本点在新的基下的拉伸程度系数，我们把拉伸系数再乘上对应的基向量（在原坐标系下的方向向量），就相当于求出投影后的样本点在原坐标系下的坐标。以三维坐标为例，将点P（x,y,z）投影到一个平面内，变成二维坐标M（x',y'）,平面使用2个基向量（基向量是3维）表示，（x',y'）再与2个基向量相乘求和，就得到点M在原三维空间的坐标。
 
即样本点到这个超平面的距离足够近。
 

不使用法线
 
#include <pcl/io/pcd_io.h>
#include <pcl/common/centroid.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

void visualization(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                   const pcl::ModelCoefficients::Ptr coefficients,
                   const pcl::PointIndices::Ptr inliers) {
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Plane"));
  viewer->setBackgroundColor(0, 0, 0);
  
  Eigen::Vector4f centroid;
  pcl::compute3DCentroid(*cloud, centroid);
  pcl::PointXYZ p1(centroid[0], centroid[1], centroid[2]);
  pcl::PointXYZ p2(centroid[0] + coefficients->values[0],
                   centroid[1] + coefficients->values[1],
                   centroid[2] + coefficients->values[2]);
  viewer->addArrow(p2, p1, 1, 0, 0, false);

  viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::copyPointCloud(*cloud, *inliers, *inlier_cloud);
  viewer->addPointCloud<pcl::PointXYZRGB>(inlier_cloud, "inlier_cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "inlier_cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "inlier_cloud");

  while (!viewer->wasStopped()) {
    viewer->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }
}

void FittingPlane(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                  pcl::ModelCoefficients::Ptr& coefficients,
                  pcl::PointIndices::Ptr& inliers) {
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients(true);
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);

  seg.setDistanceThreshold(0.03);
  seg.setMaxIterations(1000);

  seg.setInputCloud(cloud);
  seg.segment(*inliers, *coefficients);
}

void FittingPlane(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                  const Eigen::Vector3d& init_normal, const double& eps_angle_in_rad,
                  pcl::ModelCoefficients::Ptr& coefficients,
                  pcl::PointIndices::Ptr& inliers) {
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  seg.setOptimizeCoefficients(true);
  // seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);

  seg.setDistanceThreshold(0.03);
  seg.setMaxIterations(1000);

  seg.setAxis(init_normal.cast<float>());
  seg.setEpsAngle(eps_angle_in_rad);

  seg.setInputCloud(cloud);
  seg.segment(*inliers, *coefficients);
}

int main(int argc, char**argv) {
  // 加载点云模型
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("/tmp/color.pcd", *cloud) == -1) {
    std::cerr << "can't load point cloud file" << std::endl;
    return -1;
  }
  std::cout << "loaded " << cloud->width * cloud->height << " points" << std::endl;
  std::cout << "field: " << pcl::getFieldsList(*cloud) << std::endl;

  // 拟合平面
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  // FittingPlane(cloud, coefficients, inliers);
  Eigen::Vector3d init_normal;
  init_normal << 0, 0, 1;
  double eps_angle_in_rad = 10 * M_PI / 180; // 估计的法线和初始法线相差10°以内
  FittingPlane(cloud, init_normal, eps_angle_in_rad, coefficients, inliers);
  std::cout << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3];

  // 点云渲染和可视化
  visualization(cloud, coefficients, inliers);

  return 0;
}
 
 
使用法线
 
#include <pcl/io/pcd_io.h>
#include <pcl/common/centroid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

void EstimateNormal(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                    pcl::PointCloud<pcl::Normal>::Ptr normals) {
  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_est;
  normal_est.setInputCloud(cloud);
  normal_est.setRadiusSearch(0.5); // 对于每一个点都用半径为5cm的近邻搜索方式
  // normal_est.setKSearch(50); // 点云法向计算时，需要搜索的近邻点数目
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZRGB>);
  normal_est.setSearchMethod(kdtree); // 建立kdtree来进行近邻点集搜索
  normal_est.compute(*normals);
}

void FittingPlane(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                  const pcl::PointCloud<pcl::Normal>::Ptr normals,
                  pcl::ModelCoefficients::Ptr& coefficients,
                  pcl::PointIndices::Ptr& inliers) {
  pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> seg;
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight(0.1);  //设置表面法线权重系数
  seg.setInputNormals(normals);
  seg.setDistanceThreshold(0.03);
  seg.setMaxIterations(1000);
  seg.setInputCloud(cloud);
  seg.segment(*inliers, *coefficients);
}

void visualization(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                   const pcl::PointCloud<pcl::Normal>::Ptr& normals,
                   const pcl::ModelCoefficients::Ptr coefficients,
                   const pcl::PointIndices::Ptr inliers) {
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Plane"));
  viewer->setBackgroundColor(0, 0, 0);
  
  Eigen::Vector4f centroid;
  pcl::compute3DCentroid(*cloud, centroid);
  // std::cout << "centroid2: " << centroid[0] << ", " << centroid[1] << ", " << centroid[2] << ", " << centroid[3] << std::endl;
  pcl::PointXYZ p1(centroid[0], centroid[1], centroid[2]);
  pcl::PointXYZ p2(centroid[0] + coefficients->values[0],
                   centroid[1] + coefficients->values[1],
                   centroid[2] + coefficients->values[2]);
  viewer->addArrow(p2, p1, 1, 0, 0, false);

  viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::copyPointCloud(*cloud, *inliers, *inlier_cloud);
  viewer->addPointCloud<pcl::PointXYZRGB>(inlier_cloud, "inlier_cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "inlier_cloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "inlier_cloud");

  viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(cloud, normals, 5, 0.03, "normals");

  while (!viewer->wasStopped()) {
    viewer->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }
}

int main(int argc, char**argv) {
  // 加载点云模型
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("/tmp/color.pcd", *cloud) == -1) {
    std::cerr << "can't load point cloud file" << std::endl;
    return -1;
  }
  std::cout << "loaded " << cloud->width * cloud->height << " points" << std::endl;
  std::cout << "field: " << pcl::getFieldsList(*cloud) << std::endl;

  // 计算法线
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  EstimateNormal(cloud, normals);

  // 拟合平面
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  FittingPlane(cloud, normals, coefficients, inliers);
  std::cout << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3];

  // 点云渲染和可视化
  visualization(cloud, normals, coefficients, inliers);

  return 0;
}