PCL自带的裁剪方法,本人就不作介绍了,具体详见本人之间的博客,此处主要是对之前裁剪方法存在的不足进行完善,加入视点方向进行投影。保证相机转动时,投影方向随之转动:
具体代码如下:
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;
}