iterative_closest_point.cpp
Go to the documentation of this file.
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 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:31