Exemple #1
0
void ICP( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 )
{
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
	///////////////////ICP - REGISTRATION ////////////////
	 	

 LOGI("Start ICP...%d,%d,%d,%d",cloud1->width,cloud1->height,cloud2->width,cloud2->height);

	MyReg* mr=new MyReg();
	PointCloud::Ptr src (new PointCloud(mr->width_ds,mr->height_ds));
	PointCloud::Ptr tgt (new PointCloud(mr->width_ds,mr->height_ds));
	LOGI("Start Downsampling...");	
	mr->DownSampling(cloud1,cloud2,src,tgt);
      
	pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src (new pcl::PointCloud<PointNormalT>);
	pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt (new pcl::PointCloud<PointNormalT>);
	LOGI("Start Normals estimation...");	
	mr->Normals(points_with_normals_src,points_with_normals_tgt,src,tgt);
	
	LOGI("Start Matrix estimation...");
	Eigen::Matrix4f GlobalTransf;
	mr->MatrixEstimation(points_with_normals_src, points_with_normals_tgt, GlobalTransf);

		string outputstr;
		ostringstream out_message;
		out_message << GlobalTransf;
		outputstr=out_message.str();
		LOGI("%s", outputstr.c_str());

	PointCloud::Ptr transf (new PointCloud);
    // 	pcl::transformPointCloud (*cloud1, *transf,  GlobalTransf);
	pcl::transformPointCloud (*src, *transf,  GlobalTransf);

	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
	LOGI("Time of registration: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
    //  LOGI("Transform");
    //	std::cout<< GlobalTransf <<endl;
    //  LOGI("point-size: %d", transf->points.size());


/*ShowPCLtoKiwi(cloud1,255,0,0,-1.5);
ShowPCLtoKiwi(src,0,255,0,-1.6);
//ShowPCLtoKiwi(cloud2,0,255,0,1.5);
*/

ShowPCLtoKiwi(src,255,0,0,-1.5);
ShowPCLtoKiwi(tgt,0,255,0,-1.5);

ShowPCLtoKiwi(transf,255,0,0,1.5);
ShowPCLtoKiwi(tgt,0,255,0,1.5);


}
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
  //
  // Downsample for consistency and speed
  // \note enable this for large datasets
  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);
  pcl::VoxelGrid<PointT> grid;
  if (downsample)
  {
    grid.setLeafSize (0.05, 0.05, 0.05);
    grid.setInputCloud (cloud_src);
    grid.filter (*src);

    grid.setInputCloud (cloud_tgt);
    grid.filter (*tgt);
  }
  else
  {
    src = cloud_src;
    tgt = cloud_tgt;
  }

  // Compute surface normals and curvature
  PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
  PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);

  pcl::NormalEstimation<PointT, PointNormalT> norm_est;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  norm_est.setSearchMethod (tree);
  norm_est.setKSearch (30);
  
  norm_est.setInputCloud (src);
  norm_est.compute (*points_with_normals_src);
  pcl::copyPointCloud (*src, *points_with_normals_src);

  norm_est.setInputCloud (tgt);
  norm_est.compute (*points_with_normals_tgt);
  pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

  //
  // Instantiate our custom point representation (defined above) ...
  MyPointRepresentation point_representation;
  // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
  float alpha[4] = {1.0, 1.0, 1.0, 1.0};
  point_representation.setRescaleValues (alpha);

  //
  // Align
  pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
  reg.setTransformationEpsilon (1e-6);
  // Set the maximum distance between two correspondences (src<->tgt) to 10cm
  // Note: adjust this based on the size of your datasets
  reg.setMaxCorrespondenceDistance (0.1);  
  // Set the point representation
  reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));

  reg.setInputCloud (points_with_normals_src);
  reg.setInputTarget (points_with_normals_tgt);



  //
  // Run the same optimization in a loop and visualize the results
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
  PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
  reg.setMaximumIterations (2);
  for (int i = 0; i < 30; ++i)
  {
    PCL_INFO ("Iteration Nr. %d.\n", i);

    // save cloud for visualization purpose
    points_with_normals_src = reg_result;

    // Estimate
    reg.setInputCloud (points_with_normals_src);
    reg.align (*reg_result);

		//accumulate transformation between each Iteration
    Ti = reg.getFinalTransformation () * Ti;

		//if the difference between this transformation and the previous one
		//is smaller than the threshold, refine the process by reducing
		//the maximal correspondence distance
    if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
    
    prev = reg.getLastIncrementalTransformation ();

    // visualize current state
    showCloudsRight(points_with_normals_tgt, points_with_normals_src);
  }

	//
  // Get the transformation from target to source
  targetToSource = Ti.inverse();

  //
  // Transform target back in source frame
  pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);

  p->removePointCloud ("source");
  p->removePointCloud ("target");

  PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
  PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);
  p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
  p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);

	PCL_INFO ("Press q to continue the registration.\n");
  p->spin ();

  p->removePointCloud ("source"); 
  p->removePointCloud ("target");

  //add the source to the transformed target
  *output += *cloud_src;
  
  final_transform = targetToSource;
}
Exemple #3
0
/** registration two clouds using ICP with projective correspondence */
void ICP()
{
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);


  if (pcl::io::loadPCDFile<pcl::PointXYZ> (file1, *cloud1) == -1) //* load the file
  {
    LOGI("Couldn't read file1");
    return;
  }
  LOGI("Loaded %d data points from file2",cloud1->width * cloud1->height);

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (file2, *cloud2) == -1) //* load the file
  {
    LOGI("Couldn't read file2");
    return;
  }
  LOGI("Loaded %d data points from file2",cloud1->width * cloud1->height);


	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
	
	///////////////////ICP - REGISTRATION ////////////////
	MyReg* mr=new MyReg();
	PointCloud::Ptr src (new PointCloud(mr->width_ds,mr->height_ds));
	PointCloud::Ptr tgt (new PointCloud(mr->width_ds,mr->height_ds));
	mr->DownSampling(cloud1,cloud2,src,tgt);
        LOGI("Start Downsampling...");
	pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src (new pcl::PointCloud<PointNormalT>);
	pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt (new pcl::PointCloud<PointNormalT>);
	LOGI("Start Normals estimation...");	
	mr->Normals(points_with_normals_src,points_with_normals_tgt,src,tgt);
	
	LOGI("Start Matrix estimation...");
	Eigen::Matrix4f GlobalTransf;
	mr->MatrixEstimation(points_with_normals_src, points_with_normals_tgt, GlobalTransf);

		string outputstr;
		ostringstream out_message;
		out_message << GlobalTransf;
		outputstr=out_message.str();
		LOGI("%s", outputstr.c_str());

	PointCloud::Ptr transf (new PointCloud);
    // 	pcl::transformPointCloud (*cloud1, *transf,  GlobalTransf);
	pcl::transformPointCloud (*src, *transf,  GlobalTransf);

	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
	LOGI("Time of registration: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
    //  LOGI("Transform");
    //	std::cout<< GlobalTransf <<endl;
    //  LOGI("point-size: %d", transf->points.size());


ShowPCLtoKiwi(src,255,0,0,-1.5);
ShowPCLtoKiwi(tgt,0,255,0,-1.5);

ShowPCLtoKiwi(transf,255,0,0,1.5);
ShowPCLtoKiwi(tgt,0,255,0,1.5);

}
void pairAlign( const PointCloud::Ptr cloud_src , const PointCloud::Ptr cloud_tgt , Eigen::Matrix4f &final_transform )
{

    PointCloud::Ptr src (new PointCloud);
    PointCloud::Ptr tgt (new PointCloud);

    *src = *cloud_src;
    *tgt = *cloud_tgt;

    // Compute surface normals and curvature
    PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
    PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);

    pcl::NormalEstimation<PointT, PointNormalT> norm_est;
    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
    norm_est.setSearchMethod (tree);
    norm_est.setKSearch (30);

    norm_est.setInputCloud (src);
    norm_est.compute (*points_with_normals_src);
    pcl::copyPointCloud (*src, *points_with_normals_src);

    norm_est.setInputCloud (tgt);
    norm_est.compute (*points_with_normals_tgt);
    pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

    // Instantiate our custom point representation (defined above) ...
    MyPointRepresentation point_representation;
    // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
    float alpha[4] = {1.0, 1.0, 1.0, 1.0};
    point_representation.setRescaleValues (alpha);

    //
    // Align
    pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
    reg.setTransformationEpsilon (1e-3);
    // Set the maximum distance between two correspondences (src<->tgt) to 10cm
    // Note: adjust this based on the size of your datasets
    reg.setMaxCorrespondenceDistance (0.5);
    // Set the point representation
    reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));

    reg.setInputSource (points_with_normals_src);
    reg.setInputTarget (points_with_normals_tgt);



    //
    // Run the same optimization in a loop and visualize the results
    Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
    PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
    reg.setMaximumIterations (2);

    //originally iterates up to 30
    for (int i = 0; i < 10; ++i)
    {

        // save cloud for visualization purpose
        points_with_normals_src = reg_result;

        // Estimate
        reg.setInputSource (points_with_normals_src);
        reg.align (*reg_result);

            //accumulate transformation between each Iteration
        Ti = reg.getFinalTransformation () * Ti;

            //if the difference between this transformation and the previous one
            //is smaller than the threshold, refine the process by reducing
            //the maximal correspondence distance
        if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
        {
            reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
        }
        prev = reg.getLastIncrementalTransformation ();

    }

    //
    // Get the transformation from target to source
    targetToSource = Ti.inverse();

    final_transform = targetToSource;

}