PCL最小二乘法拟合平面

PCL最小二乘法拟合平面

效果

过滤掉不属于拟合平面的点（点到平面距离处于阈值外的点）

原理参考

最小二分法拟合平面
过程推导如下

PCL实现

#include <QCoreApplication>
#include<opencv2\opencv.hpp>
#include<pcl/io/io.h>
#include<pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include<pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include<Eigen/Dense>
#include <dl_planefitting.h>


//点云可视化
void showCloud(std::string windowname,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer (windowname));
    viewer->setBackgroundColor (0.5, 0.5, 0.5);  //设置背景
    viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");  //显示点云
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");  //设置点尺寸
    viewer->addCoordinateSystem (100.0);  //设置坐标轴尺寸
//    while (!viewer->wasStopped ())
//    {
//      viewer->spinOnce (100);
//      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
//    }
    //cout<<"Point couting in "<<windowname<<": "<<cloud->size()<<endl;
}


// 算点到平面距离
//设平面方程为Ax+By+Cz+D=0
//则有A/D X+B/D Y+ C/D Z+1=0 即 a0/a2 X+ a1/a2 Y +-1/a2 Z +1=0
double getDistance(double a0,double a1,double a2,pcl::PointXYZ point){
    double A=a0/a2;
    double B=a1/a2;
    double C=-1/a2;
    double up = std::abs(A*point.x+B*point.y+C*point.z+1);
    double down = std::sqrt(std::pow(A,2)+std::pow(B,2)+std::pow(C,2));
    double dis = up/down;
    return dis;
}


//平面拟合算法
Eigen::Vector3d getFlat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    Eigen::Matrix3d rot;
    double x2,xy,x,y2,y,zx,zy,z;
    for(int i=0;i<cloud->points.size();i++){
        x2 += cloud->points[i].x * cloud->points[i].x;
        xy += cloud->points[i].x * cloud->points[i].y;
        x += cloud->points[i].x;
        y2 += cloud->points[i].y * cloud->points[i].y;
        y += cloud->points[i].y;
        zx += cloud->points[i].x * cloud->points[i].z;
        zy += cloud->points[i].y * cloud->points[i].z;
        z += cloud->points[i].z;
    }
    //为矩阵赋值
    rot<<x2,  xy,  x,
         xy,  y2,  y,
         x,   y,   cloud->points.size();
    //为列向量赋值
    Eigen::Vector3d eq(zx,zy,z);
    Eigen::Vector3d X = rot.colPivHouseholderQr().solve(eq);
    std::cout<<X<<std::endl;
    std::cout<<X[0]<<" "<<X[1]<<" "<<X[2]<<std::endl;
    return X;
}


int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Qt_Project\\flat.pcd",*cloud);
    showCloud("原点云",cloud);
    Eigen::Vector3d flat = getFlat(cloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr flatPoints(new pcl::PointCloud<pcl::PointXYZ>);
    int n=0;
    for (int i=0;i<cloud->points.size();i++){
        if(getDistance(flat[0],flat[1],flat[2],cloud->points[i])<0.15){  //阈值可调 
             flatPoints->points.push_back(cloud->points[i]);
        }
        else{
            //std::cout<<"dis:"<<getDistance(flat[0],flat[1],flat[2],cloud->points[i])<<std::endl;
        }
    }
    std::cout<<"原size:"<<cloud->size()<<std::endl;
    std::cout<<"平面size:"<<flatPoints->size()<<std::endl;
    showCloud("符合要求的平面",flatPoints);
    return a.exec();
}