PCL裁剪之多边形裁剪

     PCL自带的裁剪方法,本人就不作介绍了,具体详见本人之间的博客https://www.cnblogs.com/z-web-2017/p/10187556.html,此处主要是对之前裁剪方法存在的不足进行完善,加入视点方向进行投影。保证相机转动时,投影方向随之转动:

具体代码如下:

 

    pcl::visualization::Camera camera1;
    viewer->getCameraParameters(camera1);
    PointXYZ eyeLine1 = PointXYZ(camera1.focal[0] - camera1.pos[0],
        camera1.focal[1] - camera1.pos[1], camera1.focal[2] - camera1.pos[2]);
    float mochang = sqrt(pow(eyeLine1.x, 2) + pow(eyeLine1.y, 2) + pow(eyeLine1.z, 2));
    PointXYZ eyeLine = PointXYZ(eyeLine1.x / mochang, eyeLine1.y / mochang, eyeLine1.z / mochang);

    PointCloud<PointXYZRGB>::Ptr cloudIn_Prj(new PointCloud<PointXYZRGB>);
    PointCloud<PointXYZRGB>::Ptr cloudCiecle_result(new PointCloud<PointXYZRGB>);
    ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());//ax+by+cz+d=0
    coefficients->values.resize(4);
    coefficients->values[0] = eyeLine.x;
    coefficients->values[1] = eyeLine.y;
    coefficients->values[2] = eyeLine.z;
    coefficients->values[3] = 0;
    // 创建滤波器对象
    ProjectInliers<PointXYZRGB> proj;//建立投影对象
    proj.setModelType(pcl::SACMODEL_PLANE);//设置投影类型
    proj.setInputCloud(cloudPoly);//设置输入点云
    proj.setModelCoefficients(coefficients);//加载投影参数
    proj.filter(*cloudCiecle_result);//执行程序,并将结果保存

    // 创建滤波器对象
    ProjectInliers<PointXYZRGB> projCloudIn;//建立投影对象
    projCloudIn.setModelType(pcl::SACMODEL_PLANE);//设置投影类型
    projCloudIn.setInputCloud(cloudIn);//设置输入点云
    projCloudIn.setModelCoefficients(coefficients);//加载投影参数
    projCloudIn.filter(*cloudIn_Prj);//执行程序,并将结果保存
    
    int ret;
    double *PloyXarr = new double[cloudCiecle_result->points.size()];
    double *PloyYarr = new double[cloudCiecle_result->points.size()];
    for (int i = 0; i < cloudCiecle_result->points.size(); i++)
    {
        PloyXarr[i] = cloudCiecle_result->points[i].x;
        PloyYarr[i] = cloudCiecle_result->points[i].y;
    }


    for (int i = 0; i < cloudIn_Prj->points.size(); i++)
    {
        ret = inOrNot1(cloudPoly->points.size(), PloyXarr, PloyYarr, cloudIn_Prj->points[i].x, cloudIn_Prj->points[i].y);
        if (1 == ret)//表示在里面
        {
            cloudRe->points.push_back(cloudIn->points[i]);
        }//表示在外面
    }

int inOrNot1(int poly_sides, double *poly_X, double *poly_Y, double x, double y)
{
 int i, j;
 j = poly_sides - 1;
 int res = 0;
 for (i = 0; i < poly_sides; i++)
 {
  //对每一条边进行遍历,该边的两个端点,有一个必须在待检测点(x,y)的左边,且两个点中,有一个点的y左边比p.y小,另一个点的y比p.y大。
  if ((poly_Y[i] < y && poly_Y[j] >= y || poly_Y[j] < y && poly_Y[i] >= y) && (poly_X[i] <= x || poly_X[j] <= x))
  {
   //用水平的直线与该边相交,求交点的x坐标。
   res ^= ((poly_X[i] + (y - poly_Y[i]) / (poly_Y[j] - poly_Y[i])  *(poly_X[j] - poly_X[i])) < x);
  }
  j = i;
 }
 return res;
}

 

猜你喜欢

转载自www.cnblogs.com/z-web-2017/p/10923985.html
0条评论
添加一条新回复