yangjiaolong / go-icp

Implementation of the Go-ICP algorithm for globally optimal 3D pointset registration

Go-ICP for globally optimal 3D pointset registration

(A demo video can be found on here.)

Introduction

This repository contains the C++ code for the Go-ICP algorithm (with trimming strategy for outlier handling). It is free software under the terms of the GNU General Public License (GPL) v3. Details of the Go-ICP algorithm can be found in our papers:

• J. Yang, H. Li, Y. Jia, Go-ICP: Solving 3D Registration Efficiently and Globally Optimally, International Conference on Computer Vision (ICCV), 2013. PDF

• J. Yang, H. Li, D. Campbell, Y. Jia, Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration, IEEE Transactions on Pattern Analysis and Machine Intelligence (TPAMI), 2016. PDF

Please read this file carefully prior to using the code. Some frequently-asked questions have answers here.

Compiling

Use cmake to generate desired projects on different platforms.

A pre-built Windows exe file can be found in this zip file.

Terminology

Data points: points of the source point set to be transformed.

Model points: points of the target point set.

Notes

• Make sure both model and data points are normalized to fit in [-1,1]³ prior to running (we recommend first independently centralizing the two point clouds to the origin then simultaneously scaling them). The default initial translation cube is [-0.5,0.5]³ (see “config_example.txt”).

• The convergence threshold is set on the Sum of Squared Error (SSE) as in the code and the paper. For the ease of parameter setting for different data point numbers, we use Mean of Squared Error (MSE) in the configuration (see “config_example.txt”). We use MSE threshold of 0.001 for the demos. Try smaller ones if your registration results are not satisfactory.

• Make sure you tune the trimming percentage in the configuration file properly, if there are some outliers in the data pointset (i.e., some regions that are not overlapped by the model pointset). Note that a small portion of outliers may lead to competely wrong result if no trimming is used. Refer to our TPAMI paper for more details.

• Do NOT subsample the model points! Since we use 3D distance transform for closest distance computation, model point number does not affect running speed. Subsampling the model points may increase the optimal registration error thus slowing down the BnB convergance.

• Building 3D distance transform with (default) 300 discrete nodes in each dimension takes about 20-25s in our experiments. Using smaller values can reduce memory and building time costs, but it will also degrade the distance accuracy.

Running

Run the compiled binary with following parameters: <MODEL FILENAME> <DATA FILENAME> <NUM DOWNSAMPLED DATA POINTS> <CONFIGURATION FILENAME> <OUTPUT FILENAME>, e.g. “./GoICP model data 1000 config output”, “GoICP.exe model.txt data.txt 500 config.txt output.txt”.

• <MODEL FILENAME> and <DATA FILENAME> are the point files of the model and data pointsets respectively. Each point file is in plain text format. It begins with a positive point number N in the first line, followed with N lines of X, Y, Z values of the N points.

• <NUM DOWNSAMPLED DATA POINTS> indicates the number of down-sampled data points. The code assumes the input data points are randomly ordered and uses the first <NUM DOWNSAMPLED DATA POINTS> data points for registration. Make sure you randomly permute your data points or change the code for some other sampling strategies.

• <CONFIGURATION FILENAME> is the configuration file containing parameters for the algorithm, e.g. initial rotation and translation cubes, convergence threshold and trimming percentage. See “config_example.txt” for example.

• <OUTPUT FILENAME> is the output file containing registration results. By default it contains the obtained 3x3 rotation matrix and 3x1 translation vector only. You can adapt the code to output other results as you wish.

Some sample data and scripts can be found in the /demo folder.

Other langueage

A python wrapper by @aalavandhaann can be found at https://github.com/aalavandhaann/go-icp_cython

Acknowledgments

This implementation uses the nanoflann library, and a simple matrix library written by Andreas Geiger. The distance transform implementation is adapted from the code of Alexander Vasilevskiy.

Change log

V1.3 (26-Jan-2015)

Implemented the intro-selection algorithm

Fixed some minor issues

V1.2 (12-Jun-2014)

Refined the quick-selection algorithm

Added a deconstructor to distance transform class (Thanks to Nima Tajbakhsh)

V1.1 (21-Apr-2014)

Speeded up Trimmed-GoICP (around 2-5 times experimentally) using a quick-selection algorithm

V1.0 (13-Feb-2014)

First complete version for release

go-icp's Issues

Scaling of original point clouds

I am a little confused about the scaling part:

Make sure both model and data points are normalized to fit in [-1,1]3 prior to running

I have two point clouds lets call it X (3xN) and Y (3xM). There are in general floats. What is the exact scaling I need to do? Also, do I need to scale the result of go-ICP back (inverse scaling?)

X_cap := (X - mean_colwise( X ) ) / max_colwise( X ) - min_colwise( X )

Centering and scaling ruins the results?

Hi,

thank you for the open-source code.
I am using the python wrapper for GO-ICP from @aalavandhaann and I noticed something.

When I use his script with default parameters and no trimming to register the provided model and data bunnies, I get good results.
However, when I center and scale to unit sphere the provided bunnies (as you indicated it should be done in the instructions), and run the same script, I get a completely wrong result.

Do you have any indication why is that so? Is the algorithm very sensitive to the scale and/or location of the point clouds?

I left an issue on his Github repo as well, with the centering an scaling functions I use, if you want to take a look.

Some question about this paper

Hi，I am a student of National Tsinghua University，I have some question about this paper that I'd like to discuss with you.Could you contact me?

How long does this go-icp take? Is it possible to use openmp or cuda to the ndt algorithm?

How long does this go-icp take? Is it possible to use openmp or cuda to the ndt algorithm?

Error Init

I built the code and tried to run an example. I used the config_example.txt file from the source folder and always faced the Errors as shown below. Do you know what is it and how to fix it?

winzip .exe

hello i would like to try this out and downloaded the .exe file it has a .RENAME extension i made it back into a .exe extension but it does not run

thank you

GO-ICP for 2D registration

Is it possible to configure GO-ICP for 2D registration? i.e., two translation parameters and one rotation parameter.

I believe I can effectively set the translation vector to zero by setting transWidth to 0.0 in the config.txt file. Is it possible to set the axes of the translation domain independently?

@yangjiaolong any help would be greatly appreciated.

I can not run the exe in the zip

Hello @yangjiaolong ,
I would like to try to download the .exe file but it has a .RENAME extension and it can not run.
How should I do to run it?
thanks a lot!!!

Segfault on MacOs

Building Distance Transform...0.000445s (CPU)
Model ID: demo/model_bunny.txt (35947), Data ID: demo/data_bunny.txt (30379)
Registering...
zsh: segmentation fault ./GoICP demo/model_bunny.txt demo/data_bunny.txt demo/config.txt

Regretfully, the code does not work out of the box (tried both ARM and Intel architectures). Any advice on how to correct the segfault problem would be much appreciated.

Does not compile on MacOS

Generally a non-issue, rather a comment.

/Go-ICP/jly_3ddt.cpp:13:10: fatal error: 'malloc.h' file not found
#include <malloc.h>
^~~~~~~~~~

The error appears to be standard for MacOS, no matter ARM or Intel. Removing the deprecated (since a while) malloc.h makes compilation possible.

/Go-ICP/matrix.cpp:858:9571: warning: null character ignored [-Wnull-character]

Produces ~9600 warning like this on build, unless matrix.cpp is cleaned up (some encoding problem, maybe?)

Replacing model with point cloud ?

Hi,

Is Go-ICP algorithm able to register 2 point clouds of a scanned 3d object taken from 2 different points of view ?

Thanks.

Bad alloc error

I am always getting bad alloc error while building distance transform?
Any idea why is this happening? The number of points in my pointcloud are around 250 only.

Building Distance Transform...
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
[1]    9507 abort

Problem occurs somewhere in the initiliasation of Array3d object.

void Array3d<T>::Init(int x, int y, int z)
{
  
	Xdim = x;
	Ydim = y;
	Zdim = z;
  // allocate the memory for the first level pointers.
  int n1 = z, n2 = y, n3 = x;


  data = new T** [n1];


  // set the first level pointers and allocate the memory for the second level pointers.

  {

    data[0] = new T* [n1 * n2];

    for (int row1_index = 0; row1_index < n1; row1_index++)
      data [row1_index] = data[0] + n2 * row1_index;
  }

  T* array_ptr = new T [n1*n2*n3];
  data_array = array_ptr;

  // set the second level pointers.
  for (int row1_index = 0; row1_index < n1; row1_index++)
    for (int row2_index = 0; row2_index < n2; row2_index++) {
      data [row1_index][row2_index] = array_ptr;
      array_ptr += n3;
    }
 
}

Help tunning

Hello! I want to use your implementation to register a sequence of point clouds gotten from a kinect. I normalize the points to be [-1,1], the results i'm getting are good, better than other approaches but there's a point where I can't get anything better. I can show you:

1_1_points.txt
2_1_points.txt

I'm trying to register 1 to 2, those are the originals. (I'm ignoring the rgb's in the code)

And the best i've gotten is:

1_1_points_aligned1000.txt

If you compare the transformed 1, compared to the 2, it does a good job but at one points it stops to get better.

I haven't changed the configuration because when I do I get worse results.

Could you please help me giving me a hint on what could I change to help make results better with this data?

Is it possible to extend this algorithm to multi-view cases?

This question might be a little out of scope. I am seeking an approach for multi-view registration. Is it possible to extend your algorithm to a multi-view case?

Thank you for the code

Dear yangjiaolong,

Firstly, thank you for this contribution that is a real useful contribution for the geometry processing community. Secondly, I have forked your code to a different repository and converted the code base such that it works for python. I have wrapped your existing c++ classes using Cython and Autowrap to retain the efficiency and robustness. I thought it is better to inform you prior so that I shall be able to honor any obligations from my end. Kindly let me know if there is any problem with this direction and I shall take down my repository.

Once again thank you for this valuable contribution.

Regards,

#0K