Build a map with PCL

classic Classic list List threaded Threaded
5 messages Options
Reply | Threaded
Open this post in threaded view
|

Build a map with PCL

fedex8989
Hi , i would like to build a map with a sequence of point clouds. My code works in that way: it finds the sift keypoint in 2 point clouds , successively finds the rototranslation matrix between them with icp and adds the resultant point cloud and the global trasformation to a list of models.
Now, i don't know how build a sequence of point clouds using my rototranslation matrix.
Please help!!



typedef pcl::PointXYZ PointT;
typedef pcl::PointXYZRGB PointTC;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointCloud<PointTC> PointCloudColor;

// This is a tutorial so we can afford having global variables
        //our visualizer
        pcl::visualization::PCLVisualizer *p;


struct PCD
{
  PointCloudColor::Ptr cloudD;
  std::string f_name;

  PCD() : cloudD (new PointCloudColor) {};
};



////////////////////////////////////////////////////////////////////////////////
/** \brief Load a set of PCD files that we want to register together
  * \param argc the number of arguments (pass from main ())
  * \param argv the actual command line arguments (pass from main ())
  * \param models the resultant vector of point cloud datasets
  */
void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models)
{
  std::string extension (".pcd");
  // Suppose the first argument is the actual test model
  for (int i = 1; i < argc; i++)
  {
    std::string fname = std::string (argv[i]);
    // Needs to be at least 5: .plot
    if (fname.size () <= extension.size ())
      continue;

    std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);

    //check that the argument is a pcd file
    if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0)
    {
      // Load the cloud and saves it into the global list of models
      PCD m;
      m.f_name = argv[i];
      pcl::io::loadPCDFile (argv[i], *m.cloudD);
      //remove NAN points from the cloud
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*m.cloudD,*m.cloudD, indices);

      models.push_back (m);
    }
  }
}


////////////////////////////////////////////////////////////////////////////////
/** \brief Align a pair of PointCloud datasets and return the result
  * \param cloud_src the source PointCloud
  * \param cloud_tgt the target PointCloud
  * \param output the resultant aligned source PointCloud
  * \param final_transform the resultant transform between source and target
  */
void pairAlign (const PointCloudColor::Ptr cloud_src, const PointCloudColor::Ptr cloud_tgt, PointCloudColor::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
  //
  // Downsample for consistency and speed
  PointCloudColor::Ptr src (new PointCloudColor);
  PointCloudColor::Ptr tgt (new PointCloudColor);
  pcl::VoxelGrid<pcl::PointXYZRGB> 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 SIFT FEATURES-----------//
 
   const float min_scale = 0.1f;
   const int n_octaves = 6;
  const int n_scales_per_octave = 10;
  const float min_contrast = 0.5f;

   // Estimate the sift interest points using Intensity values from RGB values
  pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale> sift;
  pcl::PointCloud<pcl::PointWithScale> result, result2;
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
  sift.setSearchMethod(tree);
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);
  sift.setMinimumContrast(min_contrast);
  sift.setInputCloud(src);
  sift.compute(result);
  sift.setInputCloud(tgt);
  sift.compute(result2);

 

    // Copying the pointwithscale to pointxyz so as visualize the cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp2 (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result2, *cloud_temp2);

  std::cout<< cloud_temp <<endl;

  std::cout<< cloud_temp2 <<endl;

  //
  // 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::IterativeClosestPoint<PointT, PointT> reg;
  // 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);  

  reg.setInputCloud (cloud_temp);
  reg.setInputTarget (cloud_temp2);



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

    // save cloud for visualization purpose
    cloud_temp = reg_result;

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

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

                   
    prev = reg.getLastIncrementalTransformation ();

  }

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

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovoutput (new pcl::PointCloud<pcl::PointXYZ>);
 
  copyPointCloud(*output, *cloudprovoutput);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovtarget (new pcl::PointCloud<pcl::PointXYZ>);

  copyPointCloud(*cloud_tgt, *cloudprovtarget);

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

   //add the source to the transformed target
  *output += *cloud_src;
 
  final_transform = targetToSource;
 }





int main (int argc, char** argv)
{
  // Load data
  std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
  loadData (argc, argv, data);

  // Check user input
  if (data.empty ())
  {
    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
    return (-1);
  }
  PCL_INFO ("Loaded %d datasets.", (int)data.size ());
 
  // Create a PCLVisualizer object
  p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");

        PointCloudColor::Ptr result (new PointCloudColor), source, target;
        Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
 
  Eigen::Affine3f pose;



  for (size_t i = 1; i < data.size (); ++i)
  {
    source = data[i-1].cloudD;
    target = data[i].cloudD;

        PCL_INFO ("Press q to begin the registration.\n");

    PointCloudColor::Ptr temp (new PointCloudColor);
    PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
    pairAlign (source, target, temp, pairTransform, true);

        //PCL_INFO ("funzione align del main passata.\n");

    //transform current pair into the global transform

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovresult (new pcl::PointCloud<pcl::PointXYZ>);

        copyPointCloud(*result, *cloudprovresult);

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovtemp (new pcl::PointCloud<pcl::PointXYZ>);

        copyPointCloud(*temp, *cloudprovtemp);

    pcl::transformPointCloud (*cloudprovtemp, *cloudprovresult, GlobalTransform);

          pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(temp);
          //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb1(result);
       
        p->addPointCloud(temp);
        p->addPointCloud(result);
       

    //update the global transform
    GlobalTransform = pairTransform * GlobalTransform;
       

        p->addCoordinateSystem(0.1,pose);

       
   while (!p->wasStopped())
   {
           p->spin();
   }

}


Thank u very much!!
Reply | Threaded
Open this post in threaded view
|

Re: Build a map with PCL

Khalid Yousif
I did not fully read your code but you might want to use the following functions:
 

pcl::transformPointCloud (*Point_cloud, *Transformed_Point_cloud ,Relative_transformation); // For transforming your the al points

and

pcl::concatenatePointCloud (*Point_cloud,*Point_cloud2,*Combined_cloud); // for adding and up pointclouds. For example you can have a point cloud called Map and keep adding to it the transformed pointcloud.

 

Regards,

Khalid



On 23 December 2013 20:34, fedex8989 <[hidden email]> wrote:
Hi , i would like to build a map with a sequence of point clouds. My code
works in that way: it finds the sift keypoint in 2 point clouds ,
successively finds the rototranslation matrix between them with icp and adds
the resultant point cloud and the global trasformation to a list of models.
Now, i don't know how build a sequence of point clouds using my
rototranslation matrix.
Please help!!



typedef pcl::PointXYZ PointT;
typedef pcl::PointXYZRGB PointTC;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointCloud<PointTC> PointCloudColor;

// This is a tutorial so we can afford having global variables
        //our visualizer
        pcl::visualization::PCLVisualizer *p;


struct PCD
{
  PointCloudColor::Ptr cloudD;
  std::string f_name;

  PCD() : cloudD (new PointCloudColor) {};
};



////////////////////////////////////////////////////////////////////////////////
/** \brief Load a set of PCD files that we want to register together
  * \param argc the number of arguments (pass from main ())
  * \param argv the actual command line arguments (pass from main ())
  * \param models the resultant vector of point cloud datasets
  */
void loadData (int argc, char **argv, std::vector<PCD,
Eigen::aligned_allocator&lt;PCD> > &models)
{
  std::string extension (".pcd");
  // Suppose the first argument is the actual test model
  for (int i = 1; i < argc; i++)
  {
    std::string fname = std::string (argv[i]);
    // Needs to be at least 5: .plot
    if (fname.size () <= extension.size ())
      continue;

    std::transform (fname.begin (), fname.end (), fname.begin (),
(int(*)(int))tolower);

    //check that the argument is a pcd file
    if (fname.compare (fname.size () - extension.size (), extension.size (),
extension) == 0)
    {
      // Load the cloud and saves it into the global list of models
      PCD m;
      m.f_name = argv[i];
      pcl::io::loadPCDFile (argv[i], *m.cloudD);
      //remove NAN points from the cloud
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*m.cloudD,*m.cloudD, indices);

      models.push_back (m);
    }
  }
}


////////////////////////////////////////////////////////////////////////////////
/** \brief Align a pair of PointCloud datasets and return the result
  * \param cloud_src the source PointCloud
  * \param cloud_tgt the target PointCloud
  * \param output the resultant aligned source PointCloud
  * \param final_transform the resultant transform between source and target
  */
void pairAlign (const PointCloudColor::Ptr cloud_src, const
PointCloudColor::Ptr cloud_tgt, PointCloudColor::Ptr output, Eigen::Matrix4f
&final_transform, bool downsample = false)
{
  //
  // Downsample for consistency and speed
  PointCloudColor::Ptr src (new PointCloudColor);
  PointCloudColor::Ptr tgt (new PointCloudColor);
  pcl::VoxelGrid<pcl::PointXYZRGB> 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 SIFT FEATURES-----------//

   const float min_scale = 0.1f;
   const int n_octaves = 6;
  const int n_scales_per_octave = 10;
  const float min_contrast = 0.5f;

   // Estimate the sift interest points using Intensity values from RGB
values
  pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale> sift;
  pcl::PointCloud<pcl::PointWithScale> result, result2;
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new
pcl::search::KdTree<pcl::PointXYZRGB> ());
  sift.setSearchMethod(tree);
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);
  sift.setMinimumContrast(min_contrast);
  sift.setInputCloud(src);
  sift.compute(result);
  sift.setInputCloud(tgt);
  sift.compute(result2);



    // Copying the pointwithscale to pointxyz so as visualize the cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new
pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp2 (new
pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result2, *cloud_temp2);

  std::cout<< cloud_temp <<endl;

  std::cout&lt;&lt; cloud_temp2 &lt;&lt;endl;

  //
  // 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::IterativeClosestPoint&lt;PointT, PointT> reg;
  // 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);

  reg.setInputCloud (cloud_temp);
  reg.setInputTarget (cloud_temp2);



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

    // save cloud for visualization purpose
    cloud_temp = reg_result;

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

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


    prev = reg.getLastIncrementalTransformation ();

  }

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

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovoutput (new
pcl::PointCloud<pcl::PointXYZ>);

  copyPointCloud(*output, *cloudprovoutput);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovtarget (new
pcl::PointCloud<pcl::PointXYZ>);

  copyPointCloud(*cloud_tgt, *cloudprovtarget);

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

   //add the source to the transformed target
  *output += *cloud_src;

  final_transform = targetToSource;
 }





int main (int argc, char** argv)
{
  // Load data
  std::vector<PCD, Eigen::aligned_allocator&lt;PCD> > data;
  loadData (argc, argv, data);

  // Check user input
  if (data.empty ())
  {
    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
    PCL_ERROR ("[*] - multiple files can be added. The registration results
of (i, i+1) will be registered against (i+2), etc");
    return (-1);
  }
  PCL_INFO ("Loaded %d datasets.", (int)data.size ());

  // Create a PCLVisualizer object
  p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise
Incremental Registration example");

        PointCloudColor::Ptr result (new PointCloudColor), source, target;
        Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (),
pairTransform;

        Eigen::Affine3f pose;



  for (size_t i = 1; i < data.size (); ++i)
  {
    source = data[i-1].cloudD;
    target = data[i].cloudD;

        PCL_INFO ("Press q to begin the registration.\n");

    PointCloudColor::Ptr temp (new PointCloudColor);
    PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (),
source->points.size (), data[i].f_name.c_str (), target->points.size ());
    pairAlign (source, target, temp, pairTransform, true);

        //PCL_INFO ("funzione align del main passata.\n");

    //transform current pair into the global transform

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovresult (new
pcl::PointCloud<pcl::PointXYZ>);

        copyPointCloud(*result, *cloudprovresult);

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloudprovtemp (new
pcl::PointCloud<pcl::PointXYZ>);

        copyPointCloud(*temp, *cloudprovtemp);

    pcl::transformPointCloud (*cloudprovtemp, *cloudprovresult,
GlobalTransform);

          pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>
rgb(temp);
          //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>
rgb1(result);

        p->addPointCloud(temp);
        p->addPointCloud(result);


    //update the global transform
    GlobalTransform = pairTransform * GlobalTransform;


        p->addCoordinateSystem(0.1,pose);


   while (!p->wasStopped())
   {
           p->spin();
   }

}


Thank u very much!!



-----
Federica Trozzi
Università degli studi dell'Aquila
Ing. Automatica
Dipartimento di Ingegneria e Scienze dell'Informazione e Matematica
(DISIM)
--
View this message in context: http://www.pcl-users.org/Build-a-map-with-PCL-tp4031457.html
Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users


_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Build a map with PCL

fedex8989
hi and merry christmas! i have tryed to use concatenatepointcloud but it uses Pointcloud2 format that doesn't fit to me...


 


this is my results, using the operator += after having trasformed point cloud source in point cloud target...
Please help :(
Thanks!!
Reply | Threaded
Open this post in threaded view
|

Re: Build a map with PCL

Jochen Sprickerhof
Administrator
Hi Federica,

* fedex8989 <[hidden email]> [2013-12-25 02:37]:

> hi and merry christmas! i have tryed to use concatenatepointcloud but it uses
> Pointcloud2 format that doesn't fit to me...
>
>
>   <http://www.pcl-users.org/file/n4031471/Immagine.png>
>
>
> this is my results, using the operator += after having trasformed point
> cloud source in point cloud target...
> Please help :(

Looks like your transformations are wrong. How did you obtain them? Can
you post more code?

Cheers Jochen
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Build a map with PCL

fedex8989
Thank you!! ok my main function is:


int main (int argc, char** argv)
{
  // Load data
  std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
  loadData (argc, argv, data);

  // Check user input
  if (data.empty ())
  {
    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
    return (-1);
  }
  PCL_INFO ("Loaded %d datasets.", (int)data.size ());
 
  // Create a PCLVisualizer object
  p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");

        PointCloudColor::Ptr result (new PointCloudColor), source, target;
       
        Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
 
  Eigen::Affine3f pose;
        PointCloudColor::Ptr temp (new PointCloudColor);


  for (size_t i = 1; i < data.size (); ++i)
  {
    source = data[i-1].cloudD;
    target = data[i].cloudD;

        PCL_INFO ("Press q to begin the registration.\n");
           
    PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
    pairAlign (source, target, temp, pairTransform, true);

        pcl::transformPointCloud (*temp, *result, GlobalTransform);
               
    GlobalTransform = pairTransform * GlobalTransform;
        std::cout<<GlobalTransform;
             
           
  }
  p->addPointCloud(temp);

  p->spin();
   
}


with the function pairAlign that is:



void pairAlign (const PointCloudColor::Ptr cloud_src, const PointCloudColor::Ptr cloud_tgt, PointCloudColor::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
  //
  // Downsample for consistency and speed
  // \note enable this for large datasets
  PointCloudColor::Ptr src (new PointCloudColor);
  PointCloudColor::Ptr tgt (new PointCloudColor);
  pcl::VoxelGrid<pcl::PointXYZRGB> 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 SIFT FEATURES-----------//
 
   const float min_scale = 0.1f;
   const int n_octaves = 6;
  const int n_scales_per_octave = 10;
  const float min_contrast = 0.5f;

   // Estimate the sift interest points using Intensity values from RGB values
  pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale> sift;
  pcl::PointCloud<pcl::PointWithScale> result, result2;
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
  sift.setSearchMethod(tree);
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);
  sift.setMinimumContrast(min_contrast);
  sift.setInputCloud(src);
  sift.compute(result);
  sift.setInputCloud(tgt);
  sift.compute(result2);

  // Copying the pointwithscale to pointxyz so as visualize the cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp2 (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result2, *cloud_temp2);

  // Align
  pcl::IterativeClosestPoint<PointT, PointT> reg;
  // Set the maximum distance between two correspondences (src<->tgt) to 10cm
   reg.setMaxCorrespondenceDistance (0.1);
   reg.setInputTarget (cloud_temp2);

  // Run the same optimization in a loop
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
  PointCloud::Ptr reg_result = cloud_temp;
  reg.setMaximumIterations (2);

  for (int i = 0; i < 30; ++i)
  {
   
    // save cloud for visualization purpose
    cloud_temp = reg_result;

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

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

    prev = reg.getLastIncrementalTransformation ();

  }

  pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);

   //add the source to the transformed target
  *output += *cloud_src;
 
  final_transform = targetToSource;
 }


i have also made another function that is called loadData (used in main ) that is:

void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models)
{
  std::string extension (".pcd");
  // Suppose the first argument is the actual test model
  for (int i = 1; i < argc; i++)
  {
    std::string fname = std::string (argv[i]);
    // Needs to be at least 5: .plot
    if (fname.size () <= extension.size ())
      continue;

    std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);

    //check that the argument is a pcd file
    if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0)
    {
      // Load the cloud and saves it into the global list of models
      PCD m;
      m.f_name = argv[i];
      pcl::io::loadPCDFile (argv[i], *m.cloudD);
      //remove NAN points from the cloud
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*m.cloudD,*m.cloudD, indices);

      models.push_back (m);
    }
  }
}


with the struct PCD that is:

struct PCD
{
  PointCloudColor::Ptr cloudD;
  std::string f_name;

  PCD() : cloudD (new PointCloudColor) {};
};

[this is all the code]

i think i have made confusion with some transformation in the pairAlign function but i have not found the mistake :/

another question: so the use of += for visualize aligned point cloud is correct?

Thank you very much!!

Cheers, Federica! :)