点云icp配准

最新推荐文章于 2024-08-25 16:09:35 发布

tunan11

最新推荐文章于 2024-08-25 16:09:35 发布

阅读量173

点赞数

分类专栏：点云配准文章标签： c++

本文链接：https://blog.csdn.net/naonao8986421/article/details/116304873

版权

点云配准专栏收录该内容

1 篇文章 0 订阅

订阅专栏

该博客介绍了如何利用PCL库进行点云配准，通过实例展示了ICP（Iterative Closest Point）算法的应用。首先加载两个点云文件，然后使用ICP进行配准，最后将原始、目标和配准后的点云进行三维可视化展示，并将结果保存为新的点云文件。

摘要由CSDN通过智能技术生成

点云ICP配准并将其可视化

代码

#define BOOST_TYPEOF_EMULATION
#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>//icp头文件
#include <pcl/visualization/pcl_visualizer.h>


using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;


void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h(pcd_src, 0, 255, 0);
	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h(pcd_tgt, 255, 0, 0);
	//匹配好的点云蓝色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> final_h(pcd_final, 0, 0, 255);

	viewer.setBackgroundColor(255, 255,255);
	viewer.addPointCloud(pcd_src, src_h, "source cloud");
	viewer.addPointCloud(pcd_tgt, tgt_h, "target cloud");
	viewer.addPointCloud(pcd_final, final_h, "result cloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

}

int main(int argc, char** argv)
{
	pcl::PointCloud<PointT>::Ptr cloud_source(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_target(new pcl::PointCloud<PointT>);

	pcl::io::loadPCDFile("C:\\Users\\yue\\Desktop\\groundtruth.pcd", *cloud_target);
	pcl::io::loadPCDFile("C:\\Users\\yue\\Desktop\\KeyFrameTrajectory.pcd", *cloud_source);

	pcl::IterativeClosestPoint<PointT, PointT> icp;

	icp.setInputCloud(cloud_source);
	icp.setInputTarget(cloud_target);
	
	PointCloud::Ptr Final(new PointCloud);
	icp.align(*Final);
	std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;
	//输出刚体变换矩阵信息
	std::cout << icp.getFinalTransformation() << std::endl;
	//可视化
	visualize_pcd(cloud_source, cloud_target, Final);
	//保存变换后的点云 pcd文件
	pcl::io::savePCDFileASCII("C:\\Users\\yue\\Desktop\\test_pcd.pcd", *Final);

	return (0);
}