00001 #include <iostream> 00002 #include <pcl/io/pcd_io.h> 00003 #include <pcl/point_types.h> 00004 #include <pcl/registration/icp.h> 00005 00006 int 00007 main (int argc, char** argv) 00008 { 00009 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>); 00010 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>); 00011 00012 // Fill in the CloudIn data 00013 cloud_in->width = 5; 00014 cloud_in->height = 1; 00015 cloud_in->is_dense = false; 00016 cloud_in->points.resize (cloud_in->width * cloud_in->height); 00017 for (size_t i = 0; i < cloud_in->points.size (); ++i) 00018 { 00019 cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); 00020 cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); 00021 cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); 00022 } 00023 std::cout << "Saved " << cloud_in->points.size () << " data points to input:" 00024 << std::endl; 00025 for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " << 00026 cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << 00027 cloud_in->points[i].z << std::endl; 00028 *cloud_out = *cloud_in; 00029 std::cout << "size:" << cloud_out->points.size() << std::endl; 00030 for (size_t i = 0; i < cloud_in->points.size (); ++i) 00031 cloud_out->points[i].x = cloud_in->points[i].x + 0.7f; 00032 std::cout << "Transformed " << cloud_in->points.size () << " data points:" 00033 << std::endl; 00034 for (size_t i = 0; i < cloud_out->points.size (); ++i) 00035 std::cout << " " << cloud_out->points[i].x << " " << 00036 cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl; 00037 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 00038 icp.setInputCloud(cloud_in); 00039 icp.setInputTarget(cloud_out); 00040 pcl::PointCloud<pcl::PointXYZ> Final; 00041 icp.align(Final); 00042 std::cout << "has converged:" << icp.hasConverged() << " score: " << 00043 icp.getFitnessScore() << std::endl; 00044 std::cout << icp.getFinalTransformation() << std::endl; 00045 00046 return (0); 00047 }