点云平面拟合及可视化

不使用法线

#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;
}