ICP Convergence Criteria

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

ICP Convergence Criteria

beaverfan
I'm struggling to uderstand exactly how to set the convergence criteria for ICP.  I forced it into a loop with max iterations of 50 to see the progression. From the result, I expect it to converge below 1e-12 within 150 iterations. Then we I back and set the max interations above 150 (500 to be safe) I still get a low score on my first iteration.

Am I missing the right way to set the convergence criteria?

Any advice is greatly appreciated.

Background info: my pcd files are from the Kinect and the source and target are from the same source, but just translated and rotated within 0.25m of each other.

Here is my code:

****************************************************************************
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZRGB>);

  //
  //  Load clouds
  //
  if (pcl::io::loadPCDFile (argv[1], *cloud_source) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    return (-1);
  }
  if (pcl::io::loadPCDFile (argv[2], *cloud_target) < 0)
  {
    std::cout << "Error loading scene cloud." << std::endl;
    return (-1);
  }

  //setup ICP
  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  icp.setTransformationEpsilon (1e-12);
  icp.setEuclideanFitnessEpsilon (1e-12);
  icp.setMaxCorrespondenceDistance (0.5);
  icp.setMaximumIterations(50);
  icp.setInputSource(cloud_source);
  icp.setInputTarget(cloud_target);

  //copy the source cloud
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source_aligned = cloud_source;
 
  //Forced iteractions with ICP loop
  for (int i = 0; i < 10; ++i)
  {
    PCL_INFO ("Iteration Nr. %d.\n", i);
    cloud_source = cloud_source_aligned;
    icp.setInputSource (cloud_source);
    icp.align (*cloud_source_aligned);
    std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
  }
  std::cout << icp.getFinalTransformation() << std::endl;
  pcl::io::savePCDFileBinary ("output.pcd", *cloud_source_aligned);
 return (0);
}
 ***************************************************************************
 Here is the result:

Iteration Nr. 0.
has converged:1 score: 0.0466388
Iteration Nr. 1.
has converged:1 score: 1.36604e-06
Iteration Nr. 2.
has converged:1 score: 2.79864e-12
Iteration Nr. 3.
has converged:1 score: 2.59911e-12
Iteration Nr. 4.
has converged:1 score: 2.58414e-12
Iteration Nr. 5.
has converged:1 score: 2.60695e-12
Iteration Nr. 6.
has converged:1 score: 2.5751e-12
Iteration Nr. 7.
has converged:1 score: 2.60297e-12
Iteration Nr. 8.
has converged:1 score: 2.61447e-12
Iteration Nr. 9.
has converged:1 score: 2.67037e-12
           1  3.72529e-09  2.98023e-08 -2.98023e-08
 7.45058e-09            1 -1.86265e-09  9.31323e-10
-5.96046e-08  5.58794e-09            1 -5.96046e-08
           0            0            0            1

**********************************************************************
Then when I increase the max iterations to 500:

Iteration Nr. 0.
has converged:1 score: 0.047526
Iteration Nr. 1.
has converged:1 score: 2.98996e-12
Iteration Nr. 2.
has converged:1 score: 2.7392e-12
Iteration Nr. 3.
has converged:1 score: 2.75555e-12
Iteration Nr. 4.
has converged:1 score: 2.6851e-12
Iteration Nr. 5.
has converged:1 score: 2.56157e-12
Iteration Nr. 6.
has converged:1 score: 2.64905e-12
Iteration Nr. 7.
has converged:1 score: 2.65373e-12
Iteration Nr. 8.
has converged:1 score: 2.64779e-12
Iteration Nr. 9.
has converged:1 score: 2.667e-12
           1 -2.79397e-09 -2.98023e-08  1.49012e-08
-1.02445e-08            1 -5.58794e-09  2.79397e-09
-2.98023e-08  5.58794e-09            1            0
           0            0            0            1
Reply | Threaded
Open this post in threaded view
|

Re: ICP Convergence Criteria

Jochen Sprickerhof
Administrator
Hi Finley,

* beaverfan <[hidden email]> [2013-10-30 08:43]:

> I'm struggling to uderstand exactly how to set the convergence criteria for
> ICP.  I forced it into a loop with max iterations of 50 to see the
> progression. From the result, I expect it to converge below 1e-12 within 150
> iterations. Then we I back and set the max interations above 150 (500 to be
> safe) I still get a low score on my first iteration.
>
> Am I missing the right way to set the convergence criteria?
>
> Any advice is greatly appreciated.
>
> Background info: my pcd files are from the Kinect and the source and target
> are from the same source, but just translated and rotated within 0.25m of
> each other.

I don't think it makes sense to get a fitness score below 1e-12 for
Kinect frames, as the noise in the point coordinates are way above.

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

Re: ICP Convergence Criteria

beaverfan
I agree that I will have to use a more reasonable fitness score, but my test is using the same data as the source and target. I just moved the source away from the target by 0.25m. Theoretically the fitness score should be perfect. I wanted to test the functionality of ICP before doing more tests with my data.

If I change the fitness score to something like 1e-8 it still gives me the same results.

Would an extremely small fitness score target cause any effect on my issue of the initial iteration having a fitness score of 0.046
Reply | Threaded
Open this post in threaded view
|

Re: ICP Convergence Criteria

Jochen Sprickerhof
Administrator
* beaverfan <[hidden email]> [2013-10-31 10:26]:

> I agree that I will have to use a more reasonable fitness score, but my test
> is using the same data as the source and target. I just moved the source
> away from the target by 0.25m. Theoretically the fitness score should be
> perfect. I wanted to test the functionality of ICP before doing more tests
> with my data.
>
> If I change the fitness score to something like 1e-8 it still gives me the
> same results.
>
> Would an extremely small fitness score target cause any effect on my issue
> of the initial iteration having a fitness score of 0.046

Depending on the point cloud, ICP could get stuck in all kinds of local
minima, so the fitness score will not really help you. I would propose
to read up on how ICP works.

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

Re: ICP Convergence Criteria

Antoine Rennuit
As Jochen is explaining your initial conditions may be in a domain of
attraction which is not the one of the global minimum but corresponds to the
local minimum you end-up in after convergence.

One way to solve this is to "shake" your source points, i.e. to start from
several initial conditions (i.e. different locations) randomly chosen. If
you sample enough points or sample them smartly enough you will end up into
the global minimum basin of convergence... This could be long though.

As a general guideline, try to make sure your initial conditions are not too
far away from your destination (both in translation and in rotation). An
easy trick to help in this is to start the convergence after a
pre-conditioning of the source data (inspired by Horn's paper). This
pre-conditioning consist in moving the source data in such a way that the
mean position of the source points is aligned with the mean position of the
destination points. Then only you start the convergence. You can even
"pre-align" the source onto the destination clouds at each step. This has
proven to work fine for me...

Cheers,

Antoine.

-----Original Message-----
From: [hidden email]
[mailto:[hidden email]] On Behalf Of Jochen Sprickerhof
Sent: jeudi 31 octobre 2013 18:40
To: Point Cloud Library (PCL) users
Subject: Re: [PCL-users] ICP Convergence Criteria

* beaverfan <[hidden email]> [2013-10-31 10:26]:

> I agree that I will have to use a more reasonable fitness score, but
> my test is using the same data as the source and target. I just moved
> the source away from the target by 0.25m. Theoretically the fitness
> score should be perfect. I wanted to test the functionality of ICP
> before doing more tests with my data.
>
> If I change the fitness score to something like 1e-8 it still gives me
> the same results.
>
> Would an extremely small fitness score target cause any effect on my
> issue of the initial iteration having a fitness score of 0.046

Depending on the point cloud, ICP could get stuck in all kinds of local
minima, so the fitness score will not really help you. I would propose to
read up on how ICP works.

Cheers Jochen
_______________________________________________
[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: ICP Convergence Criteria

beaverfan
Thank you for that detailed explanation!

My newest routine actually already moves the source cloud to the target based on the user picking on a single point in the PCLVisualizer. The source is translated to the picked point and then ICP aligns it. I'm still getting that initial low fitness score on the first iteration, but it's not that big of a deal.

The process is very similar to the template alignment demo, but that routine has not worked well with my geometry. I think that the source point cloud do not have enough features.  So far ICP has worked the best.

I will be reading that paper soon though. Thanks.
Reply | Threaded
Open this post in threaded view
|

Re: ICP Convergence Criteria

linsnos
This post has NOT been accepted by the mailing list yet.
In reply to this post by Antoine Rennuit
Hi!

What is "Horn's paper"? I didn't find good key words for the googling.

/Tobias