PCL 1.8.1 ICP 点云配准示例

#include #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/registration/icp.h> #include <boost/thread/thread.hpp> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/console/time.h>

int main(int argc, char** argv) { pcl::console::TicToc time; pcl::PointCloudpcl::PointXYZ::Ptr source(new pcl::PointCloudpcl::PointXYZ); pcl::io::loadPCDFilepcl::PointXYZ('D:\点云文件\雕像1.pcd', *source); std::cout << "从源点云中读取 " << source->size() << " 个点" << std::endl;

pcl::PointCloudpcl::PointXYZ::Ptr target(new pcl::PointCloudpcl::PointXYZ); pcl::io::loadPCDFilepcl::PointXYZ('D:\点云文件\雕像2.pcd', *target); std::cout << "从目标点云中读取 " << target->size() << " 个点" << std::endl;

time.tic(); pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; icp.setInputSource(source); icp.setInputTarget(target); icp.setTransformationEpsilon(1e-10); icp.setMaxCorrespondenceDistance(1); icp.setEuclideanFitnessEpsilon(0.001); icp.setMaximumIterations(35); icp.setUseReciprocalCorrespondences(true); pcl::PointCloudpcl::PointXYZ::Ptr icp_cloud(new pcl::PointCloudpcl::PointXYZ); icp.align(*icp_cloud); std::cout << "Applied " << 35 << " ICP iterations in " << time.toc() << " ms" << std::endl; std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl; std::cout << "变换矩阵：\n" << icp.getFinalTransformation() << std::endl; pcl::transformPointCloud(*source, *icp_cloud, icp.getFinalTransformation());

boost::shared_ptrpcl::visualization::PCLVisualizer viewer(new pcl::visualization::PCLVisualizer('ICP配准结果')); viewer->setBackgroundColor(0, 0, 0); pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ target_color(target, 255, 0, 0); viewer->addPointCloudpcl::PointXYZ(target, target_color, "target cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ source_color(source, 0, 0, 255); viewer->addPointCloudpcl::PointXYZ(source, source_color, "source cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source cloud"); pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ icp_color(icp_cloud, 0, 255, 0); viewer->addPointCloudpcl::PointXYZ(icp_cloud, icp_color, "icp cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "icp cloud"); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); }

return 0; }