Quantcast

ICP converging very slowly/not at all

classic Classic list List threaded Threaded
21 messages Options
12
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

ICP converging very slowly/not at all

Lgum
Hello

I'm trying to register two pointclouds via ICP but somehow the registration doesn't seem to work properly. The code for this is rather simple (it's basically the code from the tutorial):

[code]
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

        string sSource = argv[1];
        string sTarget = argv[2];

        pcl::io::loadPLYFile(sSource.c_str(), *cloud_in);
        pcl::io::loadPLYFile(sTarget.c_str(), *cloud_out);

        pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);

        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setInputCloud(cloud_in);
        icp.setInputTarget(cloud_out);
        icp.setMaximumIterations(40);
        icp.setRANSACIterations(40);
        pcl::PointCloud<pcl::PointXYZ> Final;
        icp.align(Final);
        std::cout << "has converged:" << icp.hasConverged() << " score: " <<
        icp.getFitnessScore() << std::endl;
        std::cout << icp.getFinalTransformation() << std::endl;

        pcl::io::savePLYFile("Output.ply", Final, true);

        return (0);
[/code]

The pointclouds have already been roughly registered and are sitting pretty close to each other (there's a small scale-difference but it doesn't matter for now). Looking at it from the side it looks like this:



The yellow points represent the target (fixed) cloud. The blue ones the source cloud. Using the above algorithm yields not much of an improvement (Yellow: fixed - Green = PCL-ICP Result)



Tweaking the convergence criteria like amount of iterations etc. doesn't really make it move much more (Yellow: fixed, blue = PCL-ICP Result with some tweaks):



I'm pretty sure I'm doing something wrong because it appears to me that this should be possible to align. A quick check with Geomagic yielded the following result (in ~2 seconds and 19 iterations):



So does anyone see the mistake I'm making? If anyone wants to have a go, here are the two ply-files that I'm using:

source
target

Cheers
David
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Lgum
One more thing to add:

I've tried to apply ICP to the same pointcloud, but transformed by 2.0/1.5/0.4 metres via pcl::transformPointCloud. The result is similar: The pointcloud does not move much at all - even after 1000 iterations.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Lgum
Hello again

I've had another go at this problem with CloudCompare. The results there are very similar to that of PCL. However - there is an option to 'Enable furthest point removal' that seems to remove the farthest points in each iteration. Enabling this aligns the pointclouds in a similar fashion to Geomagic (so I assume they're doing something similar there). Sadly I couldn't replicate this behaviour in pcl by using a statistical outlier filter and setting the maxCorrespondenceDistance respectively. I've tested this on the prebuilt PCL 1.6 package. I'll see whether the generalized ICP works better on this set (I'll build the trunk version as the prebuild package somehow doesn't include the GICP).

Cheers
David
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

rajesh
In reply to this post by Lgum
Yes. There is no convergence with PCL ICP.

Try with VTK. It is good result. Fast convergence.

From PCL, I have moved to VTK.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Radu B. Rusu
Administrator
In reply to this post by Lgum
David,

Take a look at the new Correspondence Estimation/Rejection pipelines in trunk, and create your own ICP loop.

Cheers,
Radu.
--
http://openperception.org

On 02/13/2013 12:23 AM, Lgum wrote:

> Hello again
>
> I've had another go at this problem with  CloudCompare
> <http://www.danielgm.net/cc/>  . The results there are very similar to that
> of PCL. However - there is an option to 'Enable furthest point removal' that
> seems to remove the farthest points in each iteration. Enabling this aligns
> the pointclouds in a similar fashion to Geomagic (so I assume they're doing
> something similar there). Sadly I couldn't replicate this behaviour in pcl
> by using a statistical outlier filter and setting the
> maxCorrespondenceDistance respectively. I've tested this on the prebuilt PCL
> 1.6 package. I'll see whether the generalized ICP works better on this set
> (I'll build the trunk version as the prebuild package somehow doesn't
> include the GICP).
>
> Cheers
> David
>
>
>
> --
> View this message in context: http://www.pcl-users.org/ICP-converging-very-slowly-not-at-all-tp4026013p4026028.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
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Radu B. Rusu
Administrator
In reply to this post by rajesh

On 02/13/2013 08:30 AM, rajesh wrote:
> Yes. There is no convergence with PCL ICP.

We're sorry that you couldn't get it to run in your application. We assure you that we're using PCL's ICP successfully
in a lot of projects. The key is correct parametrization. With trunk you have the capability to create your own
pipelines, so if you understand what's going on, you can solve a lot of different problems.


> Try with VTK. It is good result. Fast convergence.
>
>  From PCL, I have moved to VTK.

Good luck!

Cheers,
Radu.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

rajesh
Radu,


Of course, ICP works when the target PC has minimal transformation with respect to source PC
Translation say 1 unit  and rotation 0.5 degree. But not more than that.

When the deviation is large, it  fails.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Christian Potthast
Rajesh,
This is not true!
Convergence has nothing todo with translational distance or rotational degrees.

Two point clouds can be arbitrary far apart and be rotated in an arbitrarily rotation. It is just a matter of how correspondence points are picked.

Cheers
Christian

On Feb 13, 2013, at 8:46 AM, rajesh <[hidden email]> wrote:

> Radu,
>
>
> Of course, ICP works when the target PC has minimal transformation with
> respect to source PC
> Translation say 1 unit  and rotation 0.5 degree. But not more than that.
>
> When the deviation is large, it  fails.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/ICP-converging-very-slowly-not-at-all-tp4026013p4026049.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
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Christian Potthast
I might add that in arbitrary rotations and translations you need to add additional constrains due to the fact the it is not a convex optimization problem anymore

Cheers
Christian

On Feb 13, 2013, at 9:00 AM, Christian Potthast <[hidden email]> wrote:

> Rajesh,
> This is not true!
> Convergence has nothing todo with translational distance or rotational degrees.
>
> Two point clouds can be arbitrary far apart and be rotated in an arbitrarily rotation. It is just a matter of how correspondence points are picked.
>
> Cheers
> Christian
>
> On Feb 13, 2013, at 8:46 AM, rajesh <[hidden email]> wrote:
>
>> Radu,
>>
>>
>> Of course, ICP works when the target PC has minimal transformation with
>> respect to source PC
>> Translation say 1 unit  and rotation 0.5 degree. But not more than that.
>>
>> When the deviation is large, it  fails.
>>
>>
>>
>>
>> --
>> View this message in context: http://www.pcl-users.org/ICP-converging-very-slowly-not-at-all-tp4026013p4026049.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
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

rajesh
In reply to this post by Lgum

Max correspondence, is something I have tired of attempting.

Tell me, how do I decide the correspondence parameter.
Are there any a prior information required this setting?



[PS]
PCL is an excellent library as far Point cloud processing is concerned.
But registration is not well implemented.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Radu B. Rusu
Administrator
In reply to this post by rajesh
As I previously mentioned, you need to switch away from the classical "ICP" in 1.6 (or previous versions) and create
your own pipelines.

See http://www.pointclouds.org/documentation/tutorials/registration_api.php and maybe
http://dev.pointclouds.org/projects/pcl/repository/show/trunk/doc/tutorials/content/sources/registration_api for more
information.

Also, while we try to provide enough functionality for users that are not versed at all into this topic, it is not our
responsibility if these users do not take the time to understand the concepts carefully.

Trust us, we wouldn't be putting code out there that would be utter crap ;) If it's there, it definitely works. Whether
it works on all conditions, is a different story, hence our explanations about the right parametrization and creating
your own pipelines.

Cheers,
Radu.
--
http://openperception.org

On 02/13/2013 08:46 AM, rajesh wrote:

> Radu,
>
>
> Of course, ICP works when the target PC has minimal transformation with
> respect to source PC
> Translation say 1 unit  and rotation 0.5 degree. But not more than that.
>
> When the deviation is large, it  fails.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/ICP-converging-very-slowly-not-at-all-tp4026013p4026049.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
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Radu B. Rusu
Administrator
In reply to this post by rajesh

On 02/13/2013 09:14 AM, rajesh wrote:

>
> Max correspondence, is something I have tired of attempting.
>
> Tell me, how do I decide the correspondence parameter.
> Are there any a prior information required this setting?
>
>
>
> [PS]
> PCL is an excellent library as far Point cloud processing is concerned.
> But registration is not well implemented.

Rajesh, at this point it would be better to start discussing things technically. If you find a bug or if a particular
algorithm is not implemented appropriately with respect to its scientific paper source, please let us know, and we'd be
happy to fix it. Just saying "is not well implemented" because you haven't managed to get it to work is not helpful.


Cheers,
Radu.
--
http://openperception.org
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

andersgb1
May I add to Radu's comment that ICP has proven its efficiency for more than 20 years now - the PCL implementation allows you to completely control the parameters of the original algorithm. Thus, if the Vtk version works better, it is either 1) because the Vtk implementation is non-standard, 2) you have a bug in the way you use PCL's version, or 3) there is an internal bug in the PCL implementation. For the last case, we are of course most interested in your input to this, should you have any.


On 13 February 2013 18:40, Radu B. Rusu <[hidden email]> wrote:

On 02/13/2013 09:14 AM, rajesh wrote:

Max correspondence, is something I have tired of attempting.

Tell me, how do I decide the correspondence parameter.
Are there any a prior information required this setting?



[PS]
PCL is an excellent library as far Point cloud processing is concerned.
But registration is not well implemented.

Rajesh, at this point it would be better to start discussing things technically. If you find a bug or if a particular algorithm is not implemented appropriately with respect to its scientific paper source, please let us know, and we'd be happy to fix it. Just saying "is not well implemented" because you haven't managed to get it to work is not helpful.



Cheers,
Radu.
--
http://openperception.org


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

Re: ICP converging very slowly/not at all

astaranowicz
In reply to this post by Radu B. Rusu
I'm having the same problem except I'm trying to use data from the Kinect.  I do have a few questions about the convergence.
1) Why would 2 "noisy" point clouds from the Kinect converge to the same rotation and translation using icp.align.  I would expect some variation since the Nearest-Neighbor calculation along with RANSAC are variable (depends on the points taken).  I did select the MaxCorrespondenceDistance and the EuclideanFitnessScore to be below the noise level of the Kinect (~.04m on the depth).

2) Is there some way to check if ICP truly ran (e.g., the number of iterations before exiting) in the context of icp.align.

I tried a few data sets (bunny from the tutorials, Kinect point cloud, and Gazebo stereo-camera points) and the implementation of icp seems to converge very well for the Bunny (even with noise added to the points ~0 mean and 4cm std, very far translations, different rotations, and adding outliers to one of the point cloud).  But it does not for the Kinect point cloud, even with very small translation (~5cm) and no rotation. (The solution given is 0.5 degrees in rotation and yaw, and [0.005, 0.02,0.05] in translation (x,y,z) in meters.)

3) Is the ICP class implemented very sensitive to the amount of points used?  In the Original ICP, the more points used, the better solution in a least-square sense.  Also, the correspondences used are accurate as far as I can see.

In regards to "switch away from the classical "ICP" in 1.6 (or previous versions) and create
your own pipelines."
4) Since 1.6 is an official release, I would expect it to be very stable and would rather stick with it than move to the /trunk.  How do you create your "own" pipeline?  Or maybe I don't understand by what you mean a pipeline?


This is more of a side note:  The descriptions of functions and classes in some files are very clear, while others are very lax.  It makes it hard to follow functions and what they do.

For example, what are the differences between icp.h and icp_nl.h?  Maybe I would like to create a pipeline using both of these classes. Lets say I want to use icp.align to get a least square solution, then use icp_nl.align to refine (using Levenberg-Marquardt).  However, does icp_nl already find a least-square solution and uses it as an initial guess? Or maybe I would like to bypass icp_nl.h and use the functions from transformation_from_correspondences.h or transformation_estimation.h.  How would I turn on Levenberg-Marquardt myself for these functions?

Thanks for listening.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Lgum
In reply to this post by Radu B. Rusu
Hello Radu

Thanks for the heads-up. The new possibilities in trunk look promising :) I have however a problem with the trunk version (although I'm not sure it's the right place to post but here goes):

Apparently the voxel-grid filter got changed so that it checks whether the leaf size is too small or not. The thing is now that I can downsample my 10 million pointcloud to max ~200'000 points. Everything above gets rejected. Has this problem of integer index overflow been in 1.6 but not caught, or is this a new implementation that does things differently? And if it does, could it be possible to also have the old version available?

Cheers
David
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Radu B. Rusu
Administrator
David,

I believe the VoxelGrid code is still under investigation, per http://dev.pointclouds.org/issues/780. Unfortunately it
needs a champion -- we need a developer that is willing to do some thorough testing before we can push the changes, as
Radoslaw is busy afaik.

Cheers,
Radu.
--
http://openperception.org

On 02/13/2013 10:53 PM, Lgum wrote:

> Hello Radu
>
> Thanks for the heads-up. The new possibilities in trunk look promising :) I
> have however a problem with the trunk version (although I'm not sure it's
> the right place to post but here goes):
>
> Apparently the voxel-grid filter got changed so that it checks whether the
> leaf size is too small or not. The thing is now that I can downsample my 10
> million pointcloud to max ~200'000 points. Everything above gets rejected.
> Has this problem of integer index overflow been in 1.6 but not caught, or is
> this a new implementation that does things differently? And if it does,
> could it be possible to also have the old version available?
>
> Cheers
> David
>
>
>
> --
> View this message in context: http://www.pcl-users.org/ICP-converging-very-slowly-not-at-all-tp4026013p4026087.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
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

Lgum
Hello

So I've updated to 1.7 (I applied the temporary patch for the voxel grid filter found here: http://dev.pointclouds.org/issues/780 ) and used the example1 found in the repository: http://dev.pointclouds.org/projects/pcl/repository/show/trunk/doc/tutorials/content/sources/registration_api ).

Without any modifications it produced (although I had to obtain Normals from the pointclouds first) the expected result - so this pipeline is definitely an improvement over the 1.6 implementation of ICP. Well done guys! :)

Cheers
David
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

ollih100
This post was updated on .
Hi,

I am new to the PCL library (1.7.2) trying to get successful results using the ICP to merge point clouds from a laser line scanning device. After several tries to get the ICP to converge to the correct solution for 2 point clouds I found the post above which describes my problem pretty precise. But even the implementation as in example1 does not give me correct results. My pipeline is setup as follows:

1) Reduce Raw data using ApproximateVoxelGrid with reasonable leafsize (0.2mm)
2) Use MLS with
  mls.setPolynomialFit (true);
  mls.setSearchMethod (tree);
  mls.setSearchRadius(2.0);      // units are mm
  mls.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ,   pcl::PointXYZ>::VOXEL_GRID_DILATION);
  mls.setDilationVoxelSize(0.5);
3) Compute normals for src and tgt using NormalEstimationOMP
  ne.setSearchMethod (tree);
  ne.setKSearch(12);
4) Find correspondences using CorrespondenceEstimationBackProjection est;
  est.setInputSource (src);
  est.setInputTarget (tgt);
  est.setSourceNormals (src);
  est.setTargetNormals (tgt);
  est.setKSearch (4.0);
5) Reject Outlier using CorrespondenceRejectorMedianDistance rej;
  rej.setMedianFactor (5.0);
6) Reject outlier using CorrespondenceRejectorSampleConsensus crsac;
  crsac.setInputSource(src);
  crsac.setInputTarget(tgt);
  crsac.setInlierThreshold(0.1);
  crsac.setMaxIterations(200);

Having a look at the number of parameters to check it is quite time consuming to test different combinations. Do you have some hint how to choose some sort of robust set of parameters maybe based on cloud statistics? Is there a general mistake in the pipeline?
As David stated initially I tried to test the result of the MLS in addition to the normals to the CloudCompare tool. Enabling "farthest point removal" and taking the std parameters converges immediatly. That behaviour makes me think that there must be some very straight forward way to align these clouds. Do you know what's implemented in that SW?
I attached the two clouds to this message. It would be great if you could have a look at that.

Thanks in advance,
Olli
Src.pcd
Tgt.pcd



 
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

VictorLamoine
Administrator
Hello,

I'm not sure at which step you are providing the clouds but ICP is working great with the two point clouds you provided:

Here is the test code I use to configure my ICP parameters:

#include <iostream>
#include <string>

#include <pcl/console/time.h>   // TicToc
#include <pcl/console/parse.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

using std::cout;
using std::endl;

pcl::PolygonMesh::Ptr scan_mesh;
pcl::PolygonMesh::Ptr cad_mesh;
PointCloudT::Ptr scan_cloud;
PointCloudT::Ptr cad_cloud;
bool update_models = false;
int icp_iterations (1);
double voxel_grid_size (0.0005);  // 0.5 mm
double max_corr_distance (0.001);  // 1 mm
int iterations_count (0);

void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
                       void* nothing)
{
  static int total_icp_time (0);

  if (event.getKeySym () == "space" && event.keyDown ())
  {
    if (iterations_count % 100 == 0 && iterations_count != 0)
    {
      max_corr_distance /= 3.0;
      cout << "Maximum correspondence distance = " << max_corr_distance << endl;
    }

    // Use ICP to align point clouds
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setMaximumIterations (icp_iterations);
    icp.setInputSource (cad_cloud);
    icp.setInputTarget (scan_cloud);
    icp.setMaxCorrespondenceDistance (max_corr_distance);  // 1/5th of the object size

    // The user pressed "space"
    pcl::console::TicToc time;
    time.tic ();
    icp.align (*cad_cloud);
    double icp_time = time.toc ();
    total_icp_time += icp_time;
    cout << "Applied " << icp_iterations << " ICP iteration in " << icp_time << " ms (total = " << total_icp_time << " ms)" << endl;

    if (icp.hasConverged ())
    {
      iterations_count += icp_iterations;
      cout << "ICP has converged, score is " << icp.getFitnessScore () << endl;
      cout << "ICP iterations = " << iterations_count << endl << endl;

      PointCloudT::Ptr cloud (new PointCloudT);
      pcl::fromPCLPointCloud2 (cad_mesh->cloud, *cloud);
      pcl::transformPointCloud (*cloud, *cloud, icp.getFinalTransformation ());
      pcl::toPCLPointCloud2 (*cloud, cad_mesh->cloud);
      update_models = true;
    }
    else
    {
      PCL_ERROR("\nICP has not converged.\n");
      //return (-1);
    }
  }
}

void
displayHelp (int argc,
             char** argv)
{
  PCL_INFO("\nUsage: %s [OPTION] DEST SOURCE_MESH SOURCE_CLOUD\n", argv[0]);
  PCL_INFO("Align DEST point cloud / mesh to DEST point cloud using ICP.\n\n");

  PCL_INFO("DEST should be a mesh, the point cloud is extracted for the alignment.\n");
  PCL_INFO("SOURCE_MESH is the mesh (usually a CAD) to be aligned,\nSOURCE_CLOUD should be an uniformly sampled cloud extracted from SOURCE_MESH.\n\n");

  PCL_INFO("SOURCE_MESH, SOURCE_CLOUD and DEST must be in the PLY (Polygon File Format) file format.\n");
  PCL_INFO("To trigger the alignment, press \"space\" when the focus is on the visualizer window.\n\n");

  PCL_INFO("Available options:\n"
           "\t-i, --iterations           Specify the number of ICP iterations each time \"space\" is pressed. Default is 1\n"
           "\t-vgs --voxel_grid_size     The size of the voxel grid (in meters) used to down-sample data for faster alignment. Use -1 to disable the voxel grid. Default is 0.0005 meters\n"
           "\t-mcd --max_corr_distance  The maximum correspondence distance (in meters) for the ICP alignment. Default is 0.001 meters\n\n");
}

int
main (int argc,
      char* argv[])
{
  // Checking program arguments
  std::vector<int> ply_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".ply");

  if (argc < 3 || ply_file_indices.size () < 2)
  {
    displayHelp (argc, argv);
    return (-1);
  }

  cout << "Loading input data" << endl;
  // Loading scan mesh
  scan_mesh.reset (new pcl::PolygonMesh);
  if (pcl::io::loadPolygonFilePLY (argv[ply_file_indices[0]], *scan_mesh) == 0)
  {
    PCL_ERROR("Failed to read PLY file %s\n", argv[ply_file_indices[0]]);
    return (-1);
  }

  // Loading CAD mesh
  cad_mesh.reset (new pcl::PolygonMesh);
  if (pcl::io::loadPolygonFilePLY (argv[ply_file_indices[1]], *cad_mesh) == 0)
  {
    PCL_ERROR("Failed to read PLY file %s\n", argv[ply_file_indices[1]]);
    return (-1);
  }

  cad_cloud.reset (new PointCloudT);  // CAD point cloud
  if (ply_file_indices.size () > 2)
  {
    if (pcl::io::loadPLYFile (argv[ply_file_indices[2]], *cad_cloud) != 0)
    {
      PCL_ERROR("Could not load file %s.\n", argv[ply_file_indices[2]]);
      return (-1);
    }
  }
  else
  {
    // Convert CAD mesh to CAD cloud
    pcl::fromPCLPointCloud2 (cad_mesh->cloud, *cad_cloud);
  }

  if (argc > 3)
  {
    pcl::console::parse_argument (argc, argv, "--iterations", icp_iterations);
    pcl::console::parse_argument (argc, argv, "-i", icp_iterations);
    if (icp_iterations < 1)
    {
      PCL_WARN("Number of maximum iterations must be >= 1\n");
      icp_iterations = 1;
    }

    pcl::console::parse_argument (argc, argv, "--voxel_grid_size", voxel_grid_size);
    pcl::console::parse_argument (argc, argv, "-vgs", voxel_grid_size);

    pcl::console::parse_argument (argc, argv, "--max_corr_distance", max_corr_distance);
    pcl::console::parse_argument (argc, argv, "-mcd", max_corr_distance);
    max_corr_distance = fabs (max_corr_distance);
  }

  scan_cloud.reset (new PointCloudT);  // Scan point cloud
  pcl::fromPCLPointCloud2 (scan_mesh->cloud, *scan_cloud);

  cout << "Parameters" << endl << "----------" << endl;
  cout << "ICP iterations steps: " << icp_iterations << endl;
  cout << "Voxel grid size: " << voxel_grid_size << endl;
  cout << "Maximum correspondence distance: " << max_corr_distance << endl;
  cout << "Scan cloud size = " << scan_cloud->size () << endl;
  cout << "CAD cloud size = " << cad_cloud->size () << endl << endl;

  if (voxel_grid_size > 0)
  {
    // Down-sample the data for faster alignment
    cout << "Filtering scan and CAD clouds" << endl << "----------------------------" << endl;
    pcl::console::TicToc time;
    time.tic ();
    pcl::VoxelGrid<PointT> voxel_grid;
    voxel_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
    voxel_grid.setInputCloud (scan_cloud);
    voxel_grid.filter (*scan_cloud);
    voxel_grid.setInputCloud (cad_cloud);
    voxel_grid.filter (*cad_cloud);
    cout << "Filtered 2 clouds in " << time.toc () << " ms " << endl;
    cout << "Scan cloud size: " << scan_cloud->size () << endl;
    cout << "CAD cloud size: " << cad_cloud->size () << endl << endl;
  }

  // Visualizer
  pcl::visualization::PCLVisualizer viewer;
  int v1 (0), v2 (1);
  viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
  viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);

  viewer.addPolygonMesh (*cad_mesh, "mesh_cad", v1);
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 255 / 255., 50 / 255., 50 / 255., "mesh_cad", v1);
  viewer.addPolygonMesh (*scan_mesh, "mesh_scan", v1);

  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_cad_color (cad_cloud, 255, 50, 50);
  viewer.addPointCloud (cad_cloud, cloud_cad_color, "cloud_cad", v2);
  viewer.addPointCloud (scan_cloud, "cloud_scan", v2);

  // Display the visualizer
  viewer.setCameraPosition (1.42379, 0.159511, -0.179308, 0.217544, 0.42156, 0.880319);
  viewer.resetCamera ();
  viewer.addText ("0 iterations", 0, 20, 16, 1, 1, 1, "icp_iterations", 0);
  while (!viewer.wasStopped ())
  {
    if (update_models)
    {
      viewer.updateText (boost::lexical_cast<std::string> (iterations_count) + " iterations", 0, 20, "icp_iterations");
      viewer.updatePolygonMesh (*scan_mesh, "mesh_scan");
      viewer.updatePolygonMesh (*cad_mesh, "mesh_cad");
      viewer.updatePointCloud (scan_cloud, "cloud_scan");
      pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_cad_color (cad_cloud, 255, 50, 50);
      viewer.updatePointCloud (cad_cloud, cloud_cad_color, "cloud_cad");
      update_models = false;
    }
    viewer.spinOnce ();
  }
  return (0);
}
Use pcl_converter to convert your PCD files to PLY files and then use these parameters:
./icp -h ~/Téléchargements/src.ply ~/Téléchargements/tgt.ply -vgs 0.1 -mcd 0.5 -i 10

After 50 iterations the alignment is very good, after 70 it is nearly perfect.

Bye
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: ICP converging very slowly/not at all

ollih100
Hi Victor,

thanks for the quick response. Your code solved the issue. Great!

Best
Olli



12
Loading...