利用PCL做点云的平面拟合

前提：

需要验证一组点云的误差，基本上都是墙面上的点，直观地看点云的话可以看出一个平面，但远近不一致，因此想尝试一下做一下平面几何，计算一下内点数量，然后算一下精度；

可以自己用ＲＡＮＳＡＣ迭代，用三个非共线的点进行寻找内点数量最多的平面，也可以直接用ＰＣＬ中的平面拟合函数直接进行计算；这里采用第二种；

1. 在项目中包含PCL库

find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
target_link_libraries(your_exec_name ${PCL_LIBRARIES})

2. 将点云图像用于拟合

原本的点云图像的格式

pcl::PointCloud pcl_Stereocloud;

平面拟合之前需要先将点云的格式转换为指针类型，否则会报错；

pcl::PointCloud::Ptr stereoCloudptr = pcl_Stereocloud.makeShared();

接下来做平面拟合，PCL中的拟合也是采用的RANSAC的方法，先设定迭代的最大迭代次数，设定inliner判定的阈值，选定拟合模型，然后开始迭代；

pcl::PointCloud::Ptr ImageGrabber::PCAPlanFitting()
{
    pcl::PointCloud::Ptr stereoCloudptr = pcl_Stereocloud.makeShared();
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    //inliers表示误差能容忍的点 记录的是点云的序号
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    //创建分割器
	pcl::SACSegmentation seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(0.01);
	seg.setMaxIterations(500);
	seg.setAxis(Eigen::Vector3f(0, 0, 1));
	seg.setEpsAngle(45.0f * (M_PI / 180.0f));
	seg.setInputCloud(stereoCloudptr);
	seg.segment(*inliers, *coefficients);
	std::cerr << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << std::endl;
	std::cerr << "ax" << seg.getAxis() << std::endl;
	pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud);
    //只取inliners中索引对应的点拷贝到inlierPoints中
	pcl::copyPointCloud(*stereoCloudptr, *inliers, *inlierPoints);
    return inlierPoints;
}

接下来将Ptr转换成对应非指针型的点云类型即可；

  pcl::PointCloud::Ptr filteredpts(new pcl::PointCloud);
  filteredpts = PCAPlanFitting();
  pcl::PointCloud filtered_cloud;
  filtered_cloud = *filteredpts;

如果需要的话可以将点云 filtered_cloud 发布即可；

3.可能遇到的问题

另外，如果与ORBSLAM一起运行，会遇到优化时Eigen计算出错，这种问题是因为PCL的库的问题，更新到1.9的PCL即可；

REF:

1. PCL1.9安装

2. PCL1.10下载地址

3. PCL普通点云与指针点云之间的转换

4. copyPointCloud的使用

5. 三维点云投影到平面

6. 分别用点云和RSNSAC c++代码进行平面拟合 

7. PCA与平面拟合