2024-9-8 实现pcl平面投影以及圆的拟合

最新推荐文章于 2024-09-25 17:35:13 发布

诗赋码上叨

最新推荐文章于 2024-09-25 17:35:13 发布

阅读量248

点赞数 7

文章标签： 3d c++ 算法目标检测

本文链接：https://blog.csdn.net/weixin_65180827/article/details/141979267

版权

目前遇到一个问题，如何从深度图像中，显示出来点到目标平面的距离。

rangeimage是来自传感器一个特定角度拍摄的一个三维场景获取的有规则的有焦距等基本信息的深度图。

需要找一个特定的角度，这个角度怎么得到？

在寻找资料的过程中，意外发现了根据点云图像拟合圆形的例子，对此想仿照实现圆形的拟合和识别，用于取代摄像头。

main.cpp

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_circle.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;

int main()
{
        //------------------------------- 加载点云 -------------------------------
        cout << "->正在加载点云..." << endl;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        if (pcl::io::loadPCDFile("plane_project.pcd", *cloud) < 0)
        {
                PCL_ERROR("\a点云文件不存在！\n");
                system("pause");
                return -1;
        }
        cout << "->加载点的个数：" << cloud->points.size() << endl;
        //========================================================================

        //------------------------------- 模型估计 -------------------------------
        cout << "->正在估计二维圆..." << endl;
        pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr model_circle2D(new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>(cloud));	//选择拟合点云与几何模型
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_circle2D);	//创建随机采样一致性对象
        ransac.setDistanceThreshold(0.01);	//设置距离阈值，与模型距离小于0.01的点作为内点
        ransac.setMaxIterations(10000);		//设置最大迭代次数
        ransac.computeModel();				//执行模型估计

        vector<int> inliers;				//存储内点索引的向量
        ransac.getInliers(inliers);			//提取内点对应的索引

        /// 根据索引提取内点
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_circle(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_circle);

        /// 输出模型参数(x-x0)^2 + (y-y0)^2 = r^2;
        Eigen::VectorXf coefficient;
        ransac.getModelCoefficients(coefficient);
        cout << "二维圆方程为：\n"
                <<"(x - "<< coefficient[0] << ")^2 + "
                <<"(y - "<< coefficient[1] << ")^2 = "
                << coefficient[2] <<"^2"
                << endl;
        //========================================================================

        //---------------------------- 可视化结果（选） ---------------------------
        pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));

        viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");													//添加原始点云
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");	//颜色
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");	//点的大小

        viewer->addPointCloud<pcl::PointXYZ>(cloud_circle, "circle");											//添加拟合点云
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "circle");	//颜色
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "circle");	//点的大小

        while (!viewer->wasStopped())
        {
                viewer->spinOnce(100);
                boost::this_thread::sleep(boost::posix_time::microseconds(100000));
        }
        //========================================================================

        return 0;
}

CmakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(fit_circle)

find_package(PCL 1.3 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(fit_circle main.cpp)
target_link_libraries(fit_circle ${PCL_LIBRARIES})

将已经滤波后的三维图进行圆拟合，得到的结果并不理想。为此，想到先将目标点云先做一个投影，在投影平面上再进行圆的拟合。投影的代码如下：

main.cpp

#include <pcl/io/pcd_io.h>
#include <pcl/ModelCoefficients.h>			//模型系数定义头文件
#include <pcl/filters/project_inliers.h>	//投影滤波类头文件

using namespace std;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main()
{
	//---------------------------------- 加载点云 ----------------------------------
	cout << "->正在加载点云..." << endl;
	PointCloudT::Ptr cloud(new PointCloudT);	
        if (pcl::io::loadPCDFile("output_ranges.pcd", *cloud) < 0)
	{
		PCL_ERROR("\a->点云文件不存在！\n");
		return -1;
	}
	cout << "->加载点云的点数：" << cloud->points.size() << endl;
	//================================== 加载点云 ==================================

	//------------------- 创建平面模型 ax + by + cz + d = 0 ------------------------
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	coefficients->values.resize(4);
	coefficients->values[0] = 1.0;
	coefficients->values[1] = 1.0;
	coefficients->values[2] = 1.0;
	coefficients->values[3] = 0;
	//=================== 创建平面模型 ax + by + cz + d = 0 ========================

	//-------------------------------- 执行投影滤波 --------------------------------
	PointCloudT::Ptr cloud_projected(new PointCloudT);
	pcl::ProjectInliers<pcl::PointXYZ> proj;
	proj.setModelType(pcl::SACMODEL_PLANE);
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(coefficients);
	proj.filter(*cloud_projected);
	//================================ 执行投影滤波 ================================

	//保存滤波点云
	if (!cloud_projected->empty())
	{
		pcl::io::savePCDFileBinary("plane_project.pcd", *cloud_projected);
	}
	else
	{
		PCL_ERROR("\a->投影滤波点云为空！\n");
	}

	return 0;
}

CmakaLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(tou_ying)

find_package(PCL 1.3 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(tou_ying main.cpp)
target_link_libraries(tou_ying ${PCL_LIBRARIES})

首先投影后得到的图像如下：

可能这个滤波做的不是很好，也可能投影不可避免造成的干扰。在这个图像的基础上，继续进行圆的拟合。最后得到的图像（红色为拟合圆）以及拟合的圆的方程如上。还是有点喜感的哈哈哈，用到工程上肯定不行。