gistfile1.txt

#include <stdlib.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>

void viewerOneOff( pcl::visualization::PCLVisualizer& viewer )
{
	//viewer.setBackgroundColor (0.1, 0.1, 0.35);    
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2,"cloud_final");
	//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2,"cloud_in");
	//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2,"cloud_out");
}
//
// prog.exe numpoints divisor_for_partial_match pcd_file
//
int main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_final( new pcl::PointCloud<pcl::PointXYZRGB> );
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr result( new pcl::PointCloud<pcl::PointXYZRGB> );
  if ( argc>3 ) 
  {
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(argv[3], *cloud_in) == -1)
	{
		PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
		return (-1);
	}
  } else {

	  // Fill in the CloudIn data
	  cloud_in->width    = argc>1 ? atoi(argv[1]) : 1000;
	  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);
		cloud_in->points[i].b = 255;
	  }
  }
  for (size_t i = 0; i < 10; ++i)
    std::cout << "    " << cloud_in->points[i].x << " " <<
      cloud_in->points[i].y << " " << cloud_in->points[i].z << std::endl;

  cloud_out->width    = cloud_in->width / (argc>2 ? atoi(argv[2]) : 1) ;
  cloud_out->height   = 1;
  cloud_out->is_dense = false;
  cloud_out->points.resize (cloud_out->width * cloud_out->height);
  std::cout << "Saved " << cloud_out->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;
  std::cout << "size:" << cloud_out->points.size() << std::endl;
  for (size_t i = 0; i < cloud_out->points.size(); ++i) {
    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
    cloud_out->points[i].y = cloud_in->points[i].y + 0.7f;
    cloud_out->points[i].z = cloud_in->points[i].z + 0.7f;
    cloud_out->points[i].r = 255;
  }
  std::cout << "Transformed " << cloud_out->points.size () << " data points:"
      << std::endl;
  for (size_t i = 0; i < 10; ++i)
    std::cout << "    " << cloud_out->points[i].x << " " <<
      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  icp.setInputCloud(cloud_in);
  icp.setInputTarget(cloud_out);
  icp.align(*cloud_final);
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;

  for (size_t i = 0; i < cloud_final->points.size(); ++i) {
    cloud_final->points[i].g = 255;
    cloud_final->points[i].r = 0;
    cloud_final->points[i].b = 0;
	if ( i < 100  )
      std::cout << cloud_final->points[i].x << " " << cloud_final->points[i].y << " " <<  cloud_final->points[i].z << std::endl;
  }
  std::cout << "Final " << cloud_final->points.size () << " "  << cloud_final->width << cloud_final->height << std::endl;

  for (size_t i = 0; i < cloud_in->points.size(); ++i) {
    cloud_final->points.push_back( cloud_in->points[i] );
	cloud_final->width ++;
  }
  for (size_t i = 0; i < cloud_out->points.size(); ++i) {
    cloud_final->points.push_back( cloud_out->points[i] );
	cloud_final->width ++;
  }
  std::cout << "Final " << cloud_final->points.size () << " "  << cloud_final->width << cloud_final->height << 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;
  //result->insert( result->begin(), cloud_in->begin(), cloud_in->end() );
  //result->insert( result->begin(), cloud_out->begin(), cloud_in->end() );
  //result->insert( result->begin(), cloud_final->begin(), cloud_final->end() );

	pcl::visualization::CloudViewer viewer ("reconstruction");

	viewer.runOnVisualizationThreadOnce (viewerOneOff);
	//viewer.runOnVisualizationThread(viewerOnFrame);
	viewer.showCloud (cloud_final,  "cloud_final" );
	//viewer.showCloud (cloud_out, "cloud_out" );
	//viewer.showCloud (cloud_in,  "cloud_in" );
	while (!viewer.wasStopped ())
	{
	}
 return (0);
}