PCL：ICP配准

#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/registration/icp.h> int main(int argc, char** argv) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); //源点云 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);//目标点云 // 填入点云数据 cloud_in->width = 5; cloud_in->height = 1; cloud_in->is_dense = false; cloud_in->points.resize(cloud_in->width * cloud_in->height); for (size_t i = 0; i < cloud_in->points.size(); ++i) { cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f); cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f); cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); } std::cout << "Saved " << cloud_in->points.size() << " data points to input:" << std::endl; for (size_t i = 0; i < cloud_in->points.size(); ++i) std::cout << " " << cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << cloud_in->points[i].z << std::endl; *cloud_out = *cloud_in; std::cout << "size:" << cloud_out->points.size() << std::endl; for (size_t i = 0; i < cloud_in->points.size(); ++i) cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;//源点云在x方向平移0.7构造目标点云 std::cout << "Transformed " << cloud_in->points.size() << " data points:" << std::endl; for (size_t i = 0; i < cloud_out->points.size(); ++i) std::cout << " " << cloud_out->points[i].x << " " << cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl; pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; icp.setInputSource(cloud_in); //设置源点云 icp.setInputTarget(cloud_out); //设置目标点云 pcl::PointCloud<pcl::PointXYZ> Final;//存储经过配准变换源点云后的点云 icp.align(Final); //执行匹配存储变换后的源点云到Final std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; std::cout << icp.getFinalTransformation() << std::endl; system("pause"); return (0); }