ICP

gistfile1.txt

PointCloud::ConstPtr alignedPointClouds()
{
  PointCloud clouds_merged = *filterCloudByDepth(clouds[0]).get();

  for(int i=1; i < NUM_CAPTURES; i++) {
    if(clouds[i]) {
      pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
      icp.setMaxCorrespondenceDistance (0.5);
      icp.setTransformationEpsilon (1e-5);
      //icp.setEuclideanFitnessEpsilon (1);

      icp.setInputCloud(filterCloudByDepth(clouds[i]));
      icp.setInputTarget((new PointCloud(clouds_merged))->makeShared());

      PointCloud cloud_aligned;
      icp.align(cloud_aligned);

      clouds_merged += cloud_aligned;
    }
  }
  return clouds_merged.makeShared();
}