ProjectInliers投影滤波实现

原创已于 2022-06-08 21:14:10 修改 · 368 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #计算机视觉

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

PCL 专栏收录该内容

26 篇文章

订阅专栏

该代码实现了一个将点云数据投影到平面上的算法。通过输入点云、投影后的点云和平面方程系数，利用投影公式计算每个点的新坐标，并存储到投影点云中。该过程对于三维点云处理和可视化具有重要意义。

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

/**
 * @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));
	}
}