ProjectInliers投影滤波实现

给算法爸爸上香

已于 2022-06-08 21:14:10 修改

阅读量304

点赞数 1

分类专栏： PCL 文章标签：算法计算机视觉

于 2021-06-05 21:35:09 首次发布

本文链接：https://blog.csdn.net/taifyang/article/details/117606141

版权

PCL 专栏收录该内容

26 篇文章 69 订阅

订阅专栏

实现：以平面模型投影为例在这里插入图片描述

/**
 * @description:			点云投影
 * @param cloud				输入点云
 * @param cloud_projected	投影点云
 * @param coefficients		投影平面方程系数
 */
void projection(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_projected, pcl::ModelCoefficients::Ptr coefficients)
{
	float a = coefficients->values[0], b = coefficients->values[1], c = coefficients->values[2], d = coefficients->values[3];
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		float x0 = cloud->points[i].x;
		float y0 = cloud->points[i].y;
		float z0 = cloud->points[i].z;
		float xp = ((b*b + c*c)*x0 - a*(b*y0 + c*z0 + d)) / (a*a + b*b + c*c);
		float yp = ((a*a + c*c)*y0 - b*(a*x0 + c*z0 + d)) / (a*a + b*b + c*c);
		float zp = ((a*a + b*b)*z0 - c*(a*x0 + b*y0 + d)) / (a*a + b*b + c*c);
		cloud_projected->push_back(pcl::PointXYZ(xp, yp, zp));
	}
}