沿着法向量的方向投影到某一平面pcl

1,先求法向量,利用computePointNormal得到的前三个参数就是法向量(a,b,c)

2,选定一点,这个点是用来定位平面的,通过这个点求出平面ax+by+cz+d=0的d

3,将点云沿着法向量的方向投影到选择的平面上

4,可视化一下

部分代码如下:

1,先求法向量,利用computePointNormal得到的前三个参数就是法向量(a,b,c)

    pcl::console::TicToc time; time.tic();    
    CP cloudAfter(new CloudT);

    int cloud_size = cloudIn->size();
    Vector4f planePara;
    float wuyong;
    vector<int> indices(cloud_size);

    #pragma omp parallel for
    for (int i = 0; i < cloudIn->size(); i++)
    {
        indices[i] = i;
    }
    computePointNormal(*cloudIn, indices, planePara, wuyong);

2,选定一点,这个点是用来定位平面的,通过这个点求出平面ax+by+cz+d=0的d

    double x0 = cloudIn->points[int(cloud_size / 2)].x;
    double y0 = cloudIn->points[int(cloud_size / 2)].y;
    double z0 = cloudIn->points[int(cloud_size / 2)].z;
    double A = planePara(0);    
    double B = planePara(1);
    double C = planePara(2);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    coefficients->values.resize(4);
    coefficients->values[0] = A;
    coefficients->values[1] = B;
    coefficients->values[2] = C;
    coefficients->values[3] = -(A * x0 + B * y0 + C * z0);

3,将点云沿着法向量的方向投影到选择的平面上

    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(cloudIn);
    proj.setModelCoefficients(coefficients);
    proj.filter(*cloudAfter);
    cout << "投影时间:" << time.toc() << "ms" << endl;

4,可视化一下

    pcl::visualization::PCLVisualizer viewer;
    viewer.setBackgroundColor(0.05, 0.05, 0.05);
    viewer.addPointCloud(cloudIn,             pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudIn, 255, 0, 0), "cloudIn");
    viewer.addPointCloud(cloudAfter, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudAfter, 0, 255, 0), "cloudAfterNormal");
    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    cloudIn = cloudAfter;

    normal_plane.emplace_back(A);
    normal_plane.emplace_back(B);
    normal_plane.emplace_back(C);
    normal_plane.emplace_back(coefficients->values[3]);