ICP – Iterative closest point, is a very trivial algorithm for matching object templates to noisy data. It’s also super easy to program, so it’s good material for a tutorial. The goal is to take a known set of points (usually defining a curve or object exterior) and register it, as good as possible, to a set of other points, usually a larger and noisy set in which we would like to find the object. The basic algorithm is described very briefly in wikipedia, but there are a ton of papers on the subject.

I’ll take you through the steps of programming it with OpenCV.

So the algorithm is basically

- Find the points which are closest to our curve
- Compute the best rotation and translation between our points and the closest points
- Move our points according to the found transformation, and check the error
- Run another iteration until convergence.

How easy is that?? OpenCV gives us the tools to do everything in there!

## Finding closest points

In v2.x of OpCV they introduced the FLANN framework of range search methods. It’s fairly easy to use, and produces good results, although I did run into some problems with it before. Let’s see how it’s used:

float flann_knn(Mat& m_destinations, Mat& m_object, vector<int>& ptpairs, vector<float>& dists = vector<float>()) { // find nearest neighbors using FLANN cv::Mat m_indices(m_object.rows, 1, CV_32S); cv::Mat m_dists(m_object.rows, 1, CV_32F); Mat dest_32f; m_destinations.convertTo(dest_32f,CV_32FC2); Mat obj_32f; m_object.convertTo(obj_32f,CV_32FC2); assert(dest_32f.type() == CV_32F); cv::flann::Index flann_index(dest_32f, cv::flann::KDTreeIndexParams(2)); // using 2 randomized kdtrees flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, cv::flann::SearchParams(64) ); int* indices_ptr = m_indices.ptr<int>(0); //float* dists_ptr = m_dists.ptr<float>(0); for (int i=0;i<m_indices.rows;++i) { ptpairs.push_back(indices_ptr[i]); } dists.resize(m_dists.rows); m_dists.copyTo(Mat(dists)); return cv::sum(m_dists)[0]; }

This code was practically ripped off OpenCV’s sample code, and worked straight up.. You can see I have 2 input matrices that define the “destination” points – these are the unknown, the “object” points – these are our points, and an output vector to mark the correspondence between the two sets. I also have a vector for distances between points of each pair.

I define a FLANN index object using KD-tree and perform a KNN (K nearest neighbors) search on it, for all the object points. After invoking the search function, it’s all garnish.

## Compute transform

With the point pairs given, computing the transform between the two sets should be easy. And it is. I started by computing it using least-squares to find the parameters of the transformation: rotation angle, translation in X and Y directions. But my solution was not “resistant” to scale, so I looked up another easy solution to the problem, and surely enough a simple solution was found in this paper.

void findBestReansformSVD(Mat& _m, Mat& _d) { Mat m; _m.convertTo(m,CV_32F); Mat d; _d.convertTo(d,CV_32F); Scalar d_bar = mean(d); Scalar m_bar = mean(m); Mat mc = m - m_bar; Mat dc = d - d_bar; mc = mc.reshape(1); dc = dc.reshape(1); Mat H(2,2,CV_32FC1); for(int i=0;i<mc.rows;i++) { Mat mci = mc(Range(i,i+1),Range(0,2)); Mat dci = dc(Range(i,i+1),Range(0,2)); H = H + mci.t() * dci; } cv::SVD svd(H); Mat R = svd.vt.t() * svd.u.t(); double det_R = cv::determinant(R); if(abs(det_R + 1.0) < 0.0001) { float _tmp[4] = {1,0,0,cv::determinant(svd.vt*svd.u)}; R = svd.u * Mat(2,2,CV_32FC1,_tmp) * svd.vt; } #ifdef BTM_DEBUG //for some strange reason the debug version of OpenCV is flipping the matrix R = -R; #endif float* _R = R.ptr<float>(0); Scalar T(d_bar[0] - (m_bar[0]*_R[0] + m_bar[1]*_R[1]),d_bar[1] - (m_bar[0]*_R[2] + m_bar[1]*_R[3])); m = m.reshape(1); m = m * R; m = m.reshape(2); m = m + T;// + m_bar; m.convertTo(_m,CV_32S); }

I really just followed what they said in the paper: take the mean point off the two sets, build a correlation matrix from the distances, do SVD and use U and Vt for computing the rotation and translation. It actually works!

## Putting it together

This is the easy part, just using these two functions and small while loop:

while(true) { pair.clear(); dists.clear(); double dist = flann_knn(destination, X, pair, dists); if(lastDist <= dist) { X = lastGood; break; //converged? } lastDist = dist; X.copyTo(lastGood); cout << "distance: " << dist << endl; Mat X_bar(X.size(),X.type()); for(int i=0;i<X.rows;i++) { Point p = destination.at<Point>(pair[i],0); X_bar.at<Point>(i,0) = p; } ShowQuery(destination,X,X_bar); X = X.reshape(2); X_bar = X_bar.reshape(2); findBestReansformSVD(X,X_bar); X = X.reshape(1); // back to 1-channel }

This will iteratively search for points and move the curve until the curve doesn’t move anymore.

## Some proof

## Code

Available in the SVN repo:

svn checkout https://morethantechnical.googlecode.com/svn/trunk/ICP ICP

Thanks for joining!

Roy.

## 30 replies on “Iterative Closest Point (ICP) for 2D curves with OpenCV [w/ code]”

Hello Roy, i just check your source code but it seems like there are som error. For example, “using namespace cv”, there is not such namespace does exist. I suppose you used other way to define that things. If in that case, can you show me how to do that? Thanks

Hi Duy

The problem could be that you use an old version of OpenCV (before 2.X) which doesn’t have namespacing, or that you don’t include the correct files from the correct directory.

Since the #include directives are probably right, I’ll bet you just have to set up the project to take include files from the OpenCV include directory (e.g. “D:\opencv-2.1\include\”).

Roy.

Hi Roy

I download and set up OpenCV 2.0 in my desktop and run your code again. Here are those errors i got.

Error 15 error C2064: term does not evaluate to a function taking 0 arguments 15

Error 16 error C2064: term does not evaluate to a function taking 0 arguments 15

Error 17 error C2039: ‘at’ : is not a member of ‘CvMat’ 16

Error 18 error C2064: term does not evaluate to a function taking 0 arguments 17

Error 19 error C2064: term does not evaluate to a function taking 0 arguments 17

Error 20 error C2039: ‘at’ : is not a member of ‘CvMat’ 18

Error 23 error C2664: ‘cv::circle’ : cannot convert parameter 1 from ‘CvMat’ to ‘cv::Mat &’ 21

Error 28 error C2664: ‘ShowPoints’ : cannot convert parameter 1 from ‘cv::Mat’ to ‘CvMat &’ 61

Error 29 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 77

Error 30 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 78

Error 32 error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 107

Error 33 error C2660: ‘cv::namedWindow’ : function does not take 1 arguments 120

Error 34 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 131

Error 35 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 132

Error 36 error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 173

Error 37 error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 174

Error 40 error C2589: ‘(‘ : illegal token on right side of ‘::’ 212

Error 41 error C2059: syntax error : ‘::’ 212

Do you have any idea to solve these problem? ( I’m kind of newbie with OpenCV )

Hi Duy

It looks like your are mixing old OpenCV C API (CvMat) with new C++ API (cv::Mat), and the function definitions are not compatible…

See that you are using all cv::Mat and not CvMat.

Re the binary operator errors, please show the lines, else I can’t help much.

Roy.

Hi Roy.

You’re right about the CvMat stuff. Regarding the binary operator errors, the lines was shown at the end of each errors

error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ 77

error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 78

error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 107

error C2660: ‘cv::namedWindow’ : function does not take 1 arguments 120

error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 131

error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 132

error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 173

error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 174

error C2589: ‘(‘ : illegal token on right side of ‘::’ 212

error C2059: syntax error : ‘::’ 212

By the way, i’m using Microsof Visual Studio 2005. I also keep the same order of your source

This is very strange as there

are‘+’ and ‘-‘ binary operator for cv::Mat…Maybe you should use OpenCV v2.1, which is the latest.

This sounds like an includes problem. See that you have “#include “, “#include ” and “using namespace cv;”

And your project is searching in the right directories for the files

Roy.

hi,

thanks you for the code,

what is the dimensions of the matrices you are working with?

how many rows, how many columns?

and what do each row/column represent?

thanks,

Hi

I guess you mean the findTheBestTransform part…

Anyway, Mat m and Mat d are the 2D points matrices of size Nx2 (N rows, 2 columns for the x and y coordinates) and Mx2.

The H matrix is NxM, and is a “correlation matrix”.

The R matrix is a 2×2 rotation matrix, and T is a 1×2 translation vector.

Later, X is again the working (moving) points matrix and “destination” are the blue points – the points you wish the curve to snap to.

Hope it helps

Roy

good job Roy thanks for publishing your code.

Hi Roy.

When i tried to compile your source code. I got many errors as following:

error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion)

It appears in the code line

Mat mc = m – m_bar;// in the compute transform function

It seems like the complier cannot convert from cvScalar to cvMat.

Do you know how to solve this problem ?. I’m using VS 2005 and openCV 2.1

Hi Roy,

Nice work, thanks!

What if I have a set of selected points in the point cloud and I want to find exactly the same points even if the cloud has moved (and not a set of points with similar shape at the original pixel coordinates)?

Say,

– select four points in the cloud above

– move the cloud including the selected points (affine transformation)

– find the same points above

I tried to apply your software to this problem, but I always get another point set which has similar shape at its original pixel location. I assume this is because the nearest neighbour distances are minimal for this set.

Do you know any solution to this problem?

Thanks,

Zo

You might want to use something like “Mat H = zeros(2,2,CV_32FC1);”. Else there’s no guarantee for it being filled with zeros. At least I don’t know of any default initialization for opencv matrices.

Hi I am trying to implement this from first principles at the moment, just about got to grips with SVD, could you explain a little more about how you generated the correlation matrix? I am trying to do this from a set of 2D Cartesian points.

Thanks =]

So incredibly helpful thank you for your time in sharing this.

Maybe some example of usage? ðŸ™‚

How to transform 2D -> 3D IPC ?

Scalar T( d_bar[0] – ( m_bar[0] * _R[0] + m_bar[1] * _R[1] ), d_bar[1] – ( m_bar[0] * _R[2] + m_bar[1] * _R[3] ) );

Only this line will be problem ?

@Pol

Actually, what I like to use for 3D ICP these days is libPCL

It’s an OpenCV sister project, and they rock just as hard… ðŸ™‚

Check out:

http://pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point

All the algorithms are built in… just plug into your code.

R.

I do not understand why m = m * R;

According to the paper, it should be R * m

Thanks.

Hey Roy,

first thank u for this Tutorial it was very helpful for me to get into the ICP stuff. However, using the SVD version, I get worse results than with the standart approach. Is this possible, or may there be a bug somewhere in the function?

Cheers,

Matt

@Matt

I’m referring you to PCL (here)

they have a bunch of ICP algorithms packed in a very easy API

although they focus on 3D, you can easily use it for 2D cases

Check out

http://docs.pointclouds.org/trunk/group__registration.html

http://pointclouds.org/documentation/tutorials/iterative_closest_point.php

Thanks, I will check it out!

Cheers

Could this code be applied to two sets of points with different scales?

@Guang

At this point I think the best thing to do is look for more complete ICP libraries:

https://github.com/ethz-asl/libpointmatcher

http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms

http://www.cvlibs.net/software/libicp.html

u r cool Roy. i hope u will rock the MIT.

In findBestReansformSVD(Mat& _m, Mat& _d), please correct :

Mat H(2,2,CV_32FC1);

to

Mat H(2,2,CV_32FC1, Scalar(0)); : this creates the ‘flipped’ matrix in debug. In both Release and Debug you have undefined behavior if you do not initialize the H values as you can have bogus data to begin with …

hi am working on opencv image processing. Can you say any idea of converting a image file to hex(.dat) format,

Hi Roy,

Thanks for the tuto.

I adapted it a little for my purpose. I have sets of 1000 points to match. The results are repeatable, but :

if I run one set alone, I dont have the same result as if I run all the sets together (one after the other). The first pass is the same, but not the second, when I have new points for the sets.

I tried to replace floats by double, it improved a little the results and decreased the difference between the two type of test conditions, but not entirely and my results are still false.

The error is in flann_knn function:

flann::Index flann_index(dest_64f, flann::KDTreeIndexParams(2)); // using 4 randomized kdtrees

flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, flann::SearchParams(64) );

The m_dists are diffÃ©rents for the same set!

I don’t know what to do now…

Thanks for help!

Hi,

I’m trying to compile your code but unfortunately i get an error.

The following:

icp.cpp:187:113: error: could not convert â€˜std::vector()â€™ from â€˜std::vectorâ€™ to â€˜std::vector&â€™

float flann_knn(Mat& m_destinations, Mat& m_object, vector& ptpairs, vector& dists = vector()) {

Is this a compiler problem or did i forget something?

David

[…] I tried to match contours using the approach described here. Unfortunately, I found that it did not work well with noisy/blurry […]